Subversion Repositories FlightCtrl

Rev

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
}