Subversion Repositories FlightCtrl

#include <stdlib.h>

#include <avr/io.h>

#include "eeprom.h"

#include "flight.h"

#include "output.h"

// Necessary for external control and motor test

#include "uart0.h"

#include "timer2.h"

#include "analog.h"

#include "attitude.h"

#include "controlMixer.h"

#include "configuration.h"

#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}

/*

 * target-directions integrals.

 */

int32_t target[3];

/*

 * Error integrals.

 */

uint8_t reverse[3];

int32_t maxError[3];

int32_t IPart[3] = { 0, 0, 0 };

PID_t airspeedPID[3];

int16_t controlServos[NUM_CONTROL_SERVOS];

/************************************************************************/

/*  Neutral Readings                                                    */

/************************************************************************/

#define CONTROL_CONFIG_SCALE 10

void flight_setGround(void) {

        IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;

        target[PITCH] = attitude[PITCH];

        target[ROLL] = attitude[ROLL];

        target[YAW] = attitude[YAW];

}

void flight_updateFlightParametersToFlightMode(void) {

        debugOut.analog[16] = currentFlightMode;

    reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;

        reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;

        reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;

        // At a switch to angles, we want to kill errors first.

        // This should be triggered only once per mode change!

        if (currentFlightMode == FLIGHT_MODE_ANGLES) {

                target[PITCH] = attitude[PITCH];

                target[ROLL] = attitude[ROLL];

                target[YAW] = attitude[YAW];

        }

        for (uint8_t axis=0; axis<3; axis++) {

                maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;

        }

}

// Normal at airspeed = 10.

uint8_t calcAirspeedPID(uint8_t pid) {

        //if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {

                return pid;

        //}

                        }

void setAirspeedPIDs(void) {

        for (uint8_t axis = 0; axis<3; axis++) {

                airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);

                airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???

                airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;

        }

}

/************************************************************************/

/*  Main Flight Control                                                 */

/************************************************************************/

void flight_control(void) {

        // Mixer Fractions that are combined for Motor Control

        int16_t term[4];

        // PID controller variables

        int16_t PDPart[3];

        static int8_t debugDataTimer = 1;

        // High resolution motor values for smoothing of PID motor outputs

        // static int16_t outputFilters[MAX_OUTPUTS];

        uint8_t axis;

        setAirspeedPIDs();

        term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];

        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.

        int32_t tmp;

        tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;

        if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;

        tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;

        if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;

        tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;

        if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;

        for (axis = PITCH; axis <= YAW; axis++) {

                if (target[axis] > OVER180) {

                        target[axis] -= OVER360;

                } else if (target[axis] <= -OVER180) {

                        target[axis] += OVER360;

                }

                int32_t error = attitude[axis] - target[axis];

#define ROTATETARGET 1

// #define TRUNCATEERROR 1

#ifdef ROTATETARGET

                //if(abs(error) > OVER180) { // doesnt work!!!

        if(error > OVER180 || error < -OVER180) {

                  // The shortest way from attitude to target crosses -180.

                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.

                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.

                  if (error > 0) {

                    if (error < OVER360 - maxError[axis]) {

                      // too much err.

                      error = -maxError[axis];

                      target[axis] = attitude[axis] + maxError[axis];

                      if (target[axis] > OVER180) target[axis] -= OVER360;

                    } else {

                      // Normal case, we just need to correct for the wrap. Error will be negative.

                      error -= OVER360;

                    }

                  } else {

            if (error > maxError[axis] - OVER360) {

              // too much err.

              error = maxError[axis];

              target[axis] = attitude[axis] - maxError[axis];

              if (target[axis] < -OVER180) target[axis] += OVER360;

            } else {

              // Normal case, we just need to correct for the wrap. Error will be negative.

              error += OVER360;

            }

                  }

                } else {

                  // Simple case, linear range.

                if (error > maxError[axis]) {

                  error = maxError[axis];

                  target[axis] = attitude[axis] - maxError[axis];

                } else if (error < -maxError[axis]) {

              error = -maxError[axis];

              target[axis] = attitude[axis] + maxError[axis];

            }

                }

#endif

#ifdef TUNCATEERROR

                if (error > maxError[axis]) {

                  error = maxError[axis];

                } else if (error < -maxError[axis]) {

                  error = -maxError[axis];

                } else {

                        // update I parts here for angles mode. I parts in rate mode is something different.

                }

#endif

        debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

                /************************************************************************/

                /* Calculate control feedback from angle (gyro integral)                */

                /* and angular velocity (gyro signal)                                   */

                /************************************************************************/

                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode

                                == FLIGHT_MODE_RATE) {

                        PDPart[axis] = (((int32_t) gyro_PID[axis]

                                        * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE)

                                        + ((gyroD[axis] * (int16_t) airspeedPID[axis].D)

                                                        >> LOG_D_SCALE);

                } else {

                        PDPart[axis] = 0;

                }

                if (currentFlightMode == FLIGHT_MODE_ANGLES) {

                        int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;

                        PDPart[axis] += anglePart;

                }

                // Add I parts here... these are integrated errors.

                if (reverse[axis])

                  term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];

                else

                  term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];

        }

        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {

                int16_t tmp;

                if (servoTestActive) {

                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;

                } else {

                        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.

                        switch (i) {

                        case 0:

                                tmp = term[ROLL];

                                break;

                        case 1:

                                tmp = term[PITCH];

                                break;

                        case 2:

                                tmp = term[THROTTLE];

                                break;

                        case 3:

                                tmp = term[YAW];

                                break;

                        default:

                                tmp = 0;

                        }

                        // These are all signed and in the same units as the RC stuff in rc.c.

                        controlServos[i] = tmp;

                }

        }

        calculateControlServoValues();

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        // Debugging

        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        if (!(--debugDataTimer)) {

                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.

                debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg

                debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg

                debugOut.analog[2] = gyro_PID[YAW];

                debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

                debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

                debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);

                debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

                debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg

                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);

            debugOut.analog[12] = term[PITCH];

            debugOut.analog[13] = term[ROLL];

            debugOut.analog[14] = term[YAW];

            debugOut.analog[15] = term[THROTTLE];

                //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

                //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg

                //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg

                //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg

        }

}