Rev 2052 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | #include <inttypes.h> |
2 | #include "output.h" |
||
2189 | - | 3 | #include "debug.h" |
1887 | - | 4 | #include "timer0.h" |
2189 | - | 5 | #include "timer2.h" |
6 | #include "twimaster.h" |
||
7 | // For gimbal stab. |
||
8 | #include "attitude.h" |
||
9 | #include "definitions.h" |
||
10 | #include "flight.h" |
||
11 | #include "uart0.h" |
||
12 | #include "beeper.h" |
||
13 | #include "controlMixer.h" |
||
14 | |||
15 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
16 | |||
1775 | - | 17 | uint8_t flashCnt[2], flashMask[2]; |
1964 | - | 18 | |
2189 | - | 19 | int16_t throttleTerm; |
20 | int32_t yawTerm, term[2]; |
||
1964 | - | 21 | |
2189 | - | 22 | uint8_t positiveDynamic, negativeDynamic; |
23 | |||
24 | float previousManualValues[2]; |
||
25 | |||
1612 | dongfang | 26 | void output_init(void) { |
1869 | - | 27 | // set PC2 & PC3 as output (control of J16 & J17) |
28 | DDRC |= (1 << DDC2) | (1 << DDC3); |
||
2189 | - | 29 | output_setLED(0,0); |
30 | output_setLED(1,0); |
||
1869 | - | 31 | flashCnt[0] = flashCnt[1] = 0; |
32 | flashMask[0] = flashMask[1] = 128; |
||
2189 | - | 33 | |
34 | for (uint8_t axis=0; axis<2; axis++) |
||
35 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * (1<<LOG_CONTROL_BYTE_SCALING); |
||
1612 | dongfang | 36 | } |
37 | |||
2189 | - | 38 | void output_setParameters() { |
39 | if (staticParams.dynamicStability > PID_NORMAL_VALUE) { |
||
40 | // Normal gain of 1. |
||
41 | positiveDynamic = 1<<LOG_DYNAMIC_STABILITY_SCALER; |
||
42 | // Gain between 1 (for staticParams.dynamicStability == PID_NORMAL_VALUE) and 0(for staticParams.dynamicStability == 2*PID_NORMAL_VALUE) |
||
43 | negativeDynamic = (1<<(LOG_DYNAMIC_STABILITY_SCALER+1)) - (1<<LOG_DYNAMIC_STABILITY_SCALER) * staticParams.dynamicStability / PID_NORMAL_VALUE; |
||
44 | if (negativeDynamic < 0) |
||
45 | negativeDynamic = 0; |
||
46 | } else { |
||
47 | negativeDynamic = 1<<LOG_DYNAMIC_STABILITY_SCALER; |
||
48 | positiveDynamic = (1<<LOG_DYNAMIC_STABILITY_SCALER) * staticParams.dynamicStability / PID_NORMAL_VALUE; |
||
49 | } |
||
50 | |||
51 | } |
||
52 | |||
53 | void output_setLED(uint8_t num, uint8_t state) { |
||
1986 | - | 54 | if (staticParams.outputFlags & (OUTPUTFLAGS_INVERT_0 << num)) { |
1968 | - | 55 | if (state) OUTPUT_LOW(num) else OUTPUT_HIGH(num); |
56 | } else { |
||
57 | if (state) OUTPUT_HIGH(num) else OUTPUT_LOW(num); |
||
58 | } |
||
1986 | - | 59 | if (staticParams.outputFlags & OUTPUTFLAGS_USE_ONBOARD_LEDS) { |
1968 | - | 60 | if (num) { |
61 | if (state) GRN_ON else GRN_OFF; |
||
62 | } else { |
||
63 | if (state) RED_ON else RED_OFF; |
||
64 | } |
||
65 | } |
||
1964 | - | 66 | } |
67 | |||
68 | void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
||
1869 | - | 69 | if (timing > 250 && manual > 230) { |
1960 | - | 70 | // "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7. |
2189 | - | 71 | output_setLED(port, 1); |
1869 | - | 72 | } else if (timing > 250 && manual < 10) { |
1960 | - | 73 | // "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7. |
2189 | - | 74 | output_setLED(port, 0); |
1869 | - | 75 | } else if (!flashCnt[port]--) { |
76 | // rotating mask over bitmask... |
||
77 | flashCnt[port] = timing - 1; |
||
78 | if (flashMask[port] == 1) |
||
79 | flashMask[port] = 128; |
||
80 | else |
||
81 | flashMask[port] >>= 1; |
||
2189 | - | 82 | output_setLED(port, flashMask[port] & bitmask); |
1869 | - | 83 | } |
1775 | - | 84 | } |
85 | |||
2019 | - | 86 | void output_update(void) { |
87 | if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
||
2189 | - | 88 | output_setLED(0, 1); |
89 | output_setLED(1, 1); |
||
2019 | - | 90 | } else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
2189 | - | 91 | output_setLED(0, 0); |
92 | output_setLED(1, 0); |
||
2019 | - | 93 | } else { |
1986 | - | 94 | if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
1908 | - | 95 | flashingLight(0, 25, 0x55, 25); |
2019 | - | 96 | } else if (staticParams.outputDebugMask) { |
2189 | - | 97 | output_setLED(0, debugOut.digital[0] & staticParams.outputDebugMask); |
98 | } else flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
||
1986 | - | 99 | if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
100 | flashingLight(1, 25, 0x55, 25); |
||
2019 | - | 101 | } else if (staticParams.outputDebugMask) { |
2189 | - | 102 | output_setLED(1, debugOut.digital[1] & staticParams.outputDebugMask); |
103 | } else flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
||
1869 | - | 104 | } |
1612 | dongfang | 105 | } |
106 | |||
1887 | - | 107 | void beep(uint16_t millis) { |
108 | beepTime = millis; |
||
109 | } |
||
110 | |||
111 | /* |
||
112 | * Make [numbeeps] beeps. |
||
113 | */ |
||
114 | void beepNumber(uint8_t numbeeps) { |
||
115 | while(numbeeps--) { |
||
116 | if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren! |
||
117 | beep(100); // 0.1 second |
||
118 | delay_ms(250); // blocks 250 ms as pause to next beep, |
||
119 | // this will block the flight control loop, |
||
120 | // therefore do not use this function if motors are running |
||
121 | } |
||
122 | } |
||
123 | |||
124 | /* |
||
125 | * Beep the R/C alarm signal |
||
126 | */ |
||
127 | void beepRCAlarm(void) { |
||
1908 | - | 128 | if(beepModulation == BEEP_MODULATION_NONE) { // If not already beeping an alarm signal (?) |
1887 | - | 129 | beepTime = 15000; // 1.5 seconds |
1908 | - | 130 | beepModulation = BEEP_MODULATION_RCALARM; |
1887 | - | 131 | } |
132 | } |
||
133 | |||
134 | /* |
||
135 | * Beep the I2C bus error signal |
||
136 | */ |
||
137 | void beepI2CAlarm(void) { |
||
1908 | - | 138 | if((beepModulation == BEEP_MODULATION_NONE) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
1887 | - | 139 | beepTime = 10000; // 1 second |
1908 | - | 140 | beepModulation = BEEP_MODULATION_I2CALARM; |
1887 | - | 141 | } |
142 | } |
||
143 | |||
144 | /* |
||
145 | * Beep the battery low alarm signal |
||
146 | */ |
||
147 | void beepBatteryAlarm(void) { |
||
1908 | - | 148 | beepModulation = BEEP_MODULATION_BATTERYALARM; |
1887 | - | 149 | if(!beepTime) { |
150 | beepTime = 6000; // 0.6 seconds |
||
151 | } |
||
152 | } |
||
153 | |||
154 | /* |
||
155 | * Beep the EEPROM checksum alarm |
||
156 | */ |
||
157 | void beepEEPROMAlarm(void) { |
||
1908 | - | 158 | beepModulation = BEEP_MODULATION_EEPROMALARM; |
1887 | - | 159 | if(!beepTime) { |
160 | beepTime = 6000; // 0.6 seconds |
||
161 | } |
||
162 | } |
||
2189 | - | 163 | |
164 | // Result centered at 0 and scaled to control range steps. |
||
165 | float gimbalStabilizationPart(uint8_t axis) { |
||
166 | float value = attitude[axis]; |
||
167 | //value *= STABILIZATION_FACTOR; |
||
168 | value *= ((float)CONTROL_RANGE / 50.0 / (1<<14)); // 1<<14 scales 90 degrees to full range at normal gain setting (50) |
||
169 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
||
170 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
||
171 | return -value; |
||
172 | return value; |
||
173 | } |
||
174 | |||
175 | // Constant-speed limitation. |
||
176 | float gimbalManualPart(uint8_t axis) { |
||
177 | float manualValue = (dynamicParams.servoManualControl[axis] - 128) * (1<<LOG_CONTROL_BYTE_SCALING); |
||
178 | float diff = manualValue - previousManualValues[axis]; |
||
179 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
||
180 | if (diff > maxSpeed) diff = maxSpeed; |
||
181 | else if (diff < -maxSpeed) diff = -maxSpeed; |
||
182 | manualValue = previousManualValues[axis] + diff; |
||
183 | previousManualValues[axis] = manualValue; |
||
184 | return manualValue; |
||
185 | } |
||
186 | |||
187 | // Result centered at 0 and scaled in control range. |
||
188 | float gimbalServoValue(uint8_t axis) { |
||
189 | float value = gimbalStabilizationPart(axis); |
||
190 | value += gimbalManualPart(axis); |
||
191 | //int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
||
192 | //if (value < limit) value = limit; |
||
193 | //limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
||
194 | //if (value > limit) value = limit; |
||
195 | return value; |
||
196 | } |
||
197 | |||
198 | // Result centered at 0 and scaled in control range. |
||
199 | float getAuxValue(uint8_t auxSource) { |
||
200 | switch(auxSource) { |
||
201 | case (uint8_t)-1: |
||
202 | return 0; |
||
203 | case MIXER_SOURCE_AUX_GIMBAL_ROLL: |
||
204 | return gimbalServoValue(0); |
||
205 | case MIXER_SOURCE_AUX_GIMBAL_PITCH: |
||
206 | return gimbalServoValue(1); |
||
207 | default: // an R/C variable or channel or what we make of it... |
||
208 | return controls[auxSource - MIXER_SOURCE_AUX_RCCHANNEL]; |
||
209 | } |
||
210 | } |
||
211 | |||
212 | // value is generally in the 10 bits range. |
||
213 | // mix is 6 bits. |
||
214 | // and dynamics are 6 bits --> 22 bits needed + sign + space to spare. |
||
215 | static inline int32_t mixin(int8_t mix, int16_t value) { |
||
216 | int32_t x = (int32_t)mix * value; |
||
217 | if (x > 0) { |
||
218 | return x * positiveDynamic; |
||
219 | } else { |
||
220 | return x * negativeDynamic; |
||
221 | } |
||
222 | } |
||
223 | |||
224 | void output_applyMulticopterMixer(void) { |
||
225 | int16_t _outputs[NUM_OUTPUTS]; |
||
226 | |||
227 | for (uint8_t i=0; i<NUM_OUTPUTS; i++) { |
||
228 | _outputs[i] = 0; |
||
229 | } |
||
230 | |||
231 | // Process throttle, roll, pitch, yaw in special way with dynamic stability and with saturation to opposite motor. |
||
232 | for (uint8_t i=0; i<NUM_OUTPUTS; i++) { |
||
233 | if (outputMixer[i].outputType == OUTPUT_TYPE_MOTOR) { |
||
234 | int32_t tmp; |
||
235 | tmp = ((int32_t)throttleTerm<<6) * outputMixer[i].flightControls[MIXER_SOURCE_THROTTLE]; |
||
236 | tmp += mixin(outputMixer[i].flightControls[MIXER_SOURCE_ROLL], term[CONTROL_ROLL]); |
||
237 | tmp += mixin(outputMixer[i].flightControls[MIXER_SOURCE_PITCH], term[CONTROL_PITCH]); |
||
238 | tmp += mixin(outputMixer[i].flightControls[MIXER_SOURCE_YAW], yawTerm); |
||
239 | |||
240 | // Compensate for the factor of 64 multiplied by in matrix mixing and another factor of 64 for the positive/negative dynamic stuff. |
||
241 | _outputs[i] += tmp >> (LOG_MOTOR_MIXER_UNIT + LOG_DYNAMIC_STABILITY_SCALER); |
||
242 | |||
243 | // Deduct saturation from opposite motor output. |
||
244 | int16_t excess = _outputs[i] - (outputMixer[i].maxValue << LOG_CONTROL_BYTE_SCALING); |
||
245 | if (excess > 0) { |
||
246 | uint8_t oppositeIndex = outputMixer[i].oppositeMotor; |
||
247 | if (oppositeIndex != -1) |
||
248 | _outputs[oppositeIndex] -= excess; |
||
249 | } |
||
250 | } |
||
251 | } |
||
252 | |||
253 | // I2C part. |
||
254 | for (uint8_t i=0; i<MAX_I2CCHANNELS; i++) { |
||
255 | // I2C supports only motors anyway.. |
||
256 | if (outputMixer[i].outputType != OUTPUT_TYPE_MOTOR) continue; |
||
257 | |||
258 | if (outputTestActive) { |
||
259 | mkblcs[i].throttle = outputTest[i]; |
||
260 | } else if (MKFlags & MKFLAG_MOTOR_RUN) { |
||
261 | int16_t asByte = _outputs[i] >> LOG_CONTROL_BYTE_SCALING; |
||
262 | // Apply limits. |
||
263 | CHECK_MIN_MAX(asByte, outputMixer[i].minValue, outputMixer[i].maxValue); |
||
264 | if (i<4) |
||
265 | debugOut.analog[16 + i] = asByte; |
||
266 | mkblcs[i].throttle = asByte; |
||
267 | } else { |
||
268 | mkblcs[i].throttle = 0; |
||
269 | } |
||
270 | } |
||
271 | |||
272 | for (uint8_t i=0; i<MAX_PWMCHANNELS; i++) { |
||
273 | uint8_t sourceIndex = MAX_I2CCHANNELS + i; |
||
274 | |||
275 | if (outputMixer[sourceIndex].outputType == OUTPUT_TYPE_MOTOR) { |
||
276 | if (outputTestActive) { |
||
277 | // When testing, min/max does NOT apply. |
||
278 | pwmChannels[i] = (int16_t)(outputTest[sourceIndex] * PWM_BYTE_SCALE_FACTOR + PULSELENGTH_1000 + 0.5); |
||
279 | } else { |
||
280 | int16_t throttle; |
||
281 | if (MKFlags & MKFLAG_MOTOR_RUN) { |
||
282 | throttle = _outputs[sourceIndex]; |
||
283 | int16_t min = outputMixer[sourceIndex].minValue << LOG_CONTROL_BYTE_SCALING; |
||
284 | int16_t max = outputMixer[sourceIndex].maxValue << LOG_CONTROL_BYTE_SCALING; |
||
285 | CHECK_MIN_MAX(throttle, min, max); |
||
286 | throttle = (int16_t)(throttle * PWM_CONTROL_SCALE_FACTOR + PULSELENGTH_1000 + 0.5); |
||
287 | } else { |
||
288 | throttle = PULSELENGTH_1000; |
||
289 | } |
||
290 | pwmChannels[i] = throttle; |
||
291 | } |
||
292 | } else if (outputMixer[sourceIndex].outputType == OUTPUT_TYPE_SERVO) { |
||
293 | int16_t servoValue; |
||
294 | if (outputTestActive) { |
||
295 | servoValue = outputTest[sourceIndex]; |
||
296 | // When testing, min/max DOES apply. |
||
297 | CHECK_MIN_MAX(servoValue, outputMixer[sourceIndex].minValue, outputMixer[sourceIndex].maxValue); |
||
298 | servoValue = ((float)servoValue * PWM_BYTE_SCALE_FACTOR + PULSELENGTH_1000 + 0.5); |
||
299 | } else { |
||
300 | float fServoValue = getAuxValue(outputMixer[sourceIndex].auxSource); |
||
301 | int16_t min = (outputMixer[sourceIndex].minValue-128) << LOG_CONTROL_BYTE_SCALING; |
||
302 | int16_t max = (outputMixer[sourceIndex].maxValue-128) << LOG_CONTROL_BYTE_SCALING; |
||
303 | CHECK_MIN_MAX(fServoValue, min, max); |
||
304 | servoValue = (int16_t)(fServoValue * PWM_CONTROL_SCALE_FACTOR + PULSELENGTH_1500 + 0.5); |
||
305 | } |
||
306 | pwmChannels[i] = servoValue; |
||
307 | } else { // undefined channel. |
||
308 | pwmChannels[i] = PULSELENGTH_1500; |
||
309 | } |
||
310 | } |
||
311 | } |