Rev 2052 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2052 | Rev 2189 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
2 | #include "output.h" |
2 | #include "output.h" |
3 | #include "eeprom.h" |
3 | #include "debug.h" |
4 | #include "timer0.h" |
4 | #include "timer0.h" |
- | 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 | ||
5 | uint8_t flashCnt[2], flashMask[2]; |
17 | uint8_t flashCnt[2], flashMask[2]; |
Line 6... | Line 18... | ||
6 | 18 | ||
- | 19 | int16_t throttleTerm; |
|
- | 20 | int32_t yawTerm, term[2]; |
|
- | 21 | ||
- | 22 | uint8_t positiveDynamic, negativeDynamic; |
|
- | 23 | ||
Line 7... | Line 24... | ||
7 | DebugOut_t debugOut; |
24 | float previousManualValues[2]; |
8 | 25 | ||
9 | void output_init(void) { |
26 | void output_init(void) { |
10 | // set PC2 & PC3 as output (control of J16 & J17) |
27 | // set PC2 & PC3 as output (control of J16 & J17) |
11 | DDRC |= (1 << DDC2) | (1 << DDC3); |
28 | DDRC |= (1 << DDC2) | (1 << DDC3); |
12 | outputSet(0,0); |
29 | output_setLED(0,0); |
13 | outputSet(1,0); |
30 | output_setLED(1,0); |
- | 31 | flashCnt[0] = flashCnt[1] = 0; |
|
- | 32 | flashMask[0] = flashMask[1] = 128; |
|
- | 33 | ||
- | 34 | for (uint8_t axis=0; axis<2; axis++) |
|
- | 35 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * (1<<LOG_CONTROL_BYTE_SCALING); |
|
- | 36 | } |
|
- | 37 | ||
- | 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; |
|
14 | flashCnt[0] = flashCnt[1] = 0; |
49 | } |
Line 15... | Line 50... | ||
15 | flashMask[0] = flashMask[1] = 128; |
50 | |
16 | } |
51 | } |
17 | 52 | ||
18 | void outputSet(uint8_t num, uint8_t state) { |
53 | void output_setLED(uint8_t num, uint8_t state) { |
19 | if (staticParams.outputFlags & (OUTPUTFLAGS_INVERT_0 << num)) { |
54 | if (staticParams.outputFlags & (OUTPUTFLAGS_INVERT_0 << num)) { |
20 | if (state) OUTPUT_LOW(num) else OUTPUT_HIGH(num); |
55 | if (state) OUTPUT_LOW(num) else OUTPUT_HIGH(num); |
Line 31... | Line 66... | ||
31 | } |
66 | } |
Line 32... | Line 67... | ||
32 | 67 | ||
33 | void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
68 | void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
34 | if (timing > 250 && manual > 230) { |
69 | if (timing > 250 && manual > 230) { |
35 | // "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7. |
70 | // "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7. |
36 | outputSet(port, 1); |
71 | output_setLED(port, 1); |
37 | } else if (timing > 250 && manual < 10) { |
72 | } else if (timing > 250 && manual < 10) { |
38 | // "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7. |
73 | // "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7. |
39 | outputSet(port, 0); |
74 | output_setLED(port, 0); |
40 | } else if (!flashCnt[port]--) { |
75 | } else if (!flashCnt[port]--) { |
41 | // rotating mask over bitmask... |
76 | // rotating mask over bitmask... |
42 | flashCnt[port] = timing - 1; |
77 | flashCnt[port] = timing - 1; |
43 | if (flashMask[port] == 1) |
78 | if (flashMask[port] == 1) |
44 | flashMask[port] = 128; |
79 | flashMask[port] = 128; |
45 | else |
80 | else |
46 | flashMask[port] >>= 1; |
81 | flashMask[port] >>= 1; |
47 | outputSet(port, flashMask[port] & bitmask); |
82 | output_setLED(port, flashMask[port] & bitmask); |
48 | } |
83 | } |
Line 49... | Line 84... | ||
49 | } |
84 | } |
50 | - | ||
51 | void output_update(void) { |
- | |
52 | static int8_t delay = 0; |
- | |
53 | if (!delay--) { // 10 ms intervals |
- | |
54 | delay = 4; |
85 | |
55 | } |
86 | void output_update(void) { |
56 | if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
87 | if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
57 | outputSet(0, 1); |
88 | output_setLED(0, 1); |
58 | outputSet(1, 1); |
89 | output_setLED(1, 1); |
59 | } else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
90 | } else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
60 | outputSet(0, 0); |
91 | output_setLED(0, 0); |
61 | outputSet(1, 0); |
92 | output_setLED(1, 0); |
62 | } else { |
93 | } else { |
63 | if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
94 | if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
64 | flashingLight(0, 25, 0x55, 25); |
95 | flashingLight(0, 25, 0x55, 25); |
65 | } else if (staticParams.outputDebugMask) { |
- | |
66 | outputSet(0, debugOut.digital[0] & staticParams.outputDebugMask); |
96 | } else if (staticParams.outputDebugMask) { |
67 | } else if (!delay) { |
- | |
68 | flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
97 | output_setLED(0, debugOut.digital[0] & staticParams.outputDebugMask); |
69 | } |
98 | } else flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
70 | if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
99 | if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
71 | flashingLight(1, 25, 0x55, 25); |
100 | flashingLight(1, 25, 0x55, 25); |
72 | } else if (staticParams.outputDebugMask) { |
- | |
73 | outputSet(1, debugOut.digital[1] & staticParams.outputDebugMask); |
101 | } else if (staticParams.outputDebugMask) { |
74 | } else if (!delay) { |
- | |
75 | flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
102 | output_setLED(1, debugOut.digital[1] & staticParams.outputDebugMask); |
76 | } |
103 | } else flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
Line 77... | Line 104... | ||
77 | } |
104 | } |
78 | } |
105 | } |
Line 131... | Line 158... | ||
131 | beepModulation = BEEP_MODULATION_EEPROMALARM; |
158 | beepModulation = BEEP_MODULATION_EEPROMALARM; |
132 | if(!beepTime) { |
159 | if(!beepTime) { |
133 | beepTime = 6000; // 0.6 seconds |
160 | beepTime = 6000; // 0.6 seconds |
134 | } |
161 | } |
135 | } |
162 | } |
- | 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 | } |
|
136 | 312 |