Rev 2025 | Rev 2102 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1910 | - | 1 | #include <stdlib.h> |
2 | #include <avr/io.h> |
||
3 | #include "eeprom.h" |
||
4 | #include "flight.h" |
||
5 | #include "output.h" |
||
6 | |||
7 | // Necessary for external control and motor test |
||
8 | #include "uart0.h" |
||
9 | |||
10 | // for scope debugging |
||
11 | // #include "rc.h" |
||
12 | |||
13 | #include "timer2.h" |
||
14 | #include "attitude.h" |
||
15 | #include "controlMixer.h" |
||
16 | |||
17 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
18 | |||
19 | /* |
||
2099 | - | 20 | * target-directions integrals. |
1910 | - | 21 | */ |
2099 | - | 22 | int32_t target[3]; |
1910 | - | 23 | |
2099 | - | 24 | /* |
25 | * Error integrals. |
||
26 | */ |
||
27 | int32_t error[3]; |
||
1910 | - | 28 | |
2099 | - | 29 | uint8_t pFactor[3]; |
30 | uint8_t dFactor[3]; |
||
31 | uint8_t iFactor[3]; |
||
32 | uint8_t reverse[3]; |
||
33 | int32_t IPart[3] = { 0, 0, 0 }; |
||
1910 | - | 34 | |
2099 | - | 35 | int16_t servos[MAX_SERVOS]; |
1910 | - | 36 | |
37 | /************************************************************************/ |
||
38 | /* Neutral Readings */ |
||
39 | /************************************************************************/ |
||
40 | #define CONTROL_CONFIG_SCALE 10 |
||
41 | |||
2099 | - | 42 | void flight_setGround(void) { |
43 | IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
||
44 | target[PITCH] = attitude[PITCH]; |
||
45 | target[ROLL] = attitude[ROLL]; |
||
46 | target[YAW] = attitude[YAW]; |
||
1910 | - | 47 | } |
48 | |||
2099 | - | 49 | void switchToFlightMode(FlightMode_t flightMode) { |
50 | if (flightMode == FLIGHT_MODE_MANUAL) { |
||
51 | pFactor[PITCH] = 0; |
||
52 | pFactor[ROLL] = 0; |
||
53 | pFactor[YAW] = 0; |
||
54 | } else if (flightMode == FLIGHT_MODE_RATE) { |
||
55 | pFactor[PITCH] = 0; //staticParams...; |
||
56 | pFactor[ROLL] = 0; //staticParams...; |
||
57 | pFactor[YAW] = 0; //staticParams...; |
||
58 | } |
||
59 | if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) { |
||
60 | iFactor[PITCH] = 0; |
||
61 | iFactor[ROLL] = 0; |
||
62 | iFactor[YAW] = 0; |
||
63 | } else if (flightMode == FLIGHT_MODE_ANGLES) { |
||
64 | iFactor[PITCH] = 0; //staticParams...; |
||
65 | iFactor[ROLL] = 0; //staticParams...; |
||
66 | iFactor[YAW] = 0; //staticParams...; |
||
67 | // When entering this mode, we want to avoid jerks from accumulated uncorrected errors. |
||
68 | target[PITCH] = attitude[PITCH]; |
||
69 | target[ROLL] = attitude[ROLL]; |
||
70 | target[YAW] = attitude[YAW]; |
||
71 | } |
||
1910 | - | 72 | |
2099 | - | 73 | dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE; |
74 | dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE; |
||
75 | dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE; |
||
1910 | - | 76 | |
2099 | - | 77 | // TODO: Set reverse also. |
1910 | - | 78 | } |
79 | |||
80 | /************************************************************************/ |
||
81 | /* Main Flight Control */ |
||
82 | /************************************************************************/ |
||
83 | void flight_control(void) { |
||
84 | // Mixer Fractions that are combined for Motor Control |
||
2099 | - | 85 | int16_t throttleTerm, term[3]; |
86 | |||
1910 | - | 87 | // PID controller variables |
2099 | - | 88 | int16_t PDPart[3]; |
1910 | - | 89 | |
90 | static int8_t debugDataTimer = 1; |
||
91 | |||
92 | // High resolution motor values for smoothing of PID motor outputs |
||
2099 | - | 93 | // static int16_t outputFilters[MAX_OUTPUTS]; |
1910 | - | 94 | |
2099 | - | 95 | uint8_t axis; |
1910 | - | 96 | |
2099 | - | 97 | // TODO: Check modern version. |
98 | // calculateFlightAttitude(); |
||
99 | // TODO: Check modern version. |
||
100 | // controlMixer_update(); |
||
101 | throttleTerm = controls[CONTROL_THROTTLE]; |
||
1910 | - | 102 | |
2099 | - | 103 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
104 | target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator; |
||
105 | target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons; |
||
106 | target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder; |
||
1910 | - | 107 | |
2099 | - | 108 | for (axis = PITCH; axis <= YAW; axis++) { |
109 | if (target[axis] > OVER180) { |
||
110 | target[axis] -= OVER360; |
||
111 | } else if (attitude[axis] <= -OVER180) { |
||
112 | attitude[axis] += OVER360; |
||
113 | } |
||
1910 | - | 114 | |
2099 | - | 115 | if (reverse[axis]) |
116 | error[axis] = attitude[axis] + target[axis]; |
||
117 | else |
||
118 | error[axis] = attitude[axis] - target[axis]; |
||
119 | if (error[axis] > OVER180) { |
||
120 | error[axis] -= OVER360; |
||
121 | } else if (error[axis] <= -OVER180) { |
||
122 | error[axis] += OVER360; |
||
123 | } |
||
1910 | - | 124 | |
2099 | - | 125 | /************************************************************************/ |
126 | /* Calculate control feedback from angle (gyro integral) */ |
||
127 | /* and angular velocity (gyro signal) */ |
||
128 | /************************************************************************/ |
||
129 | PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6) |
||
130 | + ((differential[axis] * (int16_t) dFactor[axis]) >> 4); |
||
131 | if (reverse[axis]) |
||
132 | PDPart[axis] = -PDPart[axis]; |
||
1910 | - | 133 | |
2099 | - | 134 | int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
135 | if (reverse[axis]) |
||
136 | PDPart[axis] += anglePart; |
||
137 | else |
||
138 | PDPart[axis] -= anglePart; |
||
1910 | - | 139 | |
2099 | - | 140 | // Add I parts here... these are integrated errors. |
141 | // When an error wraps, actually its I part should be negated or something... |
||
1910 | - | 142 | |
2099 | - | 143 | term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
144 | } |
||
1910 | - | 145 | |
2099 | - | 146 | debugOut.analog[12] = term[PITCH]; |
147 | debugOut.analog[13] = term[ROLL]; |
||
148 | debugOut.analog[14] = throttleTerm; |
||
149 | debugOut.analog[15] = term[YAW]; |
||
150 | |||
151 | for (uint8_t i = 0; i < MAX_SERVOS; i++) { |
||
1910 | - | 152 | int16_t tmp; |
2099 | - | 153 | if (servoTestActive) { |
154 | servos[i] = ((int16_t) servoTest[i] - 128) * 4; |
||
155 | } else { |
||
156 | // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
||
157 | switch (i) { |
||
158 | case 0: |
||
159 | tmp = term[ROLL]; |
||
160 | break; |
||
161 | case 1: |
||
162 | tmp = term[PITCH]; |
||
163 | break; |
||
164 | case 2: |
||
165 | tmp = throttleTerm; |
||
166 | break; |
||
167 | case 3: |
||
168 | tmp = term[YAW]; |
||
169 | break; |
||
170 | default: |
||
171 | tmp = 0; |
||
172 | } |
||
173 | // These are all signed and in the same units as the RC stuff in rc.c. |
||
174 | servos[i] = tmp; |
||
1910 | - | 175 | } |
176 | } |
||
177 | |||
178 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
179 | // Debugging |
||
180 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
181 | if (!(--debugDataTimer)) { |
||
182 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
||
2099 | - | 183 | debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
184 | debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
185 | debugOut.analog[2] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
||
1910 | - | 186 | |
2099 | - | 187 | debugOut.analog[3] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
188 | debugOut.analog[4] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
189 | debugOut.analog[5] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
||
1910 | - | 190 | |
2099 | - | 191 | debugOut.analog[6] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
192 | debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
193 | debugOut.analog[8] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
||
194 | |||
195 | //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
196 | //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
197 | //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
||
198 | //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
||
1910 | - | 199 | } |
200 | } |