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