From 4a833cdda7c9684b4a7e4d5991033726fa126c6d Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 31 May 2020 13:58:49 +1200 Subject: [PATCH 1/9] Fixed altitude output in MSP. --- src/main/msp/msp.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 25118a361e7..9720c9c067c 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1189,11 +1189,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) break; case MSP_ALTITUDE: -#if defined(USE_BARO) || defined(USE_RANGEFINDER) sbufWriteU32(dst, getEstimatedAltitudeCm()); -#else - sbufWriteU32(dst, 0); -#endif #ifdef USE_VARIO sbufWriteU16(dst, getEstimatedVario()); #else From 6fcde6a6fbe1eb45354b6e96f605ce587b09330b Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 31 May 2020 14:10:24 +1200 Subject: [PATCH 2/9] Updated comments in 'smartport.c' to reflect the specification. --- src/main/telemetry/smartport.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index cfa7a3aab04..7485268b9cb 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -616,7 +616,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear cellCount = getBatteryCellCount(); vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0; } - smartPortSendPackage(id, vfasVoltage); // given in 0.01V, convert to volts + smartPortSendPackage(id, vfasVoltage); // in 0.01V according to SmartPort spec *clearToSend = false; break; #ifdef USE_ESC_SENSOR_TELEMETRY @@ -636,7 +636,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; #endif case FSSP_DATAID_CURRENT : - smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit + smartPortSendPackage(id, getAmperage() / 10); // in 0.1A according to SmartPort spec *clearToSend = false; break; #ifdef USE_ESC_SENSOR_TELEMETRY @@ -698,19 +698,19 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; #endif case FSSP_DATAID_ALTITUDE : - smartPortSendPackage(id, getEstimatedAltitudeCm()); // unknown given unit, requested 100 = 1 meter + smartPortSendPackage(id, getEstimatedAltitudeCm()); // in cm according to SmartPort spec *clearToSend = false; break; case FSSP_DATAID_FUEL : - smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit + smartPortSendPackage(id, getMAhDrawn()); // given in mAh, should be in percent according to SmartPort spec *clearToSend = false; break; case FSSP_DATAID_VARIO : - smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s + smartPortSendPackage(id, getEstimatedVario()); // in cm/s according to SmartPort spec *clearToSend = false; break; case FSSP_DATAID_HEADING : - smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg + smartPortSendPackage(id, attitude.values.yaw * 10); // in degrees * 100 according to SmartPort spec *clearToSend = false; break; #if defined(USE_ACC) @@ -866,14 +866,14 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; case FSSP_DATAID_GPS_ALT : if (STATE(GPS_FIX)) { - smartPortSendPackage(id, gpsSol.llh.altCm); // given in 0.01m + smartPortSendPackage(id, gpsSol.llh.altCm); // in cm according to SmartPort spec *clearToSend = false; } break; #endif case FSSP_DATAID_A4 : cellCount = getBatteryCellCount(); - vfasVoltage = cellCount ? (getBatteryVoltage() / cellCount) : 0; // given in 0.01V, convert to volts + vfasVoltage = cellCount ? (getBatteryVoltage() / cellCount) : 0; // in 0.01V according to SmartPort spec smartPortSendPackage(id, vfasVoltage); *clearToSend = false; break; From f0c4f6a25a2057078eade96d9fd4f4a2b1085ff2 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 1 Jun 2020 11:57:21 +1200 Subject: [PATCH 3/9] Moved custom defaults to dedicated block for STM32F745. --- src/main/target/STM32_UNIFIED/target.mk | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/target/STM32_UNIFIED/target.mk b/src/main/target/STM32_UNIFIED/target.mk index 0ce9a5579d3..d9ea02eb06f 100644 --- a/src/main/target/STM32_UNIFIED/target.mk +++ b/src/main/target/STM32_UNIFIED/target.mk @@ -1,10 +1,6 @@ ifeq ($(TARGET), STM32F405) F405_TARGETS += $(TARGET) -# Use a full block (16 kB) of flash for custom defaults - with 1 MB flash we have more than we know how to use anyway - -CUSTOM_DEFAULTS_EXTENDED = yes - else ifeq ($(TARGET), STM32F411) F411_TARGETS += $(TARGET) @@ -20,6 +16,13 @@ endif endif endif +ifeq ($(TARGET), $(filter $(TARGET), STM32F405 STM32F745)) +# Use a full block (16 kB) of flash for custom defaults - with 1 MB flash we have more than we know how to use anyway + +CUSTOM_DEFAULTS_EXTENDED = yes +endif + + FEATURES += VCP SDCARD_SPI SDCARD_SDIO ONBOARDFLASH TARGET_SRC = \ From a09ddb5f4ef5c6bb5a4837c90c7cb30e886c1015 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 1 Jun 2020 13:01:45 +1200 Subject: [PATCH 4/9] Fixed pin id parsing in CLI. --- src/main/cli/cli.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index a7d24b7c3e3..0c0f7133034 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -5201,24 +5201,27 @@ static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t newTag) } } -static bool strToPin(char *pch, ioTag_t *tag) +static bool strToPin(char *ptr, ioTag_t *tag) { - if (strcasecmp(pch, "NONE") == 0) { + if (strcasecmp(ptr, "NONE") == 0) { *tag = IO_TAG_NONE; + return true; } else { - unsigned pin = 0; - unsigned port = (*pch >= 'a') ? *pch - 'a' : *pch - 'A'; - + const unsigned port = (*ptr >= 'a') ? *ptr - 'a' : *ptr - 'A'; if (port < 8) { - pch++; - pin = atoi(pch); - if (pin < 16) { + ptr++; + + char *end; + const long pin = strtol(ptr, &end, 10); + if (end != ptr && pin >= 0 && pin < 16) { *tag = DEFIO_TAG_MAKE(port, pin); + return true; } } } + return false; } From 4923057eb351964e775f5082dedc19e50489f81c Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 1 Jun 2020 15:19:44 +1200 Subject: [PATCH 5/9] Fixed rangefinder reading. --- .../drivers/rangefinder/rangefinder_hcsr04.c | 94 ++++++++----------- src/main/fc/tasks.c | 17 +++- src/main/sensors/rangefinder.c | 10 +- src/main/sensors/rangefinder.h | 4 +- src/main/target/STM32_UNIFIED/target.h | 1 - src/main/target/common_post.h | 5 + 6 files changed, 62 insertions(+), 69 deletions(-) diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04.c b/src/main/drivers/rangefinder/rangefinder_hcsr04.c index 22d05638045..956cea19955 100644 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04.c +++ b/src/main/drivers/rangefinder/rangefinder_hcsr04.c @@ -25,19 +25,17 @@ #if defined(USE_RANGEFINDER_HCSR04) -#include "build/build_config.h" -#include "build/debug.h" - #include "common/time.h" -#include "drivers/time.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/nvic.h" #include "drivers/rcc.h" +#include "drivers/time.h" #include "drivers/rangefinder/rangefinder.h" -#include "drivers/rangefinder/rangefinder_hcsr04.h" + +#include "rangefinder_hcsr04.h" #define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet #define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet @@ -55,12 +53,10 @@ static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0; static volatile timeMs_t lastMeasurementReceivedAt; -static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; +static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; static timeMs_t lastMeasurementStartedAt = 0; -#ifdef USE_EXTI static extiCallbackRec_t hcsr04_extiCallbackRec; -#endif static IO_t echoIO; static IO_t triggerIO; @@ -68,15 +64,18 @@ static IO_t triggerIO; #if !defined(UNIT_TEST) void hcsr04_extiHandler(extiCallbackRec_t* cb) { - static timeUs_t timing_start; UNUSED(cb); + static timeUs_t timing_start; + + const timeUs_t currentTimeUs = microsISR(); + if (IORead(echoIO) != 0) { - timing_start = micros(); + timing_start = currentTimeUs; } else { - const timeUs_t timing_stop = micros(); + const timeUs_t timing_stop = currentTimeUs; if (timing_stop > timing_start) { - lastMeasurementReceivedAt = millis(); + lastMeasurementReceivedAt = currentTimeUs / 1000; hcsr04SonarPulseTravelTime = timing_stop - timing_start; } } @@ -88,7 +87,7 @@ void hcsr04_init(rangefinderDev_t *dev) UNUSED(dev); } -#define HCSR04_MinimumFiringIntervalMs 60 +#define HCSR04_MINIMUM_FIRING_INTERVAL_MS 60 /* * Start a range reading @@ -98,43 +97,37 @@ void hcsr04_init(rangefinderDev_t *dev) void hcsr04_start_reading(void) { #if !defined(UNIT_TEST) -#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED - IOLo(triggerIO); - delayMicroseconds(11); - IOHi(triggerIO); -#else IOHi(triggerIO); delayMicroseconds(11); IOLo(triggerIO); #endif -#endif } void hcsr04_update(rangefinderDev_t *dev) { UNUSED(dev); + const timeMs_t timeNowMs = millis(); - // the firing interval of the trigger signal should be greater than 60ms - // to avoid interference between consecutive measurements - if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) { - // We should have a valid measurement within 60ms of trigger - if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) { - // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter. - // The ping travels out and back, so to find the distance of the - // object we take half of the distance traveled. - // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59 - - lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59; - if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) { - lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; - } - } - else { - // No measurement within reasonable time - indicate failure - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; + // We should have a valid measurement within 60ms of trigger + if (lastMeasurementReceivedAt > lastMeasurementStartedAt && (lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MINIMUM_FIRING_INTERVAL_MS) { + // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter. + // The ping travels out and back, so to find the distance of the + // object we take half of the distance traveled. + // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59 + + lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59; + if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) { + lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; } + } else if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) > HCSR04_MINIMUM_FIRING_INTERVAL_MS) { + // No measurement within reasonable time - indicate failure + lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; + } + // the firing interval of the trigger signal should be greater than 60ms + // to avoid interference between consecutive measurements + if (timeNowMs > lastMeasurementStartedAt + HCSR04_MINIMUM_FIRING_INTERVAL_MS) { // Trigger a new measurement lastMeasurementStartedAt = timeNowMs; hcsr04_start_reading(); @@ -147,30 +140,22 @@ void hcsr04_update(rangefinderDev_t *dev) int32_t hcsr04_get_distance(rangefinderDev_t *dev) { UNUSED(dev); - return lastCalculatedDistance; + + const int32_t result = lastCalculatedDistance; + + lastCalculatedDistance = RANGEFINDER_NO_NEW_DATA; + + return result; } bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins) { bool detected = false; -#ifdef STM32F10X - // enable AFIO for EXTI support - RCC_ClockCmd(RCC_APB2(AFIO), ENABLE); -#endif - -#if defined(STM32F3) || defined(STM32F4) - RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this? -#endif - triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag); echoIO = IOGetByTag(rangefinderHardwarePins->echoTag); - if (IOGetOwner(triggerIO) != OWNER_FREE) { - return false; - } - - if (IOGetOwner(echoIO) != OWNER_FREE) { + if (IOGetOwner(triggerIO) || IOGetOwner(echoIO)) { return false; } @@ -190,7 +175,7 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa timeMs_t requestTime = millis(); hcsr04_start_reading(); - while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) { + while ((millis() - requestTime) < HCSR04_MINIMUM_FIRING_INTERVAL_MS) { if (IORead(echoIO) == true) { detected = true; break; @@ -201,11 +186,9 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa if (detected) { // Hardware detected - configure the driver -#ifdef USE_EXTI EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_BOTH); // TODO - priority! EXTIEnable(echoIO, true); -#endif dev->delayMs = 100; dev->maxRangeCm = HCSR04_MAX_RANGE_CM; @@ -225,5 +208,4 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa return false; } } - #endif diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 0df81eb6e4f..0941825bf6f 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -189,6 +189,21 @@ static void taskUpdateBaro(timeUs_t currentTimeUs) } #endif +#if defined(USE_RANGEFINDER) +void taskUpdateRangefinder(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (!sensors(SENSOR_RANGEFINDER)) { + return; + } + + rangefinderUpdate(); + + rangefinderProcess(getCosTiltAngle()); +} +#endif + #if defined(USE_BARO) || defined(USE_GPS) static void taskCalculateAltitude(timeUs_t currentTimeUs) { @@ -476,7 +491,7 @@ task_t tasks[TASK_COUNT] = { #endif #ifdef USE_RANGEFINDER - [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, rangefinderUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE), + [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE), #endif }; diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 7059b0201f7..a3ccac9f44e 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -254,16 +254,11 @@ static int16_t computePseudoSnr(int32_t newReading) { /* * This is called periodically by the scheduler */ -// XXX Returns timeDelta_t for iNav for pseudo-RT scheduling. -void rangefinderUpdate(timeUs_t currentTimeUs) +void rangefinderUpdate(void) { - UNUSED(currentTimeUs); - if (rangefinder.dev.update) { rangefinder.dev.update(&rangefinder.dev); } - - // return rangefinder.dev.delayMs * 1000; // to microseconds XXX iNav only } bool isSurfaceAltitudeValid() { @@ -310,8 +305,7 @@ bool rangefinderProcess(float cosTiltAngle) if (distance >= 0) { rangefinder.lastValidResponseTimeMs = millis(); rangefinder.rawAltitude = applyMedianFilter(distance); - } - else if (distance == RANGEFINDER_OUT_OF_RANGE) { + } else if (distance == RANGEFINDER_OUT_OF_RANGE) { rangefinder.lastValidResponseTimeMs = millis(); rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE; } diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 68fd4b464ed..a525edca4f9 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -56,14 +56,12 @@ typedef struct rangefinder_s { int16_t snr; } rangefinder_t; -extern rangefinder_t rangefinder; - void rangefinderResetDynamicThreshold(void); bool rangefinderInit(void); int32_t rangefinderGetLatestAltitude(void); int32_t rangefinderGetLatestRawAltitude(void); -void rangefinderUpdate(timeUs_t currentTimeUs); +void rangefinderUpdate(void); bool rangefinderProcess(float cosTiltAngle); bool rangefinderIsHealthy(void); diff --git a/src/main/target/STM32_UNIFIED/target.h b/src/main/target/STM32_UNIFIED/target.h index d2b6c201416..89a861f3df4 100644 --- a/src/main/target/STM32_UNIFIED/target.h +++ b/src/main/target/STM32_UNIFIED/target.h @@ -210,7 +210,6 @@ #define USE_TRANSPONDER -//TODO: Make this actually work by making the pins configurable #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04 #define USE_RANGEFINDER_TF diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 83107150308..ffa02025828 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -378,6 +378,11 @@ extern uint8_t __config_end; #define USE_CUSTOM_DEFAULTS_ADDRESS #endif +#if !defined(USE_EXTI) +#undef USE_RX_SPI +#undef USE_RANGEFINDER_HCSR04 +#endif + #if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2) #define USE_RX_BIND #endif From b05599fdb8fa3fea2663021ab5276f89c2a78229 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 1 Jun 2020 15:30:18 +1200 Subject: [PATCH 6/9] Added Makefile target to dump compiler versions used to build tests. --- Makefile | 4 ++++ src/test/Makefile | 7 +++++++ 2 files changed, 11 insertions(+) diff --git a/Makefile b/Makefile index cce51d3a21d..015fd8c1674 100644 --- a/Makefile +++ b/Makefile @@ -690,6 +690,10 @@ test junittest test-all test-representative: test_help: $(V0) cd src/test && $(MAKE) help +## test_versions : print the compiler versions used for the test suite +test_versions: + $(V0) cd src/test && $(MAKE) versions + ## test_% : run test 'test_%' from the test suite test_%: $(V0) cd src/test && $(MAKE) $@ diff --git a/src/test/Makefile b/src/test/Makefile index f7fa864eecd..51120398d00 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -437,6 +437,9 @@ COMMON_FLAGS = \ -isystem $(GTEST_DIR)/inc \ -MMD -MP +CC_VERSION = $(shell $(CC) -dumpversion) +CXX_VERSION = $(shell $(CXX) -dumpversion) + ifeq ($(shell $(CC) -v 2>&1 | grep -q "clang version" && echo "clang"),clang) COMMON_FLAGS += -fblocks ifndef CYGWIN @@ -543,6 +546,10 @@ help what usage: Makefile @echo "Any of the Unit Test programs (except for target specific unit tests) can be used as goals to build and run:" @$(foreach test, $(TESTS), echo " test_$(test)";) +versions: + @echo "C compiler: $(CC): $(CC_VERSION)" + @echo "C++ compiler: $(CXX): $(CXX_VERSION)" + ## clean : Cleanup the UnitTest binaries. clean : rm -rf $(OBJECT_DIR) From 9ed83cf67c16bfbce8a28ea7e12955bc529f59d3 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 1 Jun 2020 21:06:27 +1200 Subject: [PATCH 7/9] Moved calculation of available boxes into 'activateConfig()' to avoid a reboot. --- src/main/config/config.c | 4 ++++ src/main/msp/msp.c | 1 - src/main/msp/msp_box.c | 4 +++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6d8e5563d56..95e49dfe602 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -60,6 +60,8 @@ #include "io/serial.h" #include "io/vtx.h" +#include "msp/msp_box.h" + #include "osd/osd.h" #include "pg/adc.h" @@ -181,6 +183,8 @@ static void activateConfig(void) #if defined(USE_LED_STRIP_STATUS_MODE) reevaluateLedConfig(); #endif + + initActiveBoxIds(); } static void adjustFilterLimit(uint16_t *parm, uint16_t resetValue) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 25118a361e7..38cc1ecab57 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -3600,5 +3600,4 @@ void mspFcProcessReply(mspPacket_t *reply) void mspInit(void) { - initActiveBoxIds(); } diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index 12ac9f048e1..f58c64ef34b 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -125,8 +125,10 @@ const box_t *findBoxByPermanentId(uint8_t permanentId) static bool activeBoxIdGet(boxId_e boxId) { - if (boxId > sizeof(activeBoxIds) * 8) + if (boxId > sizeof(activeBoxIds) * 8) { return false; + } + return bitArrayGet(&activeBoxIds, boxId); } From 4b5273c34e3f0c7618cadb8cf6b80f42f41c5678 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 1 Jun 2020 23:08:18 +1200 Subject: [PATCH 8/9] Fixed parsing of over length custom default headers. --- src/main/cli/cli.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index a7d24b7c3e3..a671c144beb 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -228,12 +228,15 @@ static char cliBufferTemp[CLI_IN_BUFFER_SIZE]; #define CUSTOM_DEFAULTS_CHANGESET_ID_PREFIX ", version: " #define CUSTOM_DEFAULTS_DATE_PREFIX ", date: " +#define MAX_CHANGESET_ID_LENGTH 8 +#define MAX_DATE_LENGTH 20 + static bool customDefaultsHeaderParsed = false; static bool customDefaultsFound = false; static char customDefaultsManufacturerId[MAX_MANUFACTURER_ID_LENGTH + 1] = { 0 }; static char customDefaultsBoardName[MAX_BOARD_NAME_LENGTH + 1] = { 0 }; -static char customDefaultsChangesetId[9] = { 0 }; -static char customDefaultsDate[21] = { 0 }; +static char customDefaultsChangesetId[MAX_CHANGESET_ID_LENGTH + 1] = { 0 }; +static char customDefaultsDate[MAX_DATE_LENGTH + 1] = { 0 }; #endif #if defined(USE_CUSTOM_DEFAULTS_ADDRESS) @@ -4252,7 +4255,7 @@ static bool customDefaultsHasNext(const char *customDefaultsPtr) return *customDefaultsPtr && *customDefaultsPtr != 0xFF && customDefaultsPtr < customDefaultsEnd; } -static const char *parseCustomDefaultsHeaderElement(char *dest, const char *customDefaultsPtr, const char *prefix, char terminator) +static const char *parseCustomDefaultsHeaderElement(char *dest, const char *customDefaultsPtr, const char *prefix, const char terminator, const unsigned maxLength) { char *endPtr = NULL; unsigned len = strlen(prefix); @@ -4263,7 +4266,7 @@ static const char *parseCustomDefaultsHeaderElement(char *dest, const char *cust if (endPtr && customDefaultsHasNext(endPtr)) { len = endPtr - customDefaultsPtr; - memcpy(dest, customDefaultsPtr, len); + memcpy(dest, customDefaultsPtr, MIN(len, maxLength)); customDefaultsPtr += len; @@ -4284,13 +4287,13 @@ static void parseCustomDefaultsHeader(void) customDefaultsPtr++; } - customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsManufacturerId, customDefaultsPtr, CUSTOM_DEFAULTS_MANUFACTURER_ID_PREFIX, CUSTOM_DEFAULTS_BOARD_NAME_PREFIX[0]); + customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsManufacturerId, customDefaultsPtr, CUSTOM_DEFAULTS_MANUFACTURER_ID_PREFIX, CUSTOM_DEFAULTS_BOARD_NAME_PREFIX[0], MAX_MANUFACTURER_ID_LENGTH); - customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsBoardName, customDefaultsPtr, CUSTOM_DEFAULTS_BOARD_NAME_PREFIX, CUSTOM_DEFAULTS_CHANGESET_ID_PREFIX[0]); + customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsBoardName, customDefaultsPtr, CUSTOM_DEFAULTS_BOARD_NAME_PREFIX, CUSTOM_DEFAULTS_CHANGESET_ID_PREFIX[0], MAX_BOARD_NAME_LENGTH); - customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsChangesetId, customDefaultsPtr, CUSTOM_DEFAULTS_CHANGESET_ID_PREFIX, CUSTOM_DEFAULTS_DATE_PREFIX[0]); + customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsChangesetId, customDefaultsPtr, CUSTOM_DEFAULTS_CHANGESET_ID_PREFIX, CUSTOM_DEFAULTS_DATE_PREFIX[0], MAX_CHANGESET_ID_LENGTH); - customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsDate, customDefaultsPtr, CUSTOM_DEFAULTS_DATE_PREFIX, '\n'); + customDefaultsPtr = parseCustomDefaultsHeaderElement(customDefaultsDate, customDefaultsPtr, CUSTOM_DEFAULTS_DATE_PREFIX, '\n', MAX_DATE_LENGTH); } customDefaultsHeaderParsed = true; From f65c02c5e07ed7f4417b2da255792528b85ac286 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 28 May 2020 12:43:13 -0400 Subject: [PATCH 9/9] Blackbox flush SD card sector cache after writing header and before logging starts The normal "flush" for SD card only queues a cache sector for writing and the actual sync to the media happens asynchronously. During this period the cache entry is not available until the write completes sometime later. So as the blackbox header fields were written they end up consuming the majority of the cache. A "flush" was made before actual logging starts, but the async writes were not completing fast enough to ensure available cache sectors for the actual logging. This resulted in the cache getting overwritten and corrupting the header. Changed to wait until the sector cache completes writing to the media before starting the actual logging. This ensures that the logging has ample cache sectors. Changes only affect SD card logging. --- src/main/blackbox/blackbox.c | 28 +++++++++++++++++++--------- src/main/blackbox/blackbox_io.c | 28 +++++++++++++++++++++++++--- src/main/blackbox/blackbox_io.h | 2 ++ src/main/io/asyncfatfs/asyncfatfs.c | 24 +++++++++++++++++++----- src/main/io/asyncfatfs/asyncfatfs.h | 1 + 5 files changed, 66 insertions(+), 17 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 33fe7f08c35..3fd7cbf6389 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -42,17 +42,13 @@ #include "common/time.h" #include "common/utils.h" +#include "config/config.h" #include "config/feature.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/motor.h" -#include "pg/rx.h" #include "drivers/compass/compass.h" #include "drivers/sensor.h" #include "drivers/time.h" -#include "config/config.h" #include "fc/board_info.h" #include "fc/controlrate_profile.h" #include "fc/rc.h" @@ -70,6 +66,11 @@ #include "io/gps.h" #include "io/serial.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/motor.h" +#include "pg/rx.h" + #include "rx/rx.h" #include "sensors/acceleration.h" @@ -281,6 +282,7 @@ typedef enum BlackboxState { BLACKBOX_STATE_SEND_GPS_G_HEADER, BLACKBOX_STATE_SEND_SLOW_HEADER, BLACKBOX_STATE_SEND_SYSINFO, + BLACKBOX_STATE_CACHE_FLUSH, BLACKBOX_STATE_PAUSED, BLACKBOX_STATE_RUNNING, BLACKBOX_STATE_SHUTTING_DOWN, @@ -1607,6 +1609,8 @@ STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs) */ void blackboxUpdate(timeUs_t currentTimeUs) { + static BlackboxState cacheFlushNextState; + switch (blackboxState) { case BLACKBOX_STATE_STOPPED: if (ARMING_FLAG(ARMED)) { @@ -1680,7 +1684,8 @@ void blackboxUpdate(timeUs_t currentTimeUs) //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAYLEN(blackboxSlowFields), NULL, NULL)) { - blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); + cacheFlushNextState = BLACKBOX_STATE_SEND_SYSINFO; + blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH); } break; case BLACKBOX_STATE_SEND_SYSINFO: @@ -1694,9 +1699,14 @@ void blackboxUpdate(timeUs_t currentTimeUs) * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations * could wipe out the end of the header if we weren't careful) */ - if (blackboxDeviceFlushForce()) { - blackboxSetState(BLACKBOX_STATE_RUNNING); - } + cacheFlushNextState = BLACKBOX_STATE_RUNNING; + blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH); + } + break; + case BLACKBOX_STATE_CACHE_FLUSH: + // Flush the cache and wait until all possible entries have been written to the media + if (blackboxDeviceFlushForceComplete()) { + blackboxSetState(cacheFlushNextState); } break; case BLACKBOX_STATE_PAUSED: diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index da8886475d8..b3515091bb1 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -242,9 +242,11 @@ bool blackboxDeviceFlushForce(void) #ifdef USE_SDCARD case BLACKBOX_DEVICE_SDCARD: - /* SD card will flush itself without us calling it, but we need to call flush manually in order to check - * if it's done yet or not! - */ + // SD card will flush itself without us calling it, but we need to call flush manually in order to check + // if it's done yet or not! + // However the "flush" only queues one dirty sector each time and the process is asynchronous. So after + // the last dirty sector is queued the flush returns true even though the sector may not actually have + // been physically written to the SD card yet. return afatfs_flush(); #endif // USE_SDCARD @@ -253,6 +255,26 @@ bool blackboxDeviceFlushForce(void) } } +// Flush the blackbox device and only return true if sync is actually complete. +// Primarily to ensure the async operations of SD card sector writes complete thus freeing the cache entries. +bool blackboxDeviceFlushForceComplete(void) +{ + switch (blackboxConfig()->device) { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + if (afatfs_sectorCacheInSync()) { + return true; + } else { + blackboxDeviceFlushForce(); + return false; + } +#endif // USE_SDCARD + + default: + return blackboxDeviceFlushForce(); + } +} + /** * Attempt to open the logging device. Returns true if successful. */ diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 52af9d5d2f4..ba3ec1cc189 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -46,6 +46,8 @@ int blackboxWriteString(const char *s); void blackboxDeviceFlush(void); bool blackboxDeviceFlushForce(void); +bool blackboxDeviceFlushForceComplete(void); + bool blackboxDeviceOpen(void); void blackboxDeviceClose(void); diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 871ad1382ec..3b4b2db9e8f 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -44,21 +44,23 @@ #include #endif -#include "asyncfatfs.h" - -#include "fat_standard.h" -#include "drivers/sdcard.h" #include "common/maths.h" #include "common/time.h" #include "common/utils.h" +#include "drivers/sdcard.h" + +#include "fat_standard.h" + +#include "asyncfatfs.h" + #ifdef AFATFS_DEBUG #define ONLY_EXPOSE_FOR_TESTING #else #define ONLY_EXPOSE_FOR_TESTING static #endif -#define AFATFS_NUM_CACHE_SECTORS 10 +#define AFATFS_NUM_CACHE_SECTORS 11 // FAT filesystems are allowed to differ from these parameters, but we choose not to support those weird filesystems: #define AFATFS_SECTOR_SIZE 512 @@ -736,6 +738,18 @@ static void afatfs_cacheFlushSector(int cacheIndex) } } +// Check whether every sector in the cache that can be flushed has been synchronized +bool afatfs_sectorCacheInSync(void) +{ + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if ((afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_WRITING) || + ((afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_DIRTY) && !afatfs.cacheDescriptor[i].locked)) { + return false; + } + } + return true; +} + /** * Find a sector in the cache which corresponds to the given physical sector index, or NULL if the sector isn't * cached. Note that the cached sector could be in any state including completely empty. diff --git a/src/main/io/asyncfatfs/asyncfatfs.h b/src/main/io/asyncfatfs/asyncfatfs.h index 99750f1e662..67906719e28 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.h +++ b/src/main/io/asyncfatfs/asyncfatfs.h @@ -93,3 +93,4 @@ bool afatfs_isFull(void); afatfsFilesystemState_e afatfs_getFilesystemState(void); afatfsError_e afatfs_getLastError(void); +bool afatfs_sectorCacheInSync(void);