Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2103 → Rev 2104

/branches/dongfang_FC_fixedwing/flight.c
6,11 → 6,11
 
// 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;}
 
24,13 → 24,12
*/
int32_t error[3];
 
uint8_t pFactor[3];
uint8_t dFactor[3];
uint8_t iFactor[3];
uint8_t reverse[3];
int32_t maxError[3];
int32_t IPart[3] = { 0, 0, 0 };
PID_t airspeedPID[3];
 
int16_t controlServos[MAX_CONTROL_SERVOS];
int16_t controlServos[NUM_CONTROL_SERVOS];
 
/************************************************************************/
/* Neutral Readings */
44,34 → 43,48
target[YAW] = attitude[YAW];
}
 
// this should be followed by a call to switchToFlightMode!!
void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
debugOut.analog[16] = flightMode;
void flight_updateFlightParametersToFlightMode(void) {
debugOut.analog[16] = currentFlightMode;
reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS;
reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER;
 
reverse[PITCH] = staticParams.controlServosReverse
& CONTROL_SERVO_REVERSE_ELEVATOR;
reverse[ROLL] = staticParams.controlServosReverse
& CONTROL_SERVO_REVERSE_AILERONS;
reverse[YAW] = staticParams.controlServosReverse
& 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 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;
}
for (uint8_t axis=0; axis<3; axis++) {
maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
}
}
 
if (flightMode == FLIGHT_MODE_ANGLES) {
iFactor[i] = staticParams.gyroPID[i].I;
} else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) {
iFactor[i] = 0;
}
// Normal at airspeed = 10.
uint8_t calcAirspeedPID(uint8_t pid) {
if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) {
return pid;
}
 
uint16_t result = (pid * 10) / airspeed;
 
if (result > 240 || airspeed == 0) {
result = 240;
}
 
return result;
}
 
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 */
/************************************************************************/
89,10 → 102,8
 
uint8_t axis;
 
// TODO: Check modern version.
// calculateFlightAttitude();
// TODO: Check modern version.
// controlMixer_update();
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.
104,7 → 115,7
if (target[axis] > OVER180) {
target[axis] -= OVER360;
} else if (target[axis] <= -OVER180) {
target[axis] += OVER360;
target[axis] += OVER360;
}
 
if (reverse[axis])
111,27 → 122,42
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;
 
if (error[axis] > maxError[axis]) {
error[axis] = maxError[axis];
} else if (error[axis] < -maxError[axis]) {
error[axis] =- maxError[axis];
}
 
#define LOG_P_SCALE 6
#define LOG_I_SCALE 6
#define LOG_D_SCALE 4
 
/************************************************************************/
/* 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];
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);
if (reverse[axis])
PDPart[axis] = -PDPart[axis];
} else {
PDPart[axis] = 0;
}
 
int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
if (reverse[axis])
PDPart[axis] += anglePart;
else
PDPart[axis] -= anglePart;
 
if (currentFlightMode == FLIGHT_MODE_ANGLES) {
int16_t anglePart = (int32_t)(
error[axis] * (int32_t) airspeedPID[axis].I)
>> LOG_I_SCALE;
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...
 
143,7 → 169,7
debugOut.analog[14] = term[YAW];
debugOut.analog[15] = term[THROTTLE];
 
for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
int16_t tmp;
if (servoTestActive) {
controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;