From 47995e9380023d6f2b28fb3e3f8d883b0df481a1 Mon Sep 17 00:00:00 2001 From: Michael Fraser Date: Sat, 22 Jul 2023 11:59:47 +1000 Subject: [PATCH 1/4] Improved cycle timer behaviour Add some improvements to the RTDebugOutput class and use it for the cycle timer, so that the lines output from the two threads don't get mixed up on the serial monitor. --- Arduino/Esp32/Main/CycleTimer.h | 20 +++++++++----------- Arduino/Esp32/Main/Main.ino | 5 ++--- Arduino/Esp32/Main/RTDebugOutput.h | 14 +++++++++----- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/Arduino/Esp32/Main/CycleTimer.h b/Arduino/Esp32/Main/CycleTimer.h index dcf10ee5..12fee68e 100644 --- a/Arduino/Esp32/Main/CycleTimer.h +++ b/Arduino/Esp32/Main/CycleTimer.h @@ -1,38 +1,36 @@ #pragma once #include "freertos/timers.h" - +#include "RTDebugOutput.h" static const int MAX_CYCLES = 1000; class CycleTimer { private: - String _timerName; - unsigned long _timeFirst; + RTDebugOutput _rtOutput; + int64_t _timeFirst; unsigned int _cycleCount; public: CycleTimer(String timerName) - : _timerName(timerName) + : _rtOutput({ timerName }) { ResetTimer(); } void ResetTimer() { - _timeFirst = esp_timer_get_time();//micros(); + _timeFirst = esp_timer_get_time(); _cycleCount = 0; } void Bump() { _cycleCount++; if (_cycleCount > MAX_CYCLES) { - - ; - unsigned long timeEnd = esp_timer_get_time();//micros(); - unsigned long timeElapsed = timeEnd - _timeFirst; + int64_t timeEnd = esp_timer_get_time(); + int64_t timeElapsed = timeEnd - _timeFirst; - double averageCycleTime = ((double)timeElapsed) / ((double)MAX_CYCLES); - Serial.print(_timerName); Serial.print(": "); Serial.println(averageCycleTime); + float averageCycleTime = float(timeElapsed) / MAX_CYCLES; + _rtOutput.offerData({ averageCycleTime }); ResetTimer(); } diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index 76e46737..827369af 100644 --- a/Arduino/Esp32/Main/Main.ino +++ b/Arduino/Esp32/Main/Main.ino @@ -40,9 +40,6 @@ DAP_calculationVariables_st dap_calculationVariables_st; #include "CycleTimer.h" //#define PRINT_CYCLETIME -static CycleTimer timerPU("PU cycle time"); -static CycleTimer timerSC("SC cycle time"); - // target cycle time for pedal update task, to get constant cycle times, required for FIR filtering #define PUT_TARGET_CYCLE_TIME_IN_US 100 @@ -391,6 +388,7 @@ void pedalUpdateTask( void * pvParameters ) // print the execution time averaged over multiple cycles #ifdef PRINT_CYCLETIME + static CycleTimer timerPU("PU cycle time"); timerPU.Bump(); #endif @@ -613,6 +611,7 @@ void serialCommunicationTask( void * pvParameters ) // average cycle time averaged over multiple cycles #ifdef PRINT_CYCLETIME + static CycleTimer timerSC("SC cycle time"); timerSC.Bump(); #endif diff --git a/Arduino/Esp32/Main/RTDebugOutput.h b/Arduino/Esp32/Main/RTDebugOutput.h index 4425e6af..aa3e8f1e 100644 --- a/Arduino/Esp32/Main/RTDebugOutput.h +++ b/Arduino/Esp32/Main/RTDebugOutput.h @@ -67,13 +67,17 @@ class RTDebugOutput { } void printData() { - if(xSemaphoreTake(_semaphore_data, 0) == pdTRUE) { + if (xSemaphoreTake(_semaphore_data, 0) == pdTRUE) { if (_dataReady) { - for (int i=0; i Date: Tue, 1 Aug 2023 11:12:03 +1000 Subject: [PATCH 2/4] Use a queue for passing joystick value from pedal task to serial task This significantly improves task scheduling behaviour in the case of single core processors - eg ESP32 S2. On these boards, task switching between the pedal update task and the serial task was only happening upon full scheduler ticks - every 1ms. This means that each task would loop multiple times before any processing time was offered to the other task. By passing the joystick value with a queue, the task cycles are synchronised, substantially decreasing both inter-task latency and average cycle time for the pedal update task. Note that on multi-core boards (eg ESP32) this effectively decreases the polling rate on the serial task. I don't anticipate any real negative effect of this, given that the rate is still quite high. --- Arduino/Esp32/Main/Main.ino | 38 ++++++++++--------------------------- 1 file changed, 10 insertions(+), 28 deletions(-) diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index 827369af..5f473612 100644 --- a/Arduino/Esp32/Main/Main.ino +++ b/Arduino/Esp32/Main/Main.ino @@ -78,8 +78,7 @@ static SemaphoreHandle_t semaphore_updateConfig=NULL; bool configUpdateAvailable = false; // semaphore protected data DAP_config_st dap_config_st_local; -static SemaphoreHandle_t semaphore_updateJoystick=NULL; - int32_t joystickNormalizedToInt32 = 0; // semaphore protected data +static QueueHandle_t queue_updateJoystick=NULL; bool resetPedalPosition = false; @@ -249,14 +248,14 @@ void setup() // setup multi tasking - semaphore_updateJoystick = xSemaphoreCreateMutex(); + queue_updateJoystick = xQueueCreate(1, sizeof(int32_t)); semaphore_updateConfig = xSemaphoreCreateMutex(); delay(10); - if(semaphore_updateJoystick==NULL) + if(queue_updateJoystick==NULL) { - Serial.println("Could not create semaphore"); + Serial.println("Could not create queue"); ESP.restart(); } if(semaphore_updateConfig==NULL) @@ -336,6 +335,7 @@ void updatePedalCalcParameters() /* */ /**********************************************************************************************/ void loop() { + taskYIELD(); } @@ -549,17 +549,8 @@ void pedalUpdateTask( void * pvParameters ) } // compute controller output - if(semaphore_updateJoystick!=NULL) - { - if(xSemaphoreTake(semaphore_updateJoystick, 1)==pdTRUE) { - joystickNormalizedToInt32 = NormalizeControllerOutputValue(filteredReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput); - xSemaphoreGive(semaphore_updateJoystick); - } - } - else - { - Serial.println("semaphore_updateJoystick == 0"); - } + int32_t joystickNormalizedToInt32 = NormalizeControllerOutputValue(filteredReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput); + xQueueSend(queue_updateJoystick, &joystickNormalizedToInt32, /*xTicksToWait=*/10); #ifdef PRINT_USED_STACK_SIZE unsigned int temp2 = uxTaskGetStackHighWaterMark(nullptr); @@ -707,19 +698,10 @@ void serialCommunicationTask( void * pvParameters ) // transmit controller output if (IsControllerReady()) { - if(semaphore_updateJoystick!=NULL) - { - if(xSemaphoreTake(semaphore_updateJoystick, 1)==pdTRUE) - { - joystickNormalizedToInt32_local = joystickNormalizedToInt32; - xSemaphoreGive(semaphore_updateJoystick); - } - else - { - Serial.println("semaphore_updateJoystick == 0"); - } + int32_t joystickNormalizedToInt32 = 0; + if (pdTRUE == xQueueReceive(queue_updateJoystick, &joystickNormalizedToInt32, /*xTicksToWait=*/10)) { + SetControllerOutputValue(joystickNormalizedToInt32); } - SetControllerOutputValue(joystickNormalizedToInt32_local); } } From e993de793b2db608ea11473fc0c21ca05b4ef8e8 Mon Sep 17 00:00:00 2001 From: Michael Fraser Date: Tue, 1 Aug 2023 11:33:56 +1000 Subject: [PATCH 3/4] Use queue for passing config from serial task to pedal update This is just a tidy-up as it allows simpler handling of the config struct. No intentional functional changes. --- Arduino/Esp32/Main/Main.ino | 76 ++++++------------------------------- 1 file changed, 12 insertions(+), 64 deletions(-) diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index 5f473612..f32f338e 100644 --- a/Arduino/Esp32/Main/Main.ino +++ b/Arduino/Esp32/Main/Main.ino @@ -74,11 +74,8 @@ ForceCurve_Interpolated forceCurve; TaskHandle_t Task1; TaskHandle_t Task2; -static SemaphoreHandle_t semaphore_updateConfig=NULL; - bool configUpdateAvailable = false; // semaphore protected data - DAP_config_st dap_config_st_local; - static QueueHandle_t queue_updateJoystick=NULL; +static QueueHandle_t queue_updateConfig=NULL; bool resetPedalPosition = false; @@ -238,31 +235,23 @@ void setup() // interprete config values dap_calculationVariables_st.updateFromConfig(dap_config_st); - // activate parameter update in first cycle - configUpdateAvailable = true; - // equalize pedal config for both tasks - dap_config_st_local = dap_config_st; - - // setup multi tasking queue_updateJoystick = xQueueCreate(1, sizeof(int32_t)); - semaphore_updateConfig = xSemaphoreCreateMutex(); + queue_updateConfig = xQueueCreate(1, sizeof(DAP_config_st)); delay(10); - if(queue_updateJoystick==NULL) + if(queue_updateJoystick==NULL || queue_updateConfig==NULL) { Serial.println("Could not create queue"); ESP.restart(); } - if(semaphore_updateConfig==NULL) - { - Serial.println("Could not create semaphore"); - ESP.restart(); - } + + // activate parameter update in first cycle + xQueueSend(queue_updateConfig, &dap_config_st, /*xTicksToWait=*/10); disableCore0WDT(); @@ -316,15 +305,6 @@ void updatePedalCalcParameters() // tune the PID settings tunePidValues(dap_config_st); - - // equalize pedal config for both tasks - dap_config_st_local = dap_config_st; - - - - - - } @@ -394,36 +374,15 @@ void pedalUpdateTask( void * pvParameters ) // if a config update was received over serial, update the variables required for further computation - if (configUpdateAvailable == true) { - if(semaphore_updateConfig!=NULL) - { - - bool configWasUpdated_b = false; - // Take the semaphore and just update the config file, then release the semaphore - if(xSemaphoreTake(semaphore_updateConfig, 1)==pdTRUE) + DAP_config_st dap_config_st_local; + if (pdTRUE == xQueueReceive(queue_updateConfig, &dap_config_st_local, /*xTicksToWait=*/0)) { Serial.println("Update pedal config!"); - configUpdateAvailable = false; dap_config_st = dap_config_st_local; - configWasUpdated_b = true; - xSemaphoreGive(semaphore_updateConfig); - } - - // update the calc params - if (true == configWasUpdated_b) - { - Serial.println("Updating the calc params!"); - configWasUpdated_b = false; dap_config_st.storeConfigToEprom(dap_config_st); // store config to EEPROM updatePedalCalcParameters(); // update the calc parameters } - - } - else - { - Serial.println("semaphore_updateConfig == 0"); - } } @@ -616,16 +575,8 @@ void serialCommunicationTask( void * pvParameters ) // likely config structure if ( n == sizeof(DAP_config_st) ) { - - if(semaphore_updateConfig!=NULL) - { - if(xSemaphoreTake(semaphore_updateConfig, 1)==pdTRUE) - { - DAP_config_st * dap_config_st_local_ptr; - dap_config_st_local_ptr = &dap_config_st_local; - Serial.readBytes((char*)dap_config_st_local_ptr, sizeof(DAP_config_st)); - - + DAP_config_st dap_config_st_local; + Serial.readBytes((char*)&dap_config_st_local, sizeof(DAP_config_st)); // check if data is plausible bool structChecker = true; @@ -644,7 +595,7 @@ void serialCommunicationTask( void * pvParameters ) Serial.println(dap_config_st_local.payLoadHeader_.version); } // checksum validation - uint16_t crc = checksumCalculator((uint8_t*)(&(dap_config_st_local_ptr->payLoadPedalConfig_)), sizeof(dap_config_st_local.payLoadPedalConfig_) ); + uint16_t crc = checksumCalculator((uint8_t*)(&(dap_config_st_local.payLoadPedalConfig_)), sizeof(dap_config_st_local.payLoadPedalConfig_)); if (crc != dap_config_st_local.payLoadHeader_.checkSum){ structChecker = false; Serial.print("CRC expected: "); @@ -658,11 +609,8 @@ void serialCommunicationTask( void * pvParameters ) if (structChecker == true) { Serial.println("Update pedal config!"); - configUpdateAvailable = true; + xQueueSend(queue_updateConfig, &dap_config_st_local, /*xTicksToWait=*/10); } - xSemaphoreGive(semaphore_updateConfig); - } - } } else { From 946dbabfa591dcb66899cfde8564b1204d8efa61 Mon Sep 17 00:00:00 2001 From: Michael Fraser Date: Tue, 1 Aug 2023 23:05:43 +1000 Subject: [PATCH 4/4] Tidy up RTDebugOutput implementation - Allow declaration without name array as equivalent functionality to 'offerDataWithoutText'. - New template argument FLOAT_PRECISION determines how many decimal places to use when printing float values. --- Arduino/Esp32/Main/RTDebugOutput.h | 63 +++++--------------- Arduino/Esp32/Main/StepperMovementStrategy.h | 5 +- 2 files changed, 17 insertions(+), 51 deletions(-) diff --git a/Arduino/Esp32/Main/RTDebugOutput.h b/Arduino/Esp32/Main/RTDebugOutput.h index aa3e8f1e..df770083 100644 --- a/Arduino/Esp32/Main/RTDebugOutput.h +++ b/Arduino/Esp32/Main/RTDebugOutput.h @@ -3,83 +3,50 @@ #include -template +template class RTDebugOutput { private: - SemaphoreHandle_t _semaphore_data; + QueueHandle_t _queue_data; std::array _outNames; - std::array _outValues; - bool _dataReady; - bool _withoutText = false; public: - RTDebugOutput(std::array outNames) + RTDebugOutput(std::array outNames = {}) : _outNames(outNames) - , _dataReady(false) { - _semaphore_data = xSemaphoreCreateMutex(); + _queue_data = xQueueCreate(1, sizeof(std::array)); xTaskCreatePinnedToCore(this->debugOutputTask, "debugOutputTask", 5000, this, 1, NULL, 1); } void offerData(std::array values) { - if(xSemaphoreTake(_semaphore_data, 0) == pdTRUE) { - _outValues = values; - _dataReady = true; - _withoutText = false; - xSemaphoreGive(_semaphore_data); - } - } - - - void offerDataWithoutText(std::array values) { - if(xSemaphoreTake(_semaphore_data, 0) == pdTRUE) { - _outValues = values; - _dataReady = true; - _withoutText = true; - xSemaphoreGive(_semaphore_data); - } + xQueueSend(_queue_data, &values, /*xTicksToWait=*/0); } - template void printValue(String name, T value) { - - if (_withoutText == false) - { - Serial.print(name); Serial.print(":"); Serial.print(value); Serial.print(","); + if (name.length() > 0) { + Serial.print(name); Serial.print(":"); } - else - { - Serial.print(value, 9); Serial.print(","); - } - + Serial.print(value); Serial.print(","); } void printValue(String name, float value) { - if (_withoutText == false) - { - Serial.print(name); Serial.print(":"); Serial.print(value,6); Serial.print(","); - } - else - { - Serial.print(value, 9); Serial.print(","); + if (name.length() > 0) { + Serial.print(name); Serial.print(":"); } + Serial.print(value, FLOAT_PRECISION); Serial.print(","); } void printData() { - if (xSemaphoreTake(_semaphore_data, 0) == pdTRUE) { - if (_dataReady) { + std::array values; + if (pdTRUE == xQueueReceive(_queue_data, &values, /*xTicksToWait=*/0)) { static SemaphoreHandle_t semaphore_print = xSemaphoreCreateMutex(); - if (xSemaphoreTake(semaphore_print, 0) == pdTRUE) { + if (xSemaphoreTake(semaphore_print, /*xTicksToWait=*/10) == pdTRUE) { for (int i=0; igetCurrentPositionFraction(); loadcellReading = (loadcellReading - calc_st->Force_Min) / calc_st->Force_Range; - static RTDebugOutput rtDebugFilter({ "t", "y", "F"}); - rtDebugFilter.offerDataWithoutText({ ((float)t) *1e-6 , currentPos, loadcellReading}); + static RTDebugOutput rtDebugFilter; + rtDebugFilter.offerData({ ((float)t) *1e-6 , currentPos, loadcellReading}); } } Serial.println("======================================"); Serial.println("End system identification data"); } -