Revert back to semaphore strategy due to control loop issues with que…

…ue based strategy

Arduino/Esp32/Main/Main.ino

@@ -75,8 +75,12 @@ ForceCurve_Interpolated forceCurve;
 TaskHandle_t Task1;
 TaskHandle_t Task2;
 
-static QueueHandle_t queue_updateJoystick=NULL;
-static QueueHandle_t queue_updateConfig=NULL;
+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
 
 bool resetPedalPosition = false;
 
@@ -236,23 +240,31 @@ 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));
-  queue_updateConfig   = xQueueCreate(1, sizeof(DAP_config_st));
+  semaphore_updateJoystick = xSemaphoreCreateMutex();
+  semaphore_updateConfig = xSemaphoreCreateMutex();
   delay(10);
 
 
-  if(queue_updateJoystick==NULL || queue_updateConfig==NULL)
+  if(semaphore_updateJoystick==NULL)
   {
-    Serial.println("Could not create queue");
+    Serial.println("Could not create semaphore");
+    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();
 
@@ -306,6 +318,15 @@ void updatePedalCalcParameters()
 
   // tune the PID settings
   tunePidValues(dap_config_st);
+
+  // equalize pedal config for both tasks
+  dap_config_st_local = dap_config_st;
+
+
+
+
+
+
 }
 
 
@@ -375,15 +396,36 @@ void pedalUpdateTask( void * pvParameters )
 
 
     // if a config update was received over serial, update the variables required for further computation
+    if (configUpdateAvailable == true)
     {
-        DAP_config_st dap_config_st_local;
-        if (pdTRUE == xQueueReceive(queue_updateConfig, &dap_config_st_local, /*xTicksToWait=*/0))
+      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)
         {
           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");
+      }
     }
 
 
@@ -509,8 +551,17 @@ void pedalUpdateTask( void * pvParameters )
     }
 
     // compute controller output
-    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);
+    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");
+    }
 
     #ifdef PRINT_USED_STACK_SIZE
       unsigned int temp2 = uxTaskGetStackHighWaterMark(nullptr);
@@ -576,8 +627,16 @@ void serialCommunicationTask( void * pvParameters )
     // likely config structure 
     if ( n == sizeof(DAP_config_st) )
     {
-          DAP_config_st dap_config_st_local;
-          Serial.readBytes((char*)&dap_config_st_local, 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));
+
+
 
           // check if data is plausible
           bool structChecker = true;
@@ -610,8 +669,11 @@ void serialCommunicationTask( void * pvParameters )
           if (structChecker == true)
           {
             Serial.println("Update pedal config!");
-            xQueueSend(queue_updateConfig, &dap_config_st_local, /*xTicksToWait=*/10);
+            configUpdateAvailable = true;          
           }
+          xSemaphoreGive(semaphore_updateConfig);
+        }
+      }
     }
     else
     {
@@ -647,10 +709,19 @@ void serialCommunicationTask( void * pvParameters )
 
     // transmit controller output
     if (IsControllerReady()) {
-      int32_t joystickNormalizedToInt32 = 0;
-      if (pdTRUE == xQueueReceive(queue_updateJoystick, &joystickNormalizedToInt32, /*xTicksToWait=*/10)) {
-        SetControllerOutputValue(joystickNormalizedToInt32);
+      if(semaphore_updateJoystick!=NULL)
+      {
+        if(xSemaphoreTake(semaphore_updateJoystick, 1)==pdTRUE)
+        {
+          joystickNormalizedToInt32_local = joystickNormalizedToInt32;
+          xSemaphoreGive(semaphore_updateJoystick);
+        }
+        else
+        {
+          Serial.println("semaphore_updateJoystick == 0");
+        }
       }
+      SetControllerOutputValue(joystickNormalizedToInt32_local);
     }
 
   }

Arduino/Esp32/Main/StepperMovementStrategy.h

@@ -18,7 +18,7 @@ double Kp=0.3, Ki=50.0, Kd=0.000;
 QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd,  /* OPTIONS */
                myPID.pMode::pOnError,                   /* pOnError, pOnMeas, pOnErrorMeas */
                myPID.dMode::dOnMeas,                    /* dOnError, dOnMeas */
-               myPID.iAwMode::iAwCondition,             /* iAwCondition, iAwClamp, iAwOff */
+               myPID.iAwMode::iAwOff,             /* iAwCondition, iAwClamp, iAwOff */
                myPID.Action::direct);                   /* direct, reverse */
 bool pidWasInitialized = false;