From 50ac13d79b331c7f45f9fff65c2c96f88625af83 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 May 2017 07:13:55 +0100 Subject: [PATCH] Minor blackbox code tidy --- src/main/blackbox/blackbox.c | 88 ++++++++------------- src/main/common/maths.c | 9 +++ src/main/common/maths.h | 1 + src/main/flight/failsafe.c | 2 +- src/test/unit/blackbox_encoding_unittest.cc | 13 +-- 5 files changed, 48 insertions(+), 65 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ba21a2acea..ab1da3e480 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -449,6 +449,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; + default: return false; } @@ -456,11 +457,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) static void blackboxBuildConditionCache(void) { - FlightLogFieldCondition cond; - blackboxConditionCache = 0; - - for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { + for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { if (testBlackboxConditionUncached(cond)) { blackboxConditionCache |= 1 << cond; } @@ -765,10 +763,10 @@ static void loadSlowState(blackboxSlowState_t *slow) * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations * since the field was last logged. */ -static void writeSlowFrameIfNeeded(bool allowPeriodicWrite) +static void writeSlowFrameIfNeeded(void) { // Write the slow frame peridocially so it can be recovered if we ever lose sync - bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL; + bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL; if (shouldWrite) { loadSlowState(&slowHistory); @@ -790,15 +788,6 @@ static void writeSlowFrameIfNeeded(bool allowPeriodicWrite) } } -static int gcd(int num, int denom) -{ - if (denom == 0) { - return num; - } - - return gcd(denom, num % denom); -} - void blackboxValidateConfig(void) { if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0 @@ -1011,50 +1000,36 @@ static void writeGPSFrame(timeUs_t currentTimeUs) static void loadMainState(timeUs_t currentTimeUs) { blackboxMainState_t *blackboxCurrent = blackboxHistory[0]; - int i; blackboxCurrent->time = currentTimeUs; - for (i = 0; i < XYZ_AXIS_COUNT; i++) { + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_P[i] = axisPID_P[i]; - } - for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_I[i] = axisPID_I[i]; - } - for (i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); + blackboxCurrent->accSmooth[i] = acc.accSmooth[i]; +#ifdef MAG + blackboxCurrent->magADC[i] = mag.magADC[i]; +#endif } - for (i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) { blackboxCurrent->rcCommand[i] = rcCommand[i]; } - for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); - } - - for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->accSmooth[i] = acc.accSmooth[i]; - } - - for (i = 0; i < 4; i++) { + for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { blackboxCurrent->debug[i] = debug[i]; } const int motorCount = getMotorCount(); - for (i = 0; i < motorCount; i++) { + for (int i = 0; i < motorCount; i++) { blackboxCurrent->motor[i] = motor[i]; } blackboxCurrent->vbatLatest = getBatteryVoltageLatest(); blackboxCurrent->amperageLatest = getAmperageLatest(); -#ifdef MAG - for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->magADC[i] = mag.magADC[i]; - } -#endif - #ifdef BARO blackboxCurrent->BaroAlt = baro.BaroAlt; #endif @@ -1425,6 +1400,22 @@ static bool blackboxShouldLogIFrame(void) return blackboxPFrameIndex == 0; } +/* + * If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the + * GPS home position. + * + * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can + * still be interpreted correctly. + */ +STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void) +{ + if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1] + || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) { + return true; + } + return false; +} + // Called once every FC loop in order to keep track of how many FC loop iterations have passed static void blackboxAdvanceIterationTimers(void) { @@ -1447,7 +1438,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice. */ - writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes()); + if (blackboxIsOnlyLoggingIntraframes()) { + writeSlowFrameIfNeeded(); + } loadMainState(currentTimeUs); writeIntraframe(); @@ -1460,23 +1453,14 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream. * So only log slow frames during loop iterations where we log a main frame. */ - writeSlowFrameIfNeeded(true); + writeSlowFrameIfNeeded(); loadMainState(currentTimeUs); writeInterframe(); } #ifdef GPS if (feature(FEATURE_GPS)) { - /* - * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the - * GPS home position. - * - * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can - * still be interpreted correctly. - */ - if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1] - || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) { - + if (blackboxShouldLogGpsHomeFrame()) { writeGPSHomeFrame(); writeGPSFrame(currentTimeUs); } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] @@ -1497,8 +1481,6 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) */ void blackboxUpdate(timeUs_t currentTimeUs) { - int i; - switch (blackboxState) { case BLACKBOX_STATE_STOPPED: if (ARMING_FLAG(ARMED)) { @@ -1526,7 +1508,7 @@ void blackboxUpdate(timeUs_t currentTimeUs) */ if (millis() > xmitState.u.startTime + 100) { if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) { - for (i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { + for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { blackboxWrite(blackboxHeader[xmitState.headerIndex]); blackboxHeaderBudget--; } diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 97fc286798..acfbf0cc7a 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -99,6 +99,15 @@ float acos_approx(float x) } #endif +int gcd(int num, int denom) +{ + if (denom == 0) { + return num; + } + + return gcd(denom, num % denom); +} + float powerf(float base, int exp) { float result = base; for (int count = 1; count < exp; count++) result *= base; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a7839a4c4e..3cdf33705a 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -76,6 +76,7 @@ typedef union { fp_angles_def angles; } fp_angles_t; +int gcd(int num, int denom); float powerf(float base, int exp); int32_t applyDeadband(int32_t value, int32_t deadband); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 8d27688460..1527ed4fc3 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -88,7 +88,7 @@ void failsafeInit(void) return; } -failsafePhase_e failsafePhase() +failsafePhase_e failsafePhase(void) { return failsafeState.phase; } diff --git a/src/test/unit/blackbox_encoding_unittest.cc b/src/test/unit/blackbox_encoding_unittest.cc index 836b219ee9..652ae161a4 100644 --- a/src/test/unit/blackbox_encoding_unittest.cc +++ b/src/test/unit/blackbox_encoding_unittest.cc @@ -29,15 +29,6 @@ extern "C" { #include "drivers/serial.h" #include "io/serial.h" - - PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); - - PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, - .device = DEFAULT_BLACKBOX_DEVICE, - .rate_num = 1, - .rate_denom = 1, - .on_motor_test = 0 // default off - ); } #include "unittest_macros.h" @@ -115,7 +106,7 @@ void serialTestResetBuffers() serialWritePos = 0; } -TEST(BlackboxTest, Test1) +TEST(BlackboxEncodingTest, TestWriteUnsignedVB) { blackboxPort = &serialTestInstance; blackboxWriteUnsignedVB(0); @@ -127,6 +118,7 @@ TEST(BlackboxTest, Test1) // STUBS extern "C" { +PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); int32_t blackboxHeaderBudget; void mspSerialAllocatePorts(void) {} void blackboxWrite(uint8_t value) {serialWrite(blackboxPort, value);} @@ -140,5 +132,4 @@ int blackboxPrint(const char *s) const int length = pos - (uint8_t*)s; return length; } - }