| 142,6 → 142,8 |
|---|
| // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
| // int16_t dynamicCalCount; |
| |
| uint16_t accVector; |
| |
| /************************************************************************ |
| * Set inclination angles from the acc. sensor data. |
| * If acc. sensors are not used, set to zero. |
| 296,7 → 298,9 |
|---|
| debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| |
| if (1 /*controlActivity <= dynamicParams.maxControlActivityForAcc*/) { |
| if (accVector <= dynamicParams.maxAccVector) { |
| debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| |
| uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
| int32_t accDerived; |
| |
| 313,7 → 317,7 |
|---|
| |
| if (controlActivity > 10000) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| if (controlActivity > 20000) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| 381,11 → 385,23 |
|---|
| } |
| } |
| |
| void calculateAccVector(void) { |
| uint16_t temp; |
| temp = filteredAcc[0]/4; |
| accVector = temp * temp; |
| temp = filteredAcc[1]/4; |
| accVector += temp * temp; |
| temp = filteredAcc[2]/4; |
| accVector += temp * temp; |
| debugOut.analog[19] = accVector; |
| } |
| |
| /************************************************************************ |
| * Main procedure. |
| ************************************************************************/ |
| void calculateFlightAttitude(void) { |
| getAnalogData(); |
| calculateAccVector(); |
| integrate(); |
| |
| #ifdef ATTITUDE_USE_ACC_SENSORS |