Page Comparison

...

The altitude is controlled through a standard PID loop. This can be found in the code:

    sp_PitchAngle = controlSignalAltitude(sp_Altitude,(int)gps_Altitude);

    if (sp_PitchAngle > MAX_PITCH_ANGLE)

        sp_PitchAngle = MAX_PITCH_ANGLE;

    if (sp_PitchAngle < -MAX_PITCH_ANGLE)

        sp_PitchAngle = -MAX_PITCH_ANGLE;

...

Like the altitude PID loop, the throttle also has PID loop, which is also based on the altitude:

    control_Throttle = sp_ThrottleRate + controlSignalThrottle(sp_Altitude, (int)gps_Altitude);

    if (control_Throttle > MAX_PWM){

        control_Throttle = MAX_PWM;

    }

    else if (control_Throttle < MIN_PWM){

        control_Throttle = MIN_PWM;

    }

...

The heading control code looks as such:

    while (sp_Heading > 360)

        sp_Heading -=360;

    while (sp_Heading < 0)

        sp_Heading +=360;

        sp_HeadingRate = controlSignalHeading(sp_Heading, gps_PositionFix==2?gps_Heading:(int)imu_YawAngle);

        //Approximating Roll angle from Heading

        sp_RollAngle = sp_HeadingRate;//(int)(atan((float)(sp_HeadingRate)) * PI/180.0);

    if (sp_RollAngle > MAX_ROLL_ANGLE)

        sp_RollAngle = MAX_ROLL_ANGLE;

    if (sp_RollAngle < -MAX_ROLL_ANGLE)

        sp_RollAngle = -MAX_ROLL_ANGLE;

...

The code to maintain the angular roll and pitch of the aircraft look as such (one statement for each):

sp_ComputedRollRate = controlSignalAngles(sp_RollAngle,  imu_RollAngle, ROLL, -(SP_RANGE) / (MAX_ROLL_ANGLE));

Note that depending on the on the input angles, the output is a "rate". It is an angular rate. In other words, if the difference between the setpoint and the output is large, the angular rate will also be large.

...

The result from the angular PID loop gives rise to the angular rate PID loop. This loop is often acknowledged as a stabilizing system, where small quick vibrations are accounted for.

control_Roll = controlSignal((sp_ComputedRollRate / SERVO_SCALE_FACTOR), imu_RollRate, ROLL);

The output of this function completes the PID pipeline. The value of control_Roll, or the equivalent variable for the pitch for that matter, is then directly applied to the PWM module, in order to create a correction to the system (using the flaps, elevators, rudder, or what not).

...

This involves output to the PWM module. After a series of checks, to ensure that all values are within the maximum and minimum parameters for the PWM signal, a PWM call is made to the appropriate channel:

setPWM(1, control_Roll + rollTrim);

As long as the PWM module was initialized, there shouldn't be any problem. The vehicle should move its servos appropriately.

...

For instance, take this scenario:

if ((controlLevel & ROLL_CONTROL_SOURCE) == 0 && (controlLevel & HEADING_CONTROL_ON) == 0)

    sp_RollAngle = (int)((-sp_RollRate / ((float)SP\_RANGE / MAX_ROLL_ANGLE) ));

This snippet of code, converts the controller input into a roll angle. For instance, if the stick is centered, the plane will be at a 0° roll angle. If the pilot steers left, the plane will angle itself left at that same angle. The if statement contains two bit masks. Note that the controlLevel variable has a bitwise AND (&) applied to it.

...