From 909ab19a1b64ebab160d9e22b19a7e577248df99 Mon Sep 17 00:00:00 2001 From: Henry Warhurst Date: Sun, 6 Oct 2024 22:48:17 +0200 Subject: [PATCH] Log servos in blackbox (#13944) * Log servos in blackbox * Add hasServos helper --- src/main/blackbox/blackbox.c | 57 +++++++++++++++++++------- src/main/blackbox/blackbox_fielddefs.h | 5 +++ src/main/cli/settings.c | 3 ++ src/main/flight/mixer.h | 1 + src/main/flight/mixer_init.c | 5 +++ src/test/unit/blackbox_unittest.cc | 1 + 6 files changed, 57 insertions(+), 15 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 01f4e2e5a3..4f214084e6 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -257,8 +257,17 @@ 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)}, +#ifdef USE_SERVOS + /* NOTE (ledvinap, hwarhurst): Decoding would fail if previous encoding is also TAG8_8SVB and does not have exactly 8 values. To fix it, inserting ENCODING_NULL dummy value should force end of previous group. */ + {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, + {"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(TAG8_8SVB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)}, +#endif #ifdef USE_DSHOT_TELEMETRY // eRPM / 100 @@ -476,8 +485,10 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) return (getMotorCount() >= condition - CONDITION(MOTOR_1_HAS_RPM) + 1) && useDshotTelemetry && isFieldEnabled(FIELD_SELECT(RPM)); #endif - case CONDITION(TRICOPTER): - return (mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && isFieldEnabled(FIELD_SELECT(MOTOR)); +#ifdef USE_SERVOS + case CONDITION(SERVOS): + return hasServos() && (FIELD_SELECT(SERVO)); +#endif case CONDITION(PID): return isFieldEnabled(FIELD_SELECT(PID)); @@ -696,13 +707,19 @@ static void writeIntraframe(void) for (int x = 1; x < motorCount; x++) { 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); - } } +#ifdef USE_SERVOS + if (testBlackboxCondition(CONDITION(SERVOS))) { + int32_t out[ARRAYLEN(servo)]; + for (unsigned x = 0; x < ARRAYLEN(servo); ++x) { + out[x] = blackboxCurrent->servo[x] - 1500; + } + + blackboxWriteTag8_8SVB(out, ARRAYLEN(out)); + } +#endif + #ifdef USE_DSHOT_TELEMETRY if (isFieldEnabled(FIELD_SELECT(RPM))) { const int motorCount = getMotorCount(); @@ -852,11 +869,20 @@ static void writeInterframe(void) if (isFieldEnabled(FIELD_SELECT(MOTOR))) { blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount()); - - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { - blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); - } } + +#ifdef USE_SERVOS + if (testBlackboxCondition(CONDITION(SERVOS))) { + STATIC_ASSERT(ARRAYLEN(servo) <= 8, "TAG8_8SVB supports at most 8 values"); + int32_t out[ARRAYLEN(servo)]; + for (unsigned x = 0; x < ARRAYLEN(servo); ++x) { + out[x] = blackboxCurrent->servo[x] - blackboxLast->servo[x]; + } + + blackboxWriteTag8_8SVB(out, ARRAYLEN(out)); + } +#endif + #ifdef USE_DSHOT_TELEMETRY if (isFieldEnabled(FIELD_SELECT(RPM))) { const int motorCount = getMotorCount(); @@ -1195,8 +1221,9 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->rssi = getRssi(); #ifdef USE_SERVOS - //Tail servo for tricopters - blackboxCurrent->servo[5] = servo[5]; + for (unsigned i = 0; i < ARRAYLEN(blackboxCurrent->servo); i++) { + blackboxCurrent->servo[i] = servo[i]; + } #endif #else UNUSED(currentTimeUs); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 5798aaecca..d7590b3f0d 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -43,6 +43,10 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_MOTOR_8_HAS_RPM, #endif +#ifdef USE_SERVOS + FLIGHT_LOG_FIELD_CONDITION_SERVOS, +#endif + FLIGHT_LOG_FIELD_CONDITION_MAG, FLIGHT_LOG_FIELD_CONDITION_BARO, FLIGHT_LOG_FIELD_CONDITION_VBAT, @@ -86,6 +90,7 @@ typedef enum FlightLogFieldSelect_e { // no more than 32 FLIGHT_LOG_FIELD_SELECT_GPS, FLIGHT_LOG_FIELD_SELECT_RPM, FLIGHT_LOG_FIELD_SELECT_GYROUNFILT, + FLIGHT_LOG_FIELD_SELECT_SERVO, FLIGHT_LOG_FIELD_SELECT_COUNT } FlightLogFieldSelect_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 146395aa02..12d181914b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -843,6 +843,9 @@ const clivalue_t valueTable[] = { #endif { "blackbox_disable_debug", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_DEBUG_LOG, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) }, { "blackbox_disable_motors", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_MOTOR, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) }, +#ifdef USE_SERVOS + { "blackbox_disable_servos", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_SERVO, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) }, +#endif #ifdef USE_DSHOT_TELEMETRY { "blackbox_disable_rpm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = FLIGHT_LOG_FIELD_SELECT_RPM, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, fields_disabled_mask) }, #endif diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 9234db75f8..adefa34a5f 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -121,6 +121,7 @@ extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct rxConfig_s; +bool hasServos(void); uint8_t getMotorCount(void); float getMotorMixRange(void); bool areMotorsRunning(void); diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index a223f341a2..1eb5f79abc 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -278,6 +278,11 @@ const mixer_t mixers[] = { FAST_DATA_ZERO_INIT mixerRuntime_t mixerRuntime; +bool hasServos(void) +{ + return mixers[currentMixerMode].useServo; +} + uint8_t getMotorCount(void) { return mixerRuntime.motorCount; diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index 833b71c749..061fcbcb53 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -362,6 +362,7 @@ boxBitmask_t rcModeActivationMask; void mspSerialAllocatePorts(void) {} uint32_t getArmingBeepTimeMicros(void) {return 0;} uint16_t getBatteryVoltageLatest(void) {return 0;} +bool hasServos(void) { return false; } uint8_t getMotorCount(void) {return 4;} bool areMotorsRunning(void) { return false; } bool IS_RC_MODE_ACTIVE(boxId_e) {return false;}