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..f32f338e 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 @@ -77,12 +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 SemaphoreHandle_t semaphore_updateJoystick=NULL; - int32_t joystickNormalizedToInt32 = 0; // semaphore protected data +static QueueHandle_t queue_updateJoystick=NULL; +static QueueHandle_t queue_updateConfig=NULL; bool resetPedalPosition = false; @@ -242,32 +235,24 @@ 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 - semaphore_updateJoystick = xSemaphoreCreateMutex(); - semaphore_updateConfig = xSemaphoreCreateMutex(); + queue_updateJoystick = xQueueCreate(1, sizeof(int32_t)); + queue_updateConfig = xQueueCreate(1, sizeof(DAP_config_st)); delay(10); - if(semaphore_updateJoystick==NULL) + if(queue_updateJoystick==NULL || queue_updateConfig==NULL) { - Serial.println("Could not create semaphore"); - ESP.restart(); - } - if(semaphore_updateConfig==NULL) - { - Serial.println("Could not create semaphore"); + Serial.println("Could not create queue"); ESP.restart(); } + // activate parameter update in first cycle + xQueueSend(queue_updateConfig, &dap_config_st, /*xTicksToWait=*/10); + disableCore0WDT(); //create a task that will be executed in the Task2code() function, with priority 1 and executed on core 1 @@ -320,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; - - - - - - } @@ -339,6 +315,7 @@ void updatePedalCalcParameters() /* */ /**********************************************************************************************/ void loop() { + taskYIELD(); } @@ -391,41 +368,21 @@ void pedalUpdateTask( void * pvParameters ) // print the execution time averaged over multiple cycles #ifdef PRINT_CYCLETIME + static CycleTimer timerPU("PU cycle time"); timerPU.Bump(); #endif // 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"); - } } @@ -551,17 +508,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); @@ -613,6 +561,7 @@ void serialCommunicationTask( void * pvParameters ) // average cycle time averaged over multiple cycles #ifdef PRINT_CYCLETIME + static CycleTimer timerSC("SC cycle time"); timerSC.Bump(); #endif @@ -626,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; @@ -654,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: "); @@ -668,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 { @@ -708,19 +646,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); } } diff --git a/Arduino/Esp32/Main/RTDebugOutput.h b/Arduino/Esp32/Main/RTDebugOutput.h index 4425e6af..df770083 100644 --- a/Arduino/Esp32/Main/RTDebugOutput.h +++ b/Arduino/Esp32/Main/RTDebugOutput.h @@ -3,79 +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) { - for (int i=0; i values; + if (pdTRUE == xQueueReceive(_queue_data, &values, /*xTicksToWait=*/0)) { + static SemaphoreHandle_t semaphore_print = xSemaphoreCreateMutex(); + 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"); } -