Pid thread

Safety/AttitudeManager/Inc/attitudeStateClasses.hpp

@@ -17,7 +17,9 @@
 /***********************************************************************************************************************
  * Code
  **********************************************************************************************************************/
-
+extern Instructions_t *_ControlsInstructions;
+extern SFOutput_t *SFOutputG;
+extern PID_Output_t *_PidOutputG; 
 class fetchInstructionsMode : public attitudeState
 {
     public:

Safety/AttitudeManager/Src/attitudeStateClasses.cpp

@@ -3,17 +3,20 @@
 #include "safetyConfig.hpp"
 #include "RSSI.hpp"
 #include "PID.hpp"
-// #include "CommFromPMToAM.hpp"
+#include "../../boardfiles/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h"
 #include "../../boardfiles/Drivers/STM32F0xx_HAL_Driver/Inc/stm32f0xx_hal.h"
 
 /***********************************************************************************************************************
  * Definitions
  **********************************************************************************************************************/
-
+Instructions_t *_ControlsInstructions = new Instructions_t();
+SFOutput_t *SFOutputG = nullptr;
+PID_Output_t *_PidOutputG = nullptr; 
+osMutexDef(MutexIsr);
+osMutexId pidMutex = osMutexCreate(osMutex(MutexIsr));
 float OutputMixingMode::_channelOut[4];
 SFOutput_t sensorFusionMode::_SFOutput;
-PID_Output_t *PIDloopMode::_PidOutput;
-Instructions_t *_ControlsInstructions = new Instructions_t();
+PID_Output_t *PIDloopMode::_PidOutput; 
 CommandsForAM fetchInstructionsMode::_PMInstructions;
 PPM_Instructions_t fetchInstructionsMode::_TeleopInstructions;
 bool fetchInstructionsMode::_isAutonomous = false;
@@ -130,7 +133,7 @@ attitudeState& sensorFusionMode::getInstance()
 void PIDloopMode::execute(attitudeManager* attitudeMgr)
 {
     CommandsForAM *PMInstructions = nullptr;
-    SFOutput_t *SFOutput = sensorFusionMode::GetSFOutput();
+    SFOutputG = sensorFusionMode::GetSFOutput();
 
     PID_Output_t *pidOut = nullptr;
     if(fetchInstructionsMode::isAutonomous())
@@ -147,7 +150,9 @@ void PIDloopMode::execute(attitudeManager* attitudeMgr)
         _ControlsInstructions -> input2 = -map(teleopInstructions->PPMValues[1], 0, 100, -max_angle, max_angle);
         _ControlsInstructions -> input3 = teleopInstructions->PPMValues[2];
         _ControlsInstructions -> input4 = -map(teleopInstructions->PPMValues[3], 0, 100, -max_rotation_rate, max_rotation_rate);
-        _PidOutput = runControlsAndGetPWM(_ControlsInstructions, SFOutput);
+        osMutexWait(pidMutex, 1000);
+        if(_PidOutputG != nullptr) _PidOutput = _PidOutputG;
+        osMutexRelease(pidMutex);
     }
 
     #ifdef FIXED_WING

Safety/Controls/Src/pidInterface.cpp

@@ -0,0 +1,9 @@
+#include "pidInterface.hpp"
+#include "../../AttitudeManager/Inc/attitudeStateClasses.hpp"
+#include "Controls.hpp"
+void pidInterfaceInit(void) {}
+
+void pidInterfaceExecute(void)
+{
+    _PidOutputG = runControlsAndGetPWM(_ControlsInstructions, SFOutputG);
+}

Safety/Controls/Src/pidInterface.hpp

@@ -0,0 +1,17 @@
+
+#ifndef PIDINTERFACE_H
+#define PIDINTERFACE_H
+// This interface exists so that freeRTOS can call C-Style functions while not messing up the fact that most of our development is in C++
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void pidInterfaceInit(void);
+void pidInterfaceExecute(void);
+
+#ifdef __cplusplus
+}
+#endif 
+
+#endif

Safety/boardfiles/Src/freertos.c

@@ -65,13 +65,15 @@
 /* Variables -----------------------------------------------------------------*/
 osThreadId attitudeManagerHandle;
 osThreadId sensorFusionHandle;
+osThreadId pidHandle;
 
 /* USER CODE BEGIN Variables */
 /* USER CODE END Variables */
 
 /* Function prototypes -------------------------------------------------------*/
 void attitudeManagerExecute(void const * argument);
 void sensorFusionExecute(void const * argument);
+void pidExecute(void const * argument);
 
 void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
 
@@ -121,6 +123,7 @@ void MX_FREERTOS_Init(void) {
 
     SensorFusionInterfaceInit();
     AttitudeManagerInterfaceInit();
+    pidInterfaceInit();
   /* USER CODE END Init */
 
   /* USER CODE BEGIN RTOS_MUTEX */
@@ -143,6 +146,10 @@ void MX_FREERTOS_Init(void) {
   /* definition and creation of sensorFusion */
   osThreadDef(sensorFusion, sensorFusionExecute, osPriorityNormal, 0, 2000);
   sensorFusionHandle = osThreadCreate(osThread(sensorFusion), NULL);
+
+    /* definition and creation of pid */
+  osThreadDef(pid, pidExecute, osPriorityNormal, 0, 2000);
+  pidHandle = osThreadCreate(osThread(pid), NULL);
 
   /* USER CODE BEGIN RTOS_THREADS */
   /* add threads, ... */
@@ -198,6 +205,20 @@ void sensorFusionExecute(void const * argument)
   /* USER CODE END sensorFusionExecute */
 }
 
+/* pidExecute function */
+void pidExecute(void const * argument)
+{
+  /* USER CODE BEGIN pidExecute */
+  UBaseType_t uxHighWaterMark = uxTaskGetStackHighWaterMark( NULL );
+  /* Infinite loop */
+  for(;;)
+  {
+    pidInterfaceExecute();
+    uxHighWaterMark = uxTaskGetStackHighWaterMark( NULL );
+    vTaskDelay(20);
+  }
+  /* USER CODE END pidExecute */
+}
 /* USER CODE BEGIN Application */
 
 /* USER CODE END Application */

ZeroPilot.code-workspace

@@ -0,0 +1,7 @@
+{
+    "folders": [
+        {
+            "path": "."
+        }
+    ]
+}