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"

#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.
 */
int32_t error[3];

uint8_t pFactor[3];
uint8_t dFactor[3];
uint8_t iFactor[3];
uint8_t reverse[3];
int32_t IPart[3] = { 0, 0, 0 };

int16_t controlServos[MAX_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];
}

// this should be followed by a call to switchToFlightMode!!
void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
        debugOut.analog[16] = flightMode;

        reverse[PITCH] = staticParams.controlServosReverse
                        & CONTROL_SERVO_REVERSE_ELEVATOR;
        reverse[ROLL] = staticParams.controlServosReverse
                        & CONTROL_SERVO_REVERSE_AILERONS;
        reverse[YAW] = staticParams.controlServosReverse
                        & CONTROL_SERVO_REVERSE_RUDDER;

        for (uint8_t i = 0; i < 3; i++) {
                if (flightMode == FLIGHT_MODE_MANUAL) {
                        pFactor[i] = 0;
                        dFactor[i] = 0;
                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) {
                        pFactor[i] = staticParams.gyroPID[i].P;
                        dFactor[i] = staticParams.gyroPID[i].D;
                }

                if (flightMode == FLIGHT_MODE_ANGLES) {
                        iFactor[i] = staticParams.gyroPID[i].I;
                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) {
                        iFactor[i] = 0;
                }
        }
}

/************************************************************************/
/*  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;

        // TODO: Check modern version.
        // calculateFlightAttitude();
        // TODO: Check modern version.
        // controlMixer_update();
        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.
        target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6;
        target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6;
        target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6;

        for (axis = PITCH; axis <= YAW; axis++) {
                if (target[axis] > OVER180) {
                        target[axis] -= OVER360;
                } else if (target[axis] <= -OVER180) {
                  target[axis] += OVER360;
                }

                if (reverse[axis])
                        error[axis] = attitude[axis] + target[axis];
                else
                        error[axis] = attitude[axis] - target[axis];
                if (error[axis] > OVER180) {
                        error[axis] -= OVER360;
                } else if (error[axis] <= -OVER180) {
                        error[axis] += OVER360;
                }

                /************************************************************************/
                /* Calculate control feedback from angle (gyro integral)                */
                /* and angular velocity (gyro signal)                                   */
                /************************************************************************/
                PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6)
                                + ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4);
                if (reverse[axis])
                        PDPart[axis] = -PDPart[axis];

                int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
                if (reverse[axis])
                        PDPart[axis] += anglePart;
                else
                        PDPart[axis] -= anglePart;

                // Add I parts here... these are integrated errors.
                // When an error wraps, actually its I part should be negated or something...

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

        debugOut.analog[12] = term[PITCH];
        debugOut.analog[13] = term[ROLL];
        debugOut.analog[14] = term[YAW];
        debugOut.analog[15] = term[THROTTLE];

        for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
                int16_t tmp;
                if (servoTestActive) {
                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
                } 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[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
                debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);

                debugOut.analog[12] = term[PITCH];
                debugOut.analog[13] = term[ROLL];
                debugOut.analog[14] = term[YAW];

                //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
        }
}