From 0f85a4b7bcf1988c08404e45e88609de232fe462 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 25 Mar 2019 14:17:56 -0700 Subject: [PATCH] uSD card: Add synchronous logging mode. The synchronous logging mode can log variables synchronously in the stabilizer loop. This helps to analyze flight crashes etc. where the order and consistency between variables is crucial. The older (asynchronous mode) is retained, although the config file format changed slightly. Tested on a CF 2.0 (both synchronous and asynchronous modes using the config.txt that is part of this repository.) --- src/deck/drivers/interface/usddeck.h | 32 ++-- src/deck/drivers/src/usddeck.c | 219 +++++++++++++++++---------- src/modules/src/stabilizer.c | 8 + tools/usdlog/config.txt | 2 + tools/usdlog/example.py | 7 +- 5 files changed, 169 insertions(+), 99 deletions(-) diff --git a/src/deck/drivers/interface/usddeck.h b/src/deck/drivers/interface/usddeck.h index f8fb23be2f..3f1521c805 100644 --- a/src/deck/drivers/interface/usddeck.h +++ b/src/deck/drivers/interface/usddeck.h @@ -4,23 +4,23 @@ #include #include -typedef struct usdLogDataPtr_s { - uint32_t* tick; - uint8_t* data; -} usdLogQueuePtr_t; +enum usddeckLoggingMode_e +{ + usddeckLoggingMode_Disabled = 0, + usddeckLoggingMode_SynchronousStabilizer, + usddeckLoggingMode_Asyncronous, +}; -typedef struct usdLogConfig_s { - char filename[13]; - uint8_t items; - uint16_t frequency; - uint8_t bufferSize; - uint16_t numSlots; - uint16_t numBytes; - int* varIds; // dynamically allocated -} usdLogConfig_t; +// returns true if logging is enabled +bool usddeckLoggingEnabled(void); -#define USD_WRITE(FILE, MESSAGE, BYTES, BYTES_WRITTEN, CRC_VALUE, CRC_FINALXOR, CRC_TABLE) \ - f_write(FILE, MESSAGE, BYTES, BYTES_WRITTEN); \ - CRC_VALUE = crcByByte(MESSAGE, BYTES, CRC_VALUE, CRC_FINALXOR, CRC_TABLE); +// returns the current logging mode +enum usddeckLoggingMode_e usddeckLoggingMode(void); + +// returns the desired logging frequency +int usddeckFrequency(void); + +// For synchronous logging: add a new log entry +void usddeckTriggerLogging(void); #endif //__USDDECK_H__ diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c index 7701cc1d21..bbbc16d4b1 100644 --- a/src/deck/drivers/src/usddeck.c +++ b/src/deck/drivers/src/usddeck.c @@ -61,6 +61,22 @@ // Hardware defines #define USD_CS_PIN DECK_GPIO_IO4 +typedef struct usdLogConfig_s { + char filename[13]; + uint8_t items; + uint16_t frequency; + uint8_t bufferSize; + uint16_t numSlots; + uint16_t numBytes; + int* varIds; // dynamically allocated + bool enableOnStartup; + enum usddeckLoggingMode_e mode; +} usdLogConfig_t; + +#define USD_WRITE(FILE, MESSAGE, BYTES, BYTES_WRITTEN, CRC_VALUE, CRC_FINALXOR, CRC_TABLE) \ + f_write(FILE, MESSAGE, BYTES, BYTES_WRITTEN); \ + CRC_VALUE = crcByByte(MESSAGE, BYTES, CRC_VALUE, CRC_FINALXOR, CRC_TABLE); + // FATFS low lever driver functions. static void initSpi(void); static void setSlowSpiMode(void); @@ -90,6 +106,13 @@ static FATFS FatFs; //File object static FIL logFile; +static QueueHandle_t usdLogQueue; +static uint8_t* usdLogBufferStart; +static uint8_t* usdLogBuffer; +static TaskHandle_t xHandleWriteTask; + +static bool enableLogging; + static xTimerHandle timer; static void usdTimer(xTimerHandle timer); @@ -266,6 +289,14 @@ static void usdInit(DeckInfo *info) usdLogConfig.filename[l+1] = '0'; usdLogConfig.filename[l+2] = 0; + line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); + if (!line) break; + usdLogConfig.enableOnStartup = strtol(line, &endptr, 10); + + line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); + if (!line) break; + usdLogConfig.mode = strtol(line, &endptr, 10); + usdLogConfig.numSlots = 0; usdLogConfig.numBytes = 0; while (line) { @@ -296,6 +327,7 @@ static void usdInit(DeckInfo *info) DEBUG_PRINT("Frequency: %dHz. Buffer size: %d\n", usdLogConfig.frequency, usdLogConfig.bufferSize); DEBUG_PRINT("Filename: %s\n", usdLogConfig.filename); + DEBUG_PRINT("enOnStartup: %d. mode: %d\n", usdLogConfig.enableOnStartup, usdLogConfig.mode); DEBUG_PRINT("slots: %d, %d\n", usdLogConfig.numSlots, usdLogConfig.numBytes); /* create usd-log task */ @@ -330,7 +362,7 @@ static void usdLogTask(void* prm) vTaskDelayUntil(&lastWakeTime, F2T(10)); } - usdLogConfig.varIds = pvPortMalloc(usdLogConfig.numSlots * sizeof(int*)); + usdLogConfig.varIds = pvPortMalloc(usdLogConfig.numSlots * sizeof(int)); DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize()); // store logging variable ids @@ -340,13 +372,14 @@ static void usdLogTask(void* prm) while (f_open(&logFile, "config.txt", FA_READ) == FR_OK) { /* try to read configuration */ char readBuffer[32]; - TCHAR* line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); - if (!line) break; - line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); - if (!line) break; - line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); - if (!line) break; + TCHAR* line; + // skip first 5 lines + for (int i = 0; i < 5; ++i) { + line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); + if (!line) break; + } + while (line) { line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile); if (!line) break; @@ -375,90 +408,108 @@ static void usdLogTask(void* prm) f_close(&logFile); } - /* struct definition for buffering data to write - * requires up to 100 elements for 1kHz logging */ - struct usdLogStruct { - uint32_t tick; - uint8_t data[usdLogConfig.numBytes]; - }; - /* allocate memory for buffer */ DEBUG_PRINT("malloc buffer ...\n"); // vTaskDelay(10); // small delay to allow debug message to be send - struct usdLogStruct* usdLogBufferStart = - pvPortMalloc(usdLogConfig.bufferSize * sizeof(struct usdLogStruct)); - struct usdLogStruct* usdLogBuffer = usdLogBufferStart; + usdLogBufferStart = + pvPortMalloc(usdLogConfig.bufferSize * (4 + usdLogConfig.numBytes)); + usdLogBuffer = usdLogBufferStart; DEBUG_PRINT("[OK].\n"); DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize()); /* create queue to hand over pointer to usdLogData */ - QueueHandle_t usdLogQueue = - xQueueCreate(usdLogConfig.bufferSize, sizeof(usdLogQueuePtr_t)); + usdLogQueue = xQueueCreate(usdLogConfig.bufferSize, sizeof(uint8_t*)); - /* create usd-write task */ - TaskHandle_t xHandleWriteTask; - xTaskCreate(usdWriteTask, USDWRITE_TASK_NAME, - USDWRITE_TASK_STACKSIZE, usdLogQueue, - USDWRITE_TASK_PRI, &xHandleWriteTask); - - usdLogQueuePtr_t usdLogQueuePtr; - uint8_t queueMessagesWaiting = 0; + xHandleWriteTask = 0; + enableLogging = usdLogConfig.enableOnStartup; // enable logging if desired while(1) { vTaskDelayUntil(&lastWakeTime, F2T(usdLogConfig.frequency)); - queueMessagesWaiting = (uint8_t)uxQueueMessagesWaiting(usdLogQueue); - /* trigger writing once there exists at least one queue item, - * frequency will result itself */ - if (queueMessagesWaiting) { - vTaskResume(xHandleWriteTask); - } - /* skip if queue is full, one slot will be spared as mutex */ - if (queueMessagesWaiting == (usdLogConfig.bufferSize - 1)) { - continue; + + // if logging was just (re)-enabled, start write task + if (!xHandleWriteTask && enableLogging) { + xQueueReset(usdLogQueue); + /* create usd-write task */ + xTaskCreate(usdWriteTask, USDWRITE_TASK_NAME, + USDWRITE_TASK_STACKSIZE, usdLogQueue, + USDWRITE_TASK_PRI, &xHandleWriteTask); } - /* write data into buffer */ - usdLogBuffer->tick = lastWakeTime; - int offset = 0; - for (int i = 0; i < usdLogConfig.numSlots; ++i) { - int varid = usdLogConfig.varIds[i]; - switch (logGetType(varid)) { - case LOG_UINT8: - case LOG_INT8: - { - memcpy(&usdLogBuffer->data[offset], logGetAddress(varid), sizeof(uint8_t)); - offset += sizeof(uint8_t); - break; - } - case LOG_UINT16: - case LOG_INT16: - { - memcpy(&usdLogBuffer->data[offset], logGetAddress(varid), sizeof(uint16_t)); - offset += sizeof(uint16_t); - break; - } - case LOG_UINT32: - case LOG_INT32: - case LOG_FLOAT: - { - memcpy(&usdLogBuffer->data[offset], logGetAddress(varid), sizeof(uint32_t)); - offset += sizeof(uint32_t); - break; - } - default: - ASSERT(false); - } + if (enableLogging && usdLogConfig.mode == usddeckLoggingMode_Asyncronous) { + usddeckTriggerLogging(); } + } +} - /* set pointer on latest data and queue */ - usdLogQueuePtr.tick = &usdLogBuffer->tick; - usdLogQueuePtr.data = usdLogBuffer->data; - xQueueSend(usdLogQueue, &usdLogQueuePtr, 0); - /* set pointer to next buffer item */ - if (++usdLogBuffer >= usdLogBufferStart+usdLogConfig.bufferSize) { - usdLogBuffer = usdLogBufferStart; +bool usddeckLoggingEnabled(void) +{ + return enableLogging; +} + +enum usddeckLoggingMode_e usddeckLoggingMode(void) +{ + return usdLogConfig.mode; +} + +int usddeckFrequency(void) +{ + return usdLogConfig.frequency; +} + +void usddeckTriggerLogging(void) +{ + uint8_t queueMessagesWaiting = (uint8_t)uxQueueMessagesWaiting(usdLogQueue); + + /* trigger writing once there exists at least one queue item, + * frequency will result itself */ + if (queueMessagesWaiting) { + vTaskResume(xHandleWriteTask); + } + /* skip if queue is full, one slot will be spared as mutex */ + if (queueMessagesWaiting == (usdLogConfig.bufferSize - 1)) { + return; + } + + /* write data into buffer */ + uint32_t ticks = xTaskGetTickCount(); + memcpy(usdLogBuffer, &ticks, 4); + int offset = 4; + for (int i = 0; i < usdLogConfig.numSlots; ++i) { + int varid = usdLogConfig.varIds[i]; + switch (logGetType(varid)) { + case LOG_UINT8: + case LOG_INT8: + { + memcpy(usdLogBuffer + offset, logGetAddress(varid), sizeof(uint8_t)); + offset += sizeof(uint8_t); + break; + } + case LOG_UINT16: + case LOG_INT16: + { + memcpy(usdLogBuffer + offset, logGetAddress(varid), sizeof(uint16_t)); + offset += sizeof(uint16_t); + break; + } + case LOG_UINT32: + case LOG_INT32: + case LOG_FLOAT: + { + memcpy(usdLogBuffer + offset, logGetAddress(varid), sizeof(uint32_t)); + offset += sizeof(uint32_t); + break; + } + default: + ASSERT(false); } } + /* set pointer on latest data and queue */ + xQueueSend(usdLogQueue, &usdLogBuffer, 0); + /* set pointer to next buffer item */ + usdLogBuffer = usdLogBuffer + 4 + usdLogConfig.numBytes; + if (usdLogBuffer >= usdLogBufferStart + usdLogConfig.bufferSize * (4 + usdLogConfig.numBytes)) { + usdLogBuffer = usdLogBufferStart; + } } static void usdWriteTask(void* usdLogQueue) @@ -561,27 +612,26 @@ static void usdWriteTask(void* usdLogQueue) f_write(&logFile, &crcValue, 4, &bytesWritten); f_close(&logFile); - usdLogQueuePtr_t usdLogQueuePtr; + uint8_t* usdLogQueuePtr; - while (1) { + while (enableLogging) { /* sleep */ vTaskSuspend(NULL); /* determine how many sets can be written */ setsToWrite = (uint8_t)uxQueueMessagesWaiting(usdLogQueue); /* try to open file in append mode */ if (f_open(&logFile, usdLogConfig.filename, FA_OPEN_APPEND | FA_WRITE) - != FR_OK) + != FR_OK) { continue; + } f_write(&logFile, &setsToWrite, 1, &bytesWritten); crcValue = crcByByte(&setsToWrite, 1, INITIAL_REMAINDER, 0, crcTable); do { /* receive data pointer from queue */ xQueueReceive(usdLogQueue, &usdLogQueuePtr, 0); /* write binary data and point on next item */ - USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.tick, 4, - &bytesWritten, crcValue, 0, crcTable) - USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.data, - usdLogConfig.numBytes, &bytesWritten, crcValue, 0, crcTable) + USD_WRITE(&logFile, usdLogQueuePtr, + 4 + usdLogConfig.numBytes, &bytesWritten, crcValue, 0, crcTable) } while(--setsToWrite); /* final xor and negate crc value */ crcValue = ~(crcValue^FINAL_XOR_VALUE); @@ -592,8 +642,9 @@ static void usdWriteTask(void* usdLogQueue) } else { f_mount(NULL, "", 0); } - /* something went wrong */ + /* something went wrong or writing finished */ vTaskDelete(NULL); + xHandleWriteTask = 0; } static bool usdTest() @@ -638,3 +689,7 @@ DECK_DRIVER(usd_deck); PARAM_GROUP_START(deck) PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcUSD, &isInit) PARAM_GROUP_STOP(deck) + +PARAM_GROUP_START(usd) +PARAM_ADD(PARAM_UINT8, logging, &enableLogging) /* use to start/stop logging*/ +PARAM_GROUP_STOP(usd) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 28d42ef260..9df208d532 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -47,6 +47,7 @@ #include "power_distribution.h" #include "estimator.h" +#include "usddeck.h" static bool isInit; static bool emergencyStop = false; @@ -189,6 +190,13 @@ static void stabilizerTask(void* param) } else { powerDistribution(&control); } + + // Log data to uSD card if configured + if ( usddeckLoggingEnabled() + && usddeckLoggingMode() == usddeckLoggingMode_SynchronousStabilizer + && RATE_DO_EXECUTE(usddeckFrequency(), tick)) { + usddeckTriggerLogging(); + } } calcSensorToOutputLatency(&sensorData); tick++; diff --git a/tools/usdlog/config.txt b/tools/usdlog/config.txt index ab67732813..b0c283f979 100644 --- a/tools/usdlog/config.txt +++ b/tools/usdlog/config.txt @@ -1,6 +1,8 @@ 250 # frequency 50 # buffer size log # file name +1 # enable on startup (0/1) +2 # mode (0: disabled, 1: synchronous stabilizer, 2: asynchronous) acc.x acc.y acc.z diff --git a/tools/usdlog/example.py b/tools/usdlog/example.py index f6ff18e9df..3562a0367d 100644 --- a/tools/usdlog/example.py +++ b/tools/usdlog/example.py @@ -6,9 +6,14 @@ import CF_functions as cff import matplotlib.pyplot as plt import re +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument("filename") +args = parser.parse_args() # decode binary log data -logData = cff.decode("test100") +logData = cff.decode(args.filename) # set window background to white plt.rcParams['figure.facecolor'] = 'w'