Added protection against dereferencing nullptr in TMStateData obj

Yaremadzulynsky · Yaremadzulynsky · commit 56825cc0c4ae · 2024-11-02T13:39:07.000-04:00
diff --git a/SystemManager/Src/SystemManager.cpp b/SystemManager/Src/SystemManager.cpp
@@ -5,27 +5,26 @@
 
 #include "SystemManager.hpp"
 
+#include <iostream>
+
+#include "FreeRTOS.h"
+#include "GroundStationCommunication.hpp"
+#include "TelemetryManager.hpp"
+#include "cmsis_os.h"
 #include "drivers_config.hpp"
 #include "independent_watchdog.h"
 #include "rcreceiver_datatypes.h"
 #include "sbus_defines.h"
 #include "sbus_receiver.hpp"
 #include "tim.h"
-#include "GroundStationCommunication.hpp"
-#include "TelemetryManager.hpp"
-#include "GroundStationCommunication.hpp"
-#include "TelemetryManager.hpp"
-#include "cmsis_os.h"
-#include "FreeRTOS.h"
-#include <iostream>
 
 extern "C" {
-  #include "app_fatfs.h"
-  #include "log_util.h"
+#include "app_fatfs.h"
+#include "log_util.h"
 }
 
-#define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
-#define TIMOUT_MS      10000 // 10 sec
+#define TIMEOUT_CYCLES 250000  // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
+#define TIMOUT_MS 10000        // 10 sec
 
 // 0 - AM, 1 - TM, 2 - PM
 static TaskHandle_t taskHandles[3];
@@ -52,6 +51,7 @@ SystemManager::SystemManager()
     TMStateData stateData;
 
     // values to be assigned to stateData
+    // THIS IS JUST A PLACEHOLDER, THE MEMORY ADDRESSES NEED TO LIVE LONGER THAN THE SM CONSTRUCTOR
     int32_t alt = 0;
     int32_t lat = 0;
     int32_t lon = 0;
@@ -69,41 +69,41 @@ SystemManager::SystemManager()
     float yawspeed = 0;
 
     // use the memory address of the above, since stateData uses pointers
-    stateData.alt = &alt;
-    stateData.lat = &lat;
-    stateData.lon = &lon;
-    stateData.relative_alt = &relative_alt;
-    stateData.vx = &vx;
-    stateData.vy = &vy;
-    stateData.vz = &vz;
-    stateData.hdg = &hdg;
-    stateData.time_boot_ms = &time_boot_ms;
-    stateData.roll = &roll;
-    stateData.pitch = &pitch;
-    stateData.yaw = &yaw;
-    stateData.rollspeed = &rollspeed;
-    stateData.pitchspeed = &pitchspeed;
-    stateData.yawspeed = &yawspeed;
+    stateData.set_alt(&alt);
+    stateData.set_lat(&lat);
+    stateData.set_lon(&lon);
+    stateData.set_relative_alt(&relative_alt);
+    stateData.set_vx(&vx);
+    stateData.set_vy(&vy);
+    stateData.set_vz(&vz);
+    stateData.set_hdg(&hdg);
+    stateData.set_time_boot_ms(&time_boot_ms);
+    stateData.set_roll(&roll);
+    stateData.set_pitch(&pitch);
+    stateData.set_yaw(&yaw);
+    stateData.set_rollspeed(&rollspeed);
+    stateData.set_pitchspeed(&pitchspeed);
+    stateData.set_yawspeed(&yawspeed);
 
     MAV_STATE mavState = MAV_STATE::MAV_STATE_STANDBY;
     MAV_MODE_FLAG mavMode = MAV_MODE_FLAG::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 
-    // Creating parameters for the GroundStationCommunication that will be passed to telemetryManager
+    // Creating parameters for the GroundStationCommunication that will be passed to
+    // telemetryManager
     TMCircularBuffer DMAReceiveBuffer = *(new TMCircularBuffer(rfd900_circular_buffer));
 
-
     // the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the
     // ground station.
-    uint8_t* lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
+    uint8_t *lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
 
     // the buffer that stores routine/high_priority bytes (ex. heading, general state data) to be
     // sent to the ground station.
-    uint8_t* highPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
+    uint8_t *highPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
 
-    GroundStationCommunication GSC = *(new GroundStationCommunication(DMAReceiveBuffer, lowPriorityTransmitBuffer, 
-                                                                    highPriorityTransmitBuffer, RFD900_BUF_SIZE));
+    GroundStationCommunication GSC = *(new GroundStationCommunication(
+        DMAReceiveBuffer, lowPriorityTransmitBuffer, highPriorityTransmitBuffer, RFD900_BUF_SIZE));
 
-    // the buffer that stores the bytes received from the ground station.                                           
+    // the buffer that stores the bytes received from the ground station.
     MavlinkTranslator MT;
 
     this->telemetryManager = new TelemetryManager(stateData, mavState, mavMode, GSC, MT);
@@ -112,14 +112,14 @@ SystemManager::SystemManager()
     // REGULAR INTERVAL AS IT DEALS WITH MESSAGE DECODING AND LOW PRIORITY DATA TRANSMISSION
 }
 
-
-//wrapper functions are needed as FreeRTOS xTaskCreate function does not accept functions that have "this" pointers
-void SystemManager::attitudeManagerTaskWrapper(void* pvParameters){
-    SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);  
+// wrapper functions are needed as FreeRTOS xTaskCreate function does not accept functions that have
+// "this" pointers
+void SystemManager::attitudeManagerTaskWrapper(void *pvParameters) {
+    SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
     systemManagerInstance->attitudeManagerTask();
 }
 
-void SystemManager::telemetryManagerTaskWrapper(void* pvParameters){
+void SystemManager::telemetryManagerTaskWrapper(void *pvParameters) {
     SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
     systemManagerInstance->telemetryManagerTask();
 }
@@ -129,13 +129,13 @@ void SystemManager::pathManagerTaskWrapper(void *pvParameters) {
     systemManagerInstance->pathManagerTask();
 }
 
-void SystemManager::systemManagerTask(){
+void SystemManager::systemManagerTask() {
     TickType_t xNextWakeTime = xTaskGetTickCount();
     uint16_t frequency = 5;
 
-    for(;;){
+    for (;;) {
         printf("SM called\r\n");
-        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7); // toggle led light for testing
+        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);  // toggle led light for testing
 
         this->rcInputs_ = rcController_->GetRCControl();
 
@@ -177,42 +177,42 @@ void SystemManager::systemManagerTask(){
             this->invertedRollMotorChannel_.set(SBUS_MAX / 2);
         }
 
-        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency); 
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
     }
 }
 
-void SystemManager::attitudeManagerTask(){
+void SystemManager::attitudeManagerTask() {
     TickType_t xNextWakeTime = xTaskGetTickCount();
     uint16_t frequency = 5;
 
-    for(;;){
-        //call AM
+    for (;;) {
+        // call AM
 
         printf("AM called\r\n");
-        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7); // toggle led light for testing
-        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);  
+        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7);  // toggle led light for testing
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
     }
 }
 
-void SystemManager::telemetryManagerTask(){
+void SystemManager::telemetryManagerTask() {
     TickType_t xNextWakeTime = xTaskGetTickCount();
     uint16_t frequency = 5;
 
-    for(;;){
-        //call TM
+    for (;;) {
+        // call TM
 
         printf("TM called\r\n");
-        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_9); // toggle led light for testing
-        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);  
+        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_9);  // toggle led light for testing
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
     }
 }
 
-void SystemManager::pathManagerTask(){
+void SystemManager::pathManagerTask() {
     TickType_t xNextWakeTime = xTaskGetTickCount();
     uint16_t frequency = 5;
 
-    for(;;){
-        //call PM
+    for (;;) {
+        // call PM
 
         printf("PM called\r\n");
         vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
@@ -226,14 +226,18 @@ void SystemManager::startSystemManager() {
     if (MX_FATFS_Init() != APP_OK) {
         Error_Handler();
     }
-    logInit();  
-
-    //BaseType_t xTaskCreate( TaskFunction_t pvTaskCode, const char * const pcName, configSTACK_DEPTH_TYPE usStackDepth, void * pvParameters, UBaseType_t uxPriority, TaskHandle_t * pxCreatedTask ); 
-    //                          function's name             description                 size of stack to allocate        parameters for task        priority                    handler 
+    logInit();
+
+    // BaseType_t xTaskCreate( TaskFunction_t pvTaskCode, const char * const pcName,
+    // configSTACK_DEPTH_TYPE usStackDepth, void * pvParameters, UBaseType_t uxPriority,
+    // TaskHandle_t * pxCreatedTask );
+    //                           function's name             description                 size of
+    //                           stack to allocate        parameters for task        priority
+    //                           handler
     xTaskCreate(attitudeManagerTaskWrapper, "AM TASK", 800U, this, osPriorityNormal, taskHandles);
-    xTaskCreate(telemetryManagerTaskWrapper, "TM TASK", 800U, this, osPriorityNormal, taskHandles + 1);
+    xTaskCreate(telemetryManagerTaskWrapper, "TM TASK", 800U, this, osPriorityNormal,
+                taskHandles + 1);
     // xTaskCreate(pathManagerTaskWrapper, "PM TASK", 800U, this, osPriorityNormal, taskHandles +2);
 
     systemManagerTask();
-
 }
diff --git a/TelemetryManager/Inc/TelemetryManager.hpp b/TelemetryManager/Inc/TelemetryManager.hpp
@@ -16,79 +16,122 @@
 #ifndef TELEMETRYMANAGER_H
 #define TELEMETRYMANAGER_H
 
+#define CREATE_GETTER(type, member) \
+    type get_##member() const { \
+        return (member != nullptr) ? *member : std::numeric_limits<type>::max(); \
+    }
+
+#define CREATE_SETTER(type, member) \
+    void set_##member(type* value) { member = value; }
+
 #include "FreeRTOS.h"
 #include "GroundStationCommunication.hpp"
 #include "MavlinkTranslator.hpp"
 #include "task.h"
+#include <limits>
 
 struct TMStateData {
+   private:
     /*References to variables that contain the state of the drone (lat, lng, yaw, pitch, etc..).
      * They are updated by System Manager*/
     // Altitude (MSL) (unit:
     // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int32_t* alt;
+    int32_t* alt = nullptr;
 
     // The latitude of the drone (unit:
     // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int32_t* lat;
+    int32_t* lat = nullptr;
 
     // The longitude of the drone (unit:
     // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int32_t* lon;
+    int32_t* lon = nullptr;
 
     // Altitude above home (unit:
     // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int32_t* relative_alt;
+    int32_t* relative_alt = nullptr;
 
     // Ground X Speed (Latitude, positive north) (unit:
     // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int16_t* vx;
+    int16_t* vx = nullptr;
 
     // Ground Y Speed (Longitude, positive east) (unit:
     // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int16_t* vy;
+    int16_t* vy = nullptr;
 
     // Ground Z Speed (Altitude, positive down) (unit:
     // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int16_t* vz;
+    int16_t* vz = nullptr;
 
     // Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
     // (unit: cdeg)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    uint16_t* hdg;
+    uint16_t* hdg = nullptr;
     // Timestamp (time since system boot) (unit: ms)
     // (https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
-    int32_t* time_boot_ms;
+    int32_t* time_boot_ms = nullptr;
 
     // Roll angle (-pi..+pi) (unit: rad)
     // (https://mavlink.io/en/messages/common.html#ATTITUDE).
-    float* roll;
+    float* roll = nullptr;
 
     // Pitch angle (-pi..+pi) (unit: rad)
     // (https://mavlink.io/en/messages/common.html#ATTITUDE).
-    float* pitch;
+    float* pitch = nullptr;
 
     // Yaw angle (-pi..+pi) (unit: rad)
     // (https://mavlink.io/en/messages/common.html#ATTITUDE).
-    float* yaw;
+    float* yaw = nullptr;
 
     // Roll angular speed (unit: rad/s)
     // (https://mavlink.io/en/messages/common.html#ATTITUDE).
-    float* rollspeed;
+    float* rollspeed = nullptr;
 
     // Pitch angular speed (unit: rad/s)
     // (https://mavlink.io/en/messages/common.html#ATTITUDE).
-    float* pitchspeed;
+    float* pitchspeed = nullptr;
 
     // yawspeed Yaw angular speed (unit: rad/s)
     // (https://mavlink.io/en/messages/common.html#ATTITUDE).
-    float* yawspeed;
+    float* yawspeed = nullptr;
 
+   public:
+    // Generate getters using the macro
+    CREATE_GETTER(int32_t, alt)
+    CREATE_GETTER(int32_t, lat)
+    CREATE_GETTER(int32_t, lon)
+    CREATE_GETTER(int32_t, relative_alt)
+    CREATE_GETTER(int16_t, vx)
+    CREATE_GETTER(int16_t, vy)
+    CREATE_GETTER(int16_t, vz)
+    CREATE_GETTER(uint16_t, hdg)
+    CREATE_GETTER(int32_t, time_boot_ms)
+    CREATE_GETTER(float, roll)
+    CREATE_GETTER(float, pitch)
+    CREATE_GETTER(float, yaw)
+    CREATE_GETTER(float, rollspeed)
+    CREATE_GETTER(float, pitchspeed)
+    CREATE_GETTER(float, yawspeed)
+
+    CREATE_SETTER(int32_t, alt)
+    CREATE_SETTER(int32_t, lat)
+    CREATE_SETTER(int32_t, lon)
+    CREATE_SETTER(int32_t, relative_alt)
+    CREATE_SETTER(int16_t, vx)
+    CREATE_SETTER(int16_t, vy)
+    CREATE_SETTER(int16_t, vz)
+    CREATE_SETTER(uint16_t, hdg)
+    CREATE_SETTER(int32_t, time_boot_ms)
+    CREATE_SETTER(float, roll)
+    CREATE_SETTER(float, pitch)
+    CREATE_SETTER(float, yaw)
+    CREATE_SETTER(float, rollspeed)
+    CREATE_SETTER(float, pitchspeed)
+    CREATE_SETTER(float, yawspeed)
 };
 
 class TelemetryManager {
    public:
-
-    // struct containing variables that relate to the state of the drone (lat, lng, yaw, pitch, etc..).
+    // struct containing variables that relate to the state of the drone (lat, lng, yaw, pitch,
+    // etc..).
     TMStateData stateData;
 
     // the buffer that stores the bytes received from the ground station.
@@ -97,12 +140,10 @@ class TelemetryManager {
     // The object that facilitates communication with the ground station
     GroundStationCommunication& GSC;
 
-
     // System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE).
     MAV_STATE& mavState;
     // System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG).
     MAV_MODE_FLAG& mavMode;
-    
 
     /**
      * @brief Create and configure FreeRTOS tasks.
@@ -121,11 +162,12 @@ class TelemetryManager {
      * To do so call the init() method.
      * @param stateData The state of the drone (lat, lng, yaw, pitch, etc..).
      * @param mav_state System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE).
-     * @param mav_mode System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG).
+     * @param mav_mode System mode bitmap
+     * (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG).
      * @param GSC Object to handle communication with the groundstation
      * @param MT Object to translate MAVLink data
      */
-    TelemetryManager(TMStateData& stateData, MAV_STATE& mavState, MAV_MODE_FLAG& mavMode, 
+    TelemetryManager(TMStateData& stateData, MAV_STATE& mavState, MAV_MODE_FLAG& mavMode,
                      GroundStationCommunication& GSC, MavlinkTranslator& MT);
 
     /**
diff --git a/TelemetryManager/Src/TelemetryManager.cpp b/TelemetryManager/Src/TelemetryManager.cpp
