diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f60f48a64d8..03bb6cb1bec 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -285,8 +285,15 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"motor", 6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)}, {"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)}, - /* Tricopter tail servo */ - {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}, + /* servos */ + {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #ifdef NAV_BLACKBOX {"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, @@ -788,9 +795,9 @@ static void writeIntraframe(void) blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { - //Assume the tail spends most of its time around the center - blackboxWriteSignedVB(blackboxCurrent->servo[5] - 1500); + for (int x = 0; x < MAX_SUPPORTED_SERVOS; x++) { + //Assume that servos spends most of its time around the center + blackboxWriteSignedVB(blackboxCurrent->servo[x] - servoParams(x)->middle); } #ifdef NAV_BLACKBOX @@ -991,9 +998,7 @@ static void writeInterframe(void) } blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount()); - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { - blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); - } + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS); #ifdef NAV_BLACKBOX blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState); @@ -1402,8 +1407,9 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->rssi = getRSSI(); - //Tail servo for tricopters - blackboxCurrent->servo[5] = servo[5]; + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + blackboxCurrent->servo[i] = servo[i]; + } #ifdef NAV_BLACKBOX blackboxCurrent->navState = navCurrentState;