Elev-8 Commanded Pitch/roll

jakpapy

I'm curious about how the variables for RollDifference/PitchDifference in the main file is computed. I've gone through the code and found that it goes through QuatIMU_GetRoll() and same for GetPitch, and when I follow that function I get to:

int QuatIMU_GetRoll(void) {
return INT_VARS[ Roll ];
}

but I hit a dead end there. I can't seem to find how the value of INT_VARS[Roll] is filled.

I know that RollDifference (and likewise pitch) is the difference between the commanded roll (presumably from the controller) and the current roll with some scaling, but I can't seem to find where this computation is done.

Basically what I'm trying to get to is find some way in which I can command desired roll and pitch directly so I either need to modify where RollDifference is computed or know the scaling so that I can modify it in the main file.

Thank you for any insights.

Publison

Welcome to the forums!

Hopefully Jason will chime in. He wrote the firmware.

Dave Hein

Roll is defined in an enum in quatimu.cpp. It appears that it is only used in 2 places. One place is in QuatIMU_GetRoll. The other instance is in a table called QuatUpdateCommands. This table is passed to the function F32::RunStream along with a pointer to an array containing all the variables. RunStream is defined in f32.cpp. It places a command in a mailbox that talks to a PASM cog that execute 32-bit floating point operations. The PASM cog is defined in f32_driver.spin.

So it appears that the array QuatUpdateCommands contains a description of all the mathematical operations executed by the controller. The PASM cog processes this array, and executes all of the commands.

JasonDorie

First and foremost, Roll and RollDifference are two very different numbers - One is an absolute measure of your current orientation, and the other is how far off that is from where you've been commanded to be.

For what you're trying to do, here's the TL;DR version:

The simplest way for you to command a specific roll or pitch would be to just set the radio inputs yourself.
This code:

void UpdateControls_AutoLevel(void)
{
	//FUNCTION: UpdateControls_AutoLevel

	// Convert radio inputs to float, scale them to get them into the range we want
	float rx = Float(In_Elev) * const_AutoBankScale;
	float rz = Float(In_Aile) * -const_AutoBankScale;
	float ry = Float(In_Rudd) * const_YawRateScale;

...takes those numbers and scales them very directly into the angles that represent the desired orientation. The "const_AutoBankScale" value is a scale value to convert the maximum radio input into what you supply in GroundStation as the maximum roll/pitch angle when in auto-mode.

This line: https://github.com/parallaxinc/Flight-Controller/blob/master/Firmware-C/elev8-main.cpp#L365

    QuatIMU_UpdateControls( &Radio , ControlMode == ControlMode_Manual , FlightMode == FlightMode_AutoManual );   // Now update the control quaternion
    QuatIMU_WaitForCompletion();

...is where the radio input values are passed in, so if you altered the Radio.Elev or Radio.Aile values right before that function is called, you'd effectively control the exact commanded angle. As long as the flight controller is in "stable" mode instead of manual, this will work.


Now, since you asked how those numbers are computed, here's the longer, might hurt your brain a little version:

There are a few places to look, all in the QuatIMU.cpp file: https://github.com/parallaxinc/Flight-Controller/blob/master/Firmware-C/quatimu.cpp

The code in Elev8-main.cpp triggers the execution of the float math instruction streams. These streams are sequences of assembly like instructions. Initially I coded all of these by hand, but in a more recent version I have a compiler that produces them.

QuatUpdateCommands[] is the instruction stream executed by the FPU to compute the current orientation and altitude estimates. It uses the current readings from the gyro, accelerometer, and altimeter. It produces a quaternion that represents the current orientation in world space.

UpdateControls_Manual[] or UpdateControlQuaternion_AutoLevel[] are executed next. These produce another quaternion that is your desired orientation in world space.

Finally, UpdateControls_ComputeOrientationChange[] computes a quaternion that represents the difference between the first two - IE, a rotation that would bring you from your current orientation to your desired one. This is converted to an axis / angle representation (how far to rotate, and around what axis).

What comes out of the last routine is three numbers: PitchDifference, YawDifference, and RollDifference. They're in "radians * 4096" which isn't a particularly useful unit to work with, but it keeps things within range in the PIDs.

This is the newer, C++ version of the code that is now used to generate those instruction streams:
https://github.com/parallaxinc/Flight-Controller/blob/FloatStreamCompiler/FloatStreamCreator/functionstream.cpp

It'll be easier to read, but only somewhat - the use of quaternions means that it works in any orientation, but it also makes it difficult to follow if you've never used them before. It's not as simple as maintaining pitch/yaw/roll internally, because those numbers do weird things like gimbal lock.

jakpapy

Great! Thank you for your reply, it has been very informative! Definitely answered my questions.