Subversion Repositories FlightCtrl

Rev

Rev 1634 | Rev 1775 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1634 Rev 1635
Line 214... Line 214...
214
void RC_update() {
214
void RC_update() {
215
  int16_t tmp1, tmp2;
215
  int16_t tmp1, tmp2;
216
  if(RC_Quality) {
216
  if(RC_Quality) {
217
    RC_Quality--;
217
    RC_Quality--;
218
    if (NewPpmData-- == 0) {
218
    if (NewPpmData-- == 0) {
219
 
-
 
220
      DebugOut.Analog[12] = stickOffsetPitch;
-
 
221
      DebugOut.Analog[13] = stickOffsetRoll;
-
 
222
 
-
 
223
      RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
219
      RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
224
      RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll)    * staticParams.StickP + RCDiff(CH_ROLL)  * staticParams.StickD;
220
      RC_PRTY[CONTROL_ROLL] =  (RCChannel(CH_ROLL) - stickOffsetRoll)   * staticParams.StickP + RCDiff(CH_ROLL)  * staticParams.StickD;
225
      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120;
221
      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120;
226
      if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
222
      if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
227
      tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
223
      tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
228
      // exponential stick sensitivity in yawring rate
224
      // exponential stick sensitivity in yawring rate
229
      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
225
      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
230
      tmp2 += (staticParams.StickYawP * tmp1) / 4;
226
      tmp2 += (staticParams.StickYawP * tmp1) / 4;
Line 288... Line 284...
288
    stickOffsetRoll = RCChannel(CH_ROLL);
284
    stickOffsetRoll = RCChannel(CH_ROLL);
289
  }
285
  }
290
}
286
}
Line 291... Line 287...
291
 
287
 
-
 
288
#define COMMAND_THRESHOLD 85
-
 
289
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
-
 
290
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
292
#define COMMAND_THRESHOLD 85
291
 
293
uint8_t RC_getCommand(void) {
292
uint8_t RC_getCommand(void) {
294
  if(RCChannel(CH_THROTTLE) > COMMAND_THRESHOLD) {
293
  if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
295
    // throttle is up
294
    // vertical is up
296
    if(RCChannel(CH_YAW) > COMMAND_THRESHOLD)
295
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
297
      return COMMAND_GYROCAL;
296
      return COMMAND_GYROCAL;
298
    if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD)
297
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
299
      return COMMAND_ACCCAL;
298
      return COMMAND_ACCCAL;
300
    return COMMAND_NONE;
299
    return COMMAND_NONE;
301
  } else if(RCChannel(CH_THROTTLE) < -COMMAND_THRESHOLD) {
300
  } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
302
    // pitch is down
301
    // vertical is down
303
    if(RCChannel(CH_YAW) > COMMAND_THRESHOLD)
302
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
304
      return COMMAND_STOP;
303
      return COMMAND_STOP;
305
    if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD)
304
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
306
      return COMMAND_START;
305
      return COMMAND_START;
307
    return COMMAND_NONE;
306
    return COMMAND_NONE;
308
  } else {
307
  } else {
309
    // pitch is around center
308
    // vertical is around center
310
    return COMMAND_NONE;
309
    return COMMAND_NONE;
311
  }
310
  }
Line 312... Line 311...
312
}
311
}
Line 322... Line 321...
322
 * + <--
321
 * + <--
323
 *    0
322
 *    0
324
 *
323
 *
325
 * Not in any of these positions: 0
324
 * Not in any of these positions: 0
326
 */
325
 */
-
 
326
 
327
#define ARGUMENT_THRESHOLD 70
327
#define ARGUMENT_THRESHOLD 70
-
 
328
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
-
 
329
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
-
 
330
 
328
uint8_t RC_getArgument(void) {
331
uint8_t RC_getArgument(void) {
329
  if(RCChannel(CH_PITCH) > ARGUMENT_THRESHOLD) {
332
  if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
330
    // pitch is up
333
    // vertical is up
331
    if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
334
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
332
      return 2;
335
      return 2;
333
    if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
336
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
334
      return 4;
337
      return 4;
335
    return 3;
338
    return 3;
336
  } else if(RCChannel(CH_PITCH) < -ARGUMENT_THRESHOLD) {
339
  } else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
337
    // pitch is down
340
    // vertical is down
338
    if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
341
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
339
      return 8;
342
      return 8;
340
    if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
343
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
341
      return 6;
344
      return 6;
342
    return 7;
345
    return 7;
343
  } else {
346
  } else {
344
    // pitch is around center
347
    // vertical is around center
345
    if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
348
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
346
      return 1;
349
      return 1;
347
    if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
350
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
348
      return 5;
351
      return 5;
349
    return 0;
352
    return 0;
350
  }
353
  }
351
}
354
}