Rev 2103 | Rev 2106 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2103 | Rev 2104 | ||
---|---|---|---|
Line 4... | Line 4... | ||
4 | #include "flight.h" |
4 | #include "flight.h" |
5 | #include "output.h" |
5 | #include "output.h" |
Line 6... | Line 6... | ||
6 | 6 | ||
7 | // Necessary for external control and motor test |
7 | // Necessary for external control and motor test |
8 | #include "uart0.h" |
- | |
9 | 8 | #include "uart0.h" |
|
10 | #include "timer2.h" |
9 | #include "timer2.h" |
11 | #include "analog.h" |
10 | #include "analog.h" |
12 | #include "attitude.h" |
11 | #include "attitude.h" |
- | 12 | #include "controlMixer.h" |
|
Line 13... | Line 13... | ||
13 | #include "controlMixer.h" |
13 | #include "configuration.h" |
Line 14... | Line 14... | ||
14 | 14 | ||
15 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
15 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
Line 22... | Line 22... | ||
22 | /* |
22 | /* |
23 | * Error integrals. |
23 | * Error integrals. |
24 | */ |
24 | */ |
25 | int32_t error[3]; |
25 | int32_t error[3]; |
Line 26... | Line -... | ||
26 | - | ||
27 | uint8_t pFactor[3]; |
- | |
28 | uint8_t dFactor[3]; |
- | |
29 | uint8_t iFactor[3]; |
26 | |
- | 27 | uint8_t reverse[3]; |
|
30 | uint8_t reverse[3]; |
28 | int32_t maxError[3]; |
- | 29 | int32_t IPart[3] = { 0, 0, 0 }; |
|
Line 31... | Line 30... | ||
31 | int32_t IPart[3] = { 0, 0, 0 }; |
30 | PID_t airspeedPID[3]; |
Line 32... | Line 31... | ||
32 | 31 | ||
33 | int16_t controlServos[MAX_CONTROL_SERVOS]; |
32 | int16_t controlServos[NUM_CONTROL_SERVOS]; |
34 | 33 | ||
35 | /************************************************************************/ |
34 | /************************************************************************/ |
Line 42... | Line 41... | ||
42 | target[PITCH] = attitude[PITCH]; |
41 | target[PITCH] = attitude[PITCH]; |
43 | target[ROLL] = attitude[ROLL]; |
42 | target[ROLL] = attitude[ROLL]; |
44 | target[YAW] = attitude[YAW]; |
43 | target[YAW] = attitude[YAW]; |
45 | } |
44 | } |
Line 46... | Line -... | ||
46 | - | ||
47 | // this should be followed by a call to switchToFlightMode!! |
45 | |
48 | void flight_updateFlightParametersToFlightMode(uint8_t flightMode) { |
46 | void flight_updateFlightParametersToFlightMode(void) { |
49 | debugOut.analog[16] = flightMode; |
- | |
50 | 47 | debugOut.analog[16] = currentFlightMode; |
|
51 | reverse[PITCH] = staticParams.controlServosReverse |
- | |
52 | & CONTROL_SERVO_REVERSE_ELEVATOR; |
48 | reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
53 | reverse[ROLL] = staticParams.controlServosReverse |
- | |
54 | & CONTROL_SERVO_REVERSE_AILERONS; |
49 | reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
55 | reverse[YAW] = staticParams.controlServosReverse |
- | |
56 | & CONTROL_SERVO_REVERSE_RUDDER; |
50 | reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
57 | 51 | ||
- | 52 | // At a switch to angles, we want to kill errors first. |
|
58 | for (uint8_t i = 0; i < 3; i++) { |
53 | // This should be triggered only once per mode change! |
59 | if (flightMode == FLIGHT_MODE_MANUAL) { |
- | |
60 | pFactor[i] = 0; |
54 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
61 | dFactor[i] = 0; |
- | |
62 | } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) { |
55 | target[PITCH] = attitude[PITCH]; |
63 | pFactor[i] = staticParams.gyroPID[i].P; |
56 | target[ROLL] = attitude[ROLL]; |
64 | dFactor[i] = staticParams.gyroPID[i].D; |
57 | target[YAW] = attitude[YAW]; |
Line 65... | Line 58... | ||
65 | } |
58 | } |
66 | 59 | ||
- | 60 | for (uint8_t axis=0; axis<3; axis++) { |
|
- | 61 | maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR; |
|
- | 62 | } |
|
- | 63 | } |
|
- | 64 | ||
67 | if (flightMode == FLIGHT_MODE_ANGLES) { |
65 | // Normal at airspeed = 10. |
- | 66 | uint8_t calcAirspeedPID(uint8_t pid) { |
|
- | 67 | if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) { |
|
- | 68 | return pid; |
|
- | 69 | } |
|
- | 70 | ||
- | 71 | uint16_t result = (pid * 10) / airspeed; |
|
68 | iFactor[i] = staticParams.gyroPID[i].I; |
72 | |
69 | } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) { |
73 | if (result > 240 || airspeed == 0) { |
- | 74 | result = 240; |
|
- | 75 | } |
|
- | 76 | ||
- | 77 | return result; |
|
- | 78 | } |
|
- | 79 | ||
- | 80 | void setAirspeedPIDs(void) { |
|
- | 81 | for (uint8_t axis = 0; axis<3; axis++) { |
|
- | 82 | airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P); |
|
70 | iFactor[i] = 0; |
83 | airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be??? |
71 | } |
84 | airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
Line 72... | Line 85... | ||
72 | } |
85 | } |
73 | } |
86 | } |
Line 87... | Line 100... | ||
87 | // High resolution motor values for smoothing of PID motor outputs |
100 | // High resolution motor values for smoothing of PID motor outputs |
88 | // static int16_t outputFilters[MAX_OUTPUTS]; |
101 | // static int16_t outputFilters[MAX_OUTPUTS]; |
Line 89... | Line 102... | ||
89 | 102 | ||
Line 90... | Line -... | ||
90 | uint8_t axis; |
- | |
91 | - | ||
92 | // TODO: Check modern version. |
- | |
93 | // calculateFlightAttitude(); |
103 | uint8_t axis; |
- | 104 | ||
94 | // TODO: Check modern version. |
105 | setAirspeedPIDs(); |
Line 95... | Line 106... | ||
95 | // controlMixer_update(); |
106 | |
96 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
107 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
97 | 108 | ||
Line 102... | Line 113... | ||
102 | 113 | ||
103 | for (axis = PITCH; axis <= YAW; axis++) { |
114 | for (axis = PITCH; axis <= YAW; axis++) { |
104 | if (target[axis] > OVER180) { |
115 | if (target[axis] > OVER180) { |
105 | target[axis] -= OVER360; |
116 | target[axis] -= OVER360; |
106 | } else if (target[axis] <= -OVER180) { |
117 | } else if (target[axis] <= -OVER180) { |
107 | target[axis] += OVER360; |
118 | target[axis] += OVER360; |
Line 108... | Line 119... | ||
108 | } |
119 | } |
109 | 120 | ||
110 | if (reverse[axis]) |
121 | if (reverse[axis]) |
111 | error[axis] = attitude[axis] + target[axis]; |
122 | error[axis] = attitude[axis] + target[axis]; |
- | 123 | else |
|
112 | else |
124 | error[axis] = attitude[axis] - target[axis]; |
113 | error[axis] = attitude[axis] - target[axis]; |
125 | |
114 | if (error[axis] > OVER180) { |
126 | if (error[axis] > maxError[axis]) { |
115 | error[axis] -= OVER360; |
127 | error[axis] = maxError[axis]; |
116 | } else if (error[axis] <= -OVER180) { |
128 | } else if (error[axis] < -maxError[axis]) { |
Line -... | Line 129... | ||
- | 129 | error[axis] =- maxError[axis]; |
|
- | 130 | } |
|
- | 131 | ||
- | 132 | #define LOG_P_SCALE 6 |
|
117 | error[axis] += OVER360; |
133 | #define LOG_I_SCALE 6 |
118 | } |
134 | #define LOG_D_SCALE 4 |
119 | 135 | ||
120 | /************************************************************************/ |
136 | /************************************************************************/ |
121 | /* Calculate control feedback from angle (gyro integral) */ |
137 | /* Calculate control feedback from angle (gyro integral) */ |
- | 138 | /* and angular velocity (gyro signal) */ |
|
122 | /* and angular velocity (gyro signal) */ |
139 | /************************************************************************/ |
123 | /************************************************************************/ |
140 | if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode |
124 | PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6) |
141 | == FLIGHT_MODE_RATE) { |
125 | + ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4); |
- | |
126 | if (reverse[axis]) |
142 | PDPart[axis] = (((int32_t) gyro_PID[axis] |
127 | PDPart[axis] = -PDPart[axis]; |
143 | * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
128 | 144 | + ((gyroD[axis] * (int16_t) airspeedPID[axis].D) |
|
129 | int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
145 | >> LOG_D_SCALE); |
130 | if (reverse[axis]) |
146 | if (reverse[axis]) |
- | 147 | PDPart[axis] = -PDPart[axis]; |
|
Line -... | Line 148... | ||
- | 148 | } else { |
|
- | 149 | PDPart[axis] = 0; |
|
- | 150 | } |
|
- | 151 | ||
- | 152 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
|
- | 153 | int16_t anglePart = (int32_t)( |
|
- | 154 | error[axis] * (int32_t) airspeedPID[axis].I) |
|
- | 155 | >> LOG_I_SCALE; |
|
- | 156 | if (reverse[axis]) |
|
131 | PDPart[axis] += anglePart; |
157 | PDPart[axis] += anglePart; |
132 | else |
158 | else |
Line 133... | Line 159... | ||
133 | PDPart[axis] -= anglePart; |
159 | PDPart[axis] -= anglePart; |
134 | 160 | } |
|
Line 141... | Line 167... | ||
141 | debugOut.analog[12] = term[PITCH]; |
167 | debugOut.analog[12] = term[PITCH]; |
142 | debugOut.analog[13] = term[ROLL]; |
168 | debugOut.analog[13] = term[ROLL]; |
143 | debugOut.analog[14] = term[YAW]; |
169 | debugOut.analog[14] = term[YAW]; |
144 | debugOut.analog[15] = term[THROTTLE]; |
170 | debugOut.analog[15] = term[THROTTLE]; |
Line 145... | Line 171... | ||
145 | 171 | ||
146 | for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) { |
172 | for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
147 | int16_t tmp; |
173 | int16_t tmp; |
148 | if (servoTestActive) { |
174 | if (servoTestActive) { |
149 | controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
175 | controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
150 | } else { |
176 | } else { |