From 0ee5c764665ab2172678efe6c28b8d9e6bb865b1 Mon Sep 17 00:00:00 2001 From: enjineerMan Date: Mon, 20 Jun 2022 21:31:06 -0400 Subject: [PATCH 1/5] pid thread done --- .../Inc/attitudeStateClasses.hpp | 4 +++- .../Src/attitudeStateClasses.cpp | 12 +++++++---- Safety/Controls/Src/pidInterface.cpp | 9 ++++++++ Safety/Controls/Src/pidInterface.hpp | 17 +++++++++++++++ Safety/boardfiles/Src/freertos.c | 21 +++++++++++++++++++ ZeroPilot.code-workspace | 7 +++++++ 6 files changed, 65 insertions(+), 5 deletions(-) create mode 100644 Safety/Controls/Src/pidInterface.cpp create mode 100644 Safety/Controls/Src/pidInterface.hpp create mode 100644 ZeroPilot.code-workspace diff --git a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp index 6b51ad94..fc95842e 100644 --- a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp +++ b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp @@ -17,7 +17,9 @@ /*********************************************************************************************************************** * Code **********************************************************************************************************************/ - +Instructions_t *_ControlsInstructions; +SFOutput_t *SFOutputG; +PID_Output_t _PidOutputG; //global pid output class fetchInstructionsMode : public attitudeState { public: diff --git a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp index e004097c..3d964d00 100644 --- a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp +++ b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp @@ -3,16 +3,17 @@ #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 **********************************************************************************************************************/ +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(); CommandsForAM fetchInstructionsMode::_PMInstructions; PPM_Instructions_t fetchInstructionsMode::_TeleopInstructions; @@ -130,7 +131,8 @@ attitudeState& sensorFusionMode::getInstance() void PIDloopMode::execute(attitudeManager* attitudeMgr) { CommandsForAM *PMInstructions = nullptr; - SFOutput_t *SFOutput = sensorFusionMode::GetSFOutput(); + // add mutex here if another thread changes this + SFOutput_t *SFOutputG = sensorFusionMode::GetSFOutput(); PID_Output_t *pidOut = nullptr; if(fetchInstructionsMode::isAutonomous()) @@ -145,7 +147,9 @@ void PIDloopMode::execute(attitudeManager* attitudeMgr) _ControlsInstructions -> input2 = teleopInstructions->PPMValues[1]; _ControlsInstructions -> input3 = teleopInstructions->PPMValues[2]; _ControlsInstructions -> input4 = teleopInstructions->PPMValues[3]; - _PidOutput = runControlsAndGetPWM(_ControlsInstructions, SFOutput); + osMutexWait(pidMutex, 1000); + _PidOutput = _PidOutputG; + osMutexRelease(pidMutex); } #ifdef FIXED_WING diff --git a/Safety/Controls/Src/pidInterface.cpp b/Safety/Controls/Src/pidInterface.cpp new file mode 100644 index 00000000..b715213c --- /dev/null +++ b/Safety/Controls/Src/pidInterface.cpp @@ -0,0 +1,9 @@ +#include "pidInterface.h" +#include "../../AttitudeManager/Inc/attitudeStateClasses.hpp" +#include "Controls.hpp" +void pidInterfaceInit(void) {} + +void pidInterfaceExecute(void) +{ + _PidOutputG = runControlsAndGetPWM(_ControlsInstructions, SFOutputG); +} \ No newline at end of file diff --git a/Safety/Controls/Src/pidInterface.hpp b/Safety/Controls/Src/pidInterface.hpp new file mode 100644 index 00000000..1317e681 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Safety/boardfiles/Src/freertos.c b/Safety/boardfiles/Src/freertos.c index 95f6c467..d6e9f8b0 100644 --- a/Safety/boardfiles/Src/freertos.c +++ b/Safety/boardfiles/Src/freertos.c @@ -64,6 +64,7 @@ /* Variables -----------------------------------------------------------------*/ osThreadId attitudeManagerHandle; osThreadId sensorFusionHandle; +osThreadId pidHandle; /* USER CODE BEGIN Variables */ /* USER CODE END Variables */ @@ -71,6 +72,7 @@ osThreadId sensorFusionHandle; /* 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) */ @@ -120,6 +122,7 @@ void MX_FREERTOS_Init(void) { SensorFusionInterfaceInit(); AttitudeManagerInterfaceInit(); + pidInterfaceInit(); /* USER CODE END Init */ /* USER CODE BEGIN RTOS_MUTEX */ @@ -142,6 +145,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, ... */ @@ -197,6 +204,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 */ diff --git a/ZeroPilot.code-workspace b/ZeroPilot.code-workspace new file mode 100644 index 00000000..ef9f5d27 --- /dev/null +++ b/ZeroPilot.code-workspace @@ -0,0 +1,7 @@ +{ + "folders": [ + { + "path": "." + } + ] +} \ No newline at end of file From e678ee7241a0d75d6ead224e1cac1aff049b855d Mon Sep 17 00:00:00 2001 From: enjineerMan Date: Tue, 21 Jun 2022 20:21:33 -0400 Subject: [PATCH 2/5] added comment for multiple definition error --- Safety/AttitudeManager/Inc/attitudeStateClasses.hpp | 5 +++-- Safety/AttitudeManager/Src/attitudeStateClasses.cpp | 5 ++--- Safety/Controls/Src/pidInterface.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp index fc95842e..9a798318 100644 --- a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp +++ b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp @@ -17,9 +17,10 @@ /*********************************************************************************************************************** * Code **********************************************************************************************************************/ -Instructions_t *_ControlsInstructions; +// multiple definition errors caused by these declarations +Instructions_t *_ControlsInstructions = new Instructions_t(); SFOutput_t *SFOutputG; -PID_Output_t _PidOutputG; //global pid output +PID_Output_t *_PidOutputG; class fetchInstructionsMode : public attitudeState { public: diff --git a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp index 3d964d00..a3c3619f 100644 --- a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp +++ b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp @@ -14,7 +14,7 @@ osMutexDef(MutexIsr); osMutexId pidMutex = osMutexCreate(osMutex(MutexIsr)); float OutputMixingMode::_channelOut[4]; SFOutput_t sensorFusionMode::_SFOutput; -Instructions_t *_ControlsInstructions = new Instructions_t(); +PID_Output_t *PIDloopMode::_PidOutput; CommandsForAM fetchInstructionsMode::_PMInstructions; PPM_Instructions_t fetchInstructionsMode::_TeleopInstructions; bool fetchInstructionsMode::_isAutonomous = false; @@ -131,8 +131,7 @@ attitudeState& sensorFusionMode::getInstance() void PIDloopMode::execute(attitudeManager* attitudeMgr) { CommandsForAM *PMInstructions = nullptr; - // add mutex here if another thread changes this - SFOutput_t *SFOutputG = sensorFusionMode::GetSFOutput(); + SFOutputG = sensorFusionMode::GetSFOutput(); PID_Output_t *pidOut = nullptr; if(fetchInstructionsMode::isAutonomous()) diff --git a/Safety/Controls/Src/pidInterface.cpp b/Safety/Controls/Src/pidInterface.cpp index b715213c..01850b11 100644 --- a/Safety/Controls/Src/pidInterface.cpp +++ b/Safety/Controls/Src/pidInterface.cpp @@ -1,4 +1,4 @@ -#include "pidInterface.h" +#include "pidInterface.hpp" #include "../../AttitudeManager/Inc/attitudeStateClasses.hpp" #include "Controls.hpp" void pidInterfaceInit(void) {} From 1759e6aa2c0b2c26395319a49f494513f3cb3f62 Mon Sep 17 00:00:00 2001 From: enjineerMan Date: Tue, 21 Jun 2022 20:39:31 -0400 Subject: [PATCH 3/5] fix by dynamic allocation and extern --- Safety/AttitudeManager/Inc/attitudeStateClasses.hpp | 6 +++--- Safety/AttitudeManager/Src/attitudeStateClasses.cpp | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp index 9a798318..60f9fa7d 100644 --- a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp +++ b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp @@ -18,9 +18,9 @@ * Code **********************************************************************************************************************/ // multiple definition errors caused by these declarations -Instructions_t *_ControlsInstructions = new Instructions_t(); -SFOutput_t *SFOutputG; -PID_Output_t *_PidOutputG; +extern Instructions_t *_ControlsInstructions; +extern SFOutput_t *SFOutputG; +extern PID_Output_t *_PidOutputG; class fetchInstructionsMode : public attitudeState { public: diff --git a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp index a3c3619f..ec7d8f71 100644 --- a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp +++ b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp @@ -9,7 +9,9 @@ /*********************************************************************************************************************** * Definitions **********************************************************************************************************************/ - +Instructions_t *_ControlsInstructions = new Instructions_t(); +SFOutput_t *SFOutputG = new SFOutput_t; +PID_Output_t *_PidOutputG = new PID_Output_t; osMutexDef(MutexIsr); osMutexId pidMutex = osMutexCreate(osMutex(MutexIsr)); float OutputMixingMode::_channelOut[4]; From 02b18aa8f5dee16e3995270ff005e6106fb70411 Mon Sep 17 00:00:00 2001 From: enjineerMan Date: Tue, 21 Jun 2022 20:45:55 -0400 Subject: [PATCH 4/5] better fix by initiating global vars to nullptr --- Safety/AttitudeManager/Src/attitudeStateClasses.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp index ec7d8f71..6882fb12 100644 --- a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp +++ b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp @@ -10,8 +10,8 @@ * Definitions **********************************************************************************************************************/ Instructions_t *_ControlsInstructions = new Instructions_t(); -SFOutput_t *SFOutputG = new SFOutput_t; -PID_Output_t *_PidOutputG = new PID_Output_t; +SFOutput_t *SFOutputG = nullptr; +PID_Output_t *_PidOutputG = nullptr; osMutexDef(MutexIsr); osMutexId pidMutex = osMutexCreate(osMutex(MutexIsr)); float OutputMixingMode::_channelOut[4]; @@ -149,7 +149,7 @@ void PIDloopMode::execute(attitudeManager* attitudeMgr) _ControlsInstructions -> input3 = teleopInstructions->PPMValues[2]; _ControlsInstructions -> input4 = teleopInstructions->PPMValues[3]; osMutexWait(pidMutex, 1000); - _PidOutput = _PidOutputG; + if(_PidOutputG != nullptr) _PidOutput = _PidOutputG; osMutexRelease(pidMutex); } From fc0635b38936faaed643e24d8ade3ac3662952a9 Mon Sep 17 00:00:00 2001 From: enjineerMan Date: Tue, 21 Jun 2022 20:50:07 -0400 Subject: [PATCH 5/5] refactor --- Safety/AttitudeManager/Inc/attitudeStateClasses.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp index 60f9fa7d..ddf36b5e 100644 --- a/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp +++ b/Safety/AttitudeManager/Inc/attitudeStateClasses.hpp @@ -17,7 +17,6 @@ /*********************************************************************************************************************** * Code **********************************************************************************************************************/ -// multiple definition errors caused by these declarations extern Instructions_t *_ControlsInstructions; extern SFOutput_t *SFOutputG; extern PID_Output_t *_PidOutputG;