WebSVN - FlightCtrl - Rev 2139 - /branches/dongfang_FC_fixedwing/arduino

#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.
*/

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;

// 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 (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp;

tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp;

tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
if (staticParams.servos[YAW].reverse) 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 (staticParams.servos[axis].reverse)
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
}
}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c - Rev 2139