282,6 → 282,15 |
uint8_t axis; |
int32_t temp; |
|
// for debug LEDs, to be removed with that. |
static uint8_t controlActivityFlash=1; |
static uint8_t accFlash=1; |
#define CF_MAX 10 |
// [1..n[=off [n..10]=on |
// 1 -->1=on, 2=on, ..., 10=on |
// 2 -->1=off,2=on, ..., 10=on |
// 10-->1=off,2=off,..., 10=on |
// 11-->1=off,2=off,..., 10=off |
uint16_t ca = controlActivity >> 6; |
uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
if (!controlActivityWeighted) controlActivityWeighted = 1; |
294,17 → 303,6 |
debugOut.analog[14] = controlActivity; |
debugOut.analog[15] = accVector; |
|
/* |
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
|
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
permilleAcc /= 2; |
debugFullWeight = 0; |
*/ |
|
debugOut.analog[20] = controlActivityWeighted; |
debugOut.analog[21] = accVectorWeighted; |
debugOut.analog[24] = accVector; |
311,6 → 309,21 |
|
accPart /= controlActivityWeighted; |
accPart /= accVectorWeighted; |
|
if (controlActivityFlash < controlActivityWeighted) { |
debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
} else { |
debugOut.digital[0] |= DEBUG_ACC0THORDER; |
} |
if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1; |
|
if (accFlash < accVectorWeighted) { |
debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
} else { |
debugOut.digital[1] |= DEBUG_ACC0THORDER; |
} |
if (++accFlash > CF_MAX+1) accFlash=1; |
|
/* |
* Add to each sum: The amount by which the angle is changed just below. |
*/ |