Subversion Repositories FlightCtrl

Rev

Rev 2065 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2065 Rev 2084
Line 280... Line 280...
280 
  // are less than ....., or reintroduce Kalman.
 280 
  // are less than ....., or reintroduce Kalman.
281 
  // Well actually the Z axis acc. check is not so silly.
 281 
  // Well actually the Z axis acc. check is not so silly.
282 
  uint8_t axis;
 282 
  uint8_t axis;
283 
  int32_t temp;
 283 
  int32_t temp;
Line -... Line 284...
- 
 
 284 
 
- 
 
 285 
  // for debug LEDs, to be removed with that.
- 
 
 286 
  static uint8_t controlActivityFlash=1;
- 
 
 287 
  static uint8_t accFlash=1;
- 
 
 288 
#define CF_MAX 10
- 
 
 289 
  // [1..n[=off    [n..10]=on
- 
 
 290 
  // 1 -->1=on, 2=on, ..., 10=on
- 
 
 291 
  // 2 -->1=off,2=on, ..., 10=on
- 
 
 292 
  // 10-->1=off,2=off,..., 10=on
284 
 
 293 
  // 11-->1=off,2=off,..., 10=off
285 
  uint16_t ca = controlActivity >> 6;
 294 
  uint16_t ca = controlActivity >> 6;
286 
  uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance;
 295 
  uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance;
287 
  if (!controlActivityWeighted) controlActivityWeighted = 1;
 296 
  if (!controlActivityWeighted) controlActivityWeighted = 1;
288 
  uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance;
 297 
  uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance;
Line 292... Line 301...
292 
  int32_t accDerived;
 301 
  int32_t accDerived;
Line 293... Line 302...
293 
 
 302 
 
294 
  debugOut.analog[14] = controlActivity;
 303 
  debugOut.analog[14] = controlActivity;
Line 295... Line -...
295 
  debugOut.analog[15] = accVector;
 - 
 
296 
 
 - 
 
297 
  /*
 - 
 
298 
  if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
 - 
 
299 
  permilleAcc /= 2;
 - 
 
300 
  debugFullWeight = 0;
 - 
 
301 
  }
 - 
 
302 
 
 - 
 
303 
  if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
 - 
 
304 
  permilleAcc /= 2;
 - 
 
305 
  debugFullWeight = 0;
 - 
 
306 
  */
 304 
  debugOut.analog[15] = accVector;
307 
 
 305 
 
308 
  debugOut.analog[20] = controlActivityWeighted;
 306 
  debugOut.analog[20] = controlActivityWeighted;
Line 309... Line 307...
309 
  debugOut.analog[21] = accVectorWeighted;
 307 
  debugOut.analog[21] = accVectorWeighted;
310 
  debugOut.analog[24] = accVector;
 308 
  debugOut.analog[24] = accVector;
- 
 
 309 
 
- 
 
 310 
  accPart /=  controlActivityWeighted;
- 
 
 311 
  accPart /=  accVectorWeighted;
- 
 
 312 
 
- 
 
 313 
  if (controlActivityFlash < controlActivityWeighted) {
- 
 
 314 
          debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
- 
 
 315 
  } else {
- 
 
 316 
          debugOut.digital[0] |= DEBUG_ACC0THORDER;
- 
 
 317 
  }
- 
 
 318 
  if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1;
- 
 
 319 
 
- 
 
 320 
  if (accFlash < accVectorWeighted) {
- 
 
 321 
          debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
- 
 
 322 
  } else {
- 
 
 323 
          debugOut.digital[1] |= DEBUG_ACC0THORDER;
311 
 
 324 
  }
312 
  accPart /=  controlActivityWeighted;
 325 
  if (++accFlash > CF_MAX+1) accFlash=1;
313 
  accPart /=  accVectorWeighted;
 326 
 
314 
  /*
 327 
  /*
315 
   * Add to each sum: The amount by which the angle is changed just below.
 328 
   * Add to each sum: The amount by which the angle is changed just below.