chore: improve code quality

zeropilot3.5/include/attitude_manager/attitude_manager.hpp

@@ -4,7 +4,6 @@
 #include "systemutils_iface.hpp"
 #include "flightmode.hpp"
 #include "queue_iface.hpp"
-#include "motor_iface.hpp"
 #include "motor_datatype.hpp"
 #include "gps_iface.hpp"
 #include "tm_queue.hpp"
@@ -49,7 +48,7 @@ class AttitudeManager {
         IMessageQueue<char[100]> *smLoggerQueue;
 
         Flightmode *controlAlgorithm;
-        RCMotorControlMessage_t controlMsg;
+        RCMotorControlMessage_t controlMsg = {};
         int noDataCount = 0;
 
         MotorGroupInstance_t *rollMotors;
@@ -64,9 +63,9 @@ class AttitudeManager {
 
         uint8_t amSchedulingCounter;
 
-        bool getControlInputs(RCMotorControlMessage_t *pControlMsg);
+        bool getControlInputs(RCMotorControlMessage_t *pControlMsg) const;
 
-        void outputToMotor(ControlAxis_t axis, uint8_t percent);
+        void outputToMotor(ControlAxis_t AXIS, uint8_t PERCENT) const;
 
         void sendGPSDataToTelemetryManager(const GpsData_t &gpsData, const bool &armed);
 };

zeropilot3.5/include/attitude_manager/direct_mapping.hpp

@@ -2,9 +2,9 @@
 
 #include "flightmode.hpp"
 
-class DirectMapping : public Flightmode {
+class DirectMapping final : public Flightmode {
     public:
         DirectMapping() = default;
 
-        RCMotorControlMessage_t runControl(RCMotorControlMessage_t controlInput) override;
+        RCMotorControlMessage_t runControl(RCMotorControlMessage_t CONTROL_INPUTS) override;
 };

zeropilot3.5/include/driver_ifaces/gps_datatypes.hpp

@@ -2,8 +2,8 @@
 
 #include <cstdint>
 
-#define INVALID_TRACK_ANGLE -1
-#define INVALID_DATA -2
+#define INVALID_TRACK_ANGLE (-1)
+#define INVALID_DATA (-2)
 
 typedef struct {
     uint8_t year;

zeropilot3.5/include/driver_ifaces/rc_datatypes.hpp

@@ -9,7 +9,7 @@
 class RCControl {
     public:
 
-        float controlSignals[SBUS_INPUT_CHANNELS];
+        float controlSignals[SBUS_INPUT_CHANNELS] = {};
         bool isDataNew;
 
         float &roll     = controlSignals[0];
@@ -30,7 +30,7 @@ class RCControl {
         float &aux11    = controlSignals[15];
 
         RCControl operator=(const RCControl& other){
-            std::copy(other.controlSignals, other.controlSignals + SBUS_INPUT_CHANNELS, this->controlSignals);
+            std::copy_n(other.controlSignals, SBUS_INPUT_CHANNELS, this->controlSignals);
             this->isDataNew = other.isDataNew; // Add this line to copy the isDataNew flag
             return *this;
         }

zeropilot3.5/include/system_manager/system_manager.hpp

@@ -40,8 +40,8 @@ class SystemManager {
 
         uint8_t smSchedulingCounter;
 
-        void sendRCDataToAttitudeManager(const RCControl &rcData);
-        void sendRCDataToTelemetryManager(const RCControl &rcData);
-        void sendHeartbeatDataToTelemetryManager(uint8_t baseMode, uint32_t customMode, MAV_STATE systemStatus);
-        void sendMessagesToLogger();
+        void sendRCDataToAttitudeManager(const RCControl &rcData) const;
+        void sendRCDataToTelemetryManager(const RCControl &rcData) const;
+        void sendHeartbeatDataToTelemetryManager(uint8_t BASE_MODE, uint32_t customMode, MAV_STATE SYSTEM_STATUS) const;
+        void sendMessagesToLogger() const;
 };

zeropilot3.5/include/telemetry_manager/telemetry_manager.hpp

@@ -10,25 +10,24 @@
 #include "rc_motor_control.hpp"
 #include "rfd_iface.hpp"
 class TelemetryManager {
-  private:
     ISystemUtils *systemUtilsDriver;			                  // System Utils Driver
     IRFD *rfdDriver;										                    // Driver used to actually send mavlink messages
     IMessageQueue<TMMessage_t> *tmQueueDriver;				      // Driver that receives messages from other managers
     IMessageQueue<RCMotorControlMessage_t> *amQueueDriver;	// Driver that currently is only used to set arm/disarm
     IMessageQueue<mavlink_message_t> *messageBuffer{};	  	// GPOS, Attitude and Heartbeat/Connection Messages
-    mavlink_status_t status;
-    mavlink_message_t message;
-    mavlink_message_t overflowBuf;
+    mavlink_status_t status = {};
+    mavlink_message_t message = {};
+    mavlink_message_t overflowBuf = {};
     bool overflowMsgPending;
 
-    void handleRxMsg(const mavlink_message_t &msg);
-    void processMsgQueue();
+    void handleRxMsg(const mavlink_message_t &msg) const;
+    void processMsgQueue() const;
     void transmit();
     void reconstructMsg();
 
   public:
     TelemetryManager(ISystemUtils *systemUtilsDriver, IRFD *rfdDriver, IMessageQueue<TMMessage_t>  *tmQueueDriver,  IMessageQueue<RCMotorControlMessage_t> *amQueueDriver,IMessageQueue<mavlink_message_t> *messageBuffer);
-    ~TelemetryManager();
+    ~TelemetryManager() = default;
 
     void tmUpdate();
 };

zeropilot3.5/include/thread_msgs/tm_queue.hpp

@@ -48,37 +48,37 @@ typedef struct TMMessage{
     uint32_t timeBootMs = 0;
 } TMMessage_t;
 
-inline TMMessage_t heartbeatPack(uint32_t time_boot_ms, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) {
-    const TMMessageData_t DATA = {.heartbeatData={base_mode, custom_mode, system_status }};
-    return TMMessage_t{TMMessage_t::HEARTBEAT_DATA, DATA, time_boot_ms};
+inline TMMessage_t heartbeatPack(const uint32_t TIME_BOOT_MS, const uint8_t BASE_MODE, const uint32_t CUSTOM_MODE, const uint8_t SYSTEM_STATUS) {
+    const TMMessageData_t DATA = {.heartbeatData={BASE_MODE, CUSTOM_MODE, SYSTEM_STATUS }};
+    return TMMessage_t{TMMessage_t::HEARTBEAT_DATA, DATA, TIME_BOOT_MS};
 }
 
-inline TMMessage_t gposDataPack(uint32_t time_boot_ms, int32_t alt, int32_t lat, int32_t lon, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz,uint16_t hdg) {
-    const TMMessageData_t DATA = {.gposData={alt, lat, lon, relative_alt, vx, vy, vz, hdg }};
-    return TMMessage_t{TMMessage_t::GPOS_DATA, DATA, time_boot_ms};
+inline TMMessage_t gposDataPack(const uint32_t TIME_BOOT_MS, const int32_t ALT, const int32_t LAT, const int32_t LON, const int32_t RELATIVE_ALT, const int16_t VX, const int16_t VY, const int16_t VZ, const uint16_t HDG) {
+    const TMMessageData_t DATA = {.gposData={ALT, LAT, LON, RELATIVE_ALT, VX, VY, VZ, HDG }};
+    return TMMessage_t{TMMessage_t::GPOS_DATA, DATA, TIME_BOOT_MS};
 }
 
-inline TMMessage_t rcDataPack(uint32_t time_boot_ms, float roll, float pitch, float yaw, float throttle, float flap_angle, float arm) {
-    auto rollPPM = static_cast<uint16_t>(1000 + roll * 10);
-    auto pitchPPM = static_cast<uint16_t>(1000 + pitch * 10);
-    auto yawPPM = static_cast<uint16_t>(1000 + yaw * 10);
-    auto throttlePPM = static_cast<uint16_t>(1000 + throttle * 10);
-    auto flapAnglePPM = static_cast<uint16_t>(1000 + flap_angle * 10);
-    auto armPPM = static_cast<uint16_t>(1000 + arm * 10);
-    const TMMessageData_t DATA = {.rcData ={rollPPM, pitchPPM, yawPPM, throttlePPM, flapAnglePPM, armPPM }};
-    return TMMessage_t{TMMessage_t::RC_DATA, DATA, time_boot_ms};
+inline TMMessage_t rcDataPack(const uint32_t TIME_BOOT_MS, const float ROLL, const float PITCH, const float YAW, const float THROTTLE, const float FLAP_ANGLE, const float ARM) {
+    const auto ROLL_PPM = static_cast<uint16_t>(1000 + ROLL * 10);
+    const auto PITCH_PPM = static_cast<uint16_t>(1000 + PITCH * 10);
+    const auto YAW_PPM = static_cast<uint16_t>(1000 + YAW * 10);
+    const auto THROTTLE_PPM = static_cast<uint16_t>(1000 + THROTTLE * 10);
+    const auto FLAP_ANGLE_PPM = static_cast<uint16_t>(1000 + FLAP_ANGLE * 10);
+    const auto ARM_PPM = static_cast<uint16_t>(1000 + ARM * 10);
+    const TMMessageData_t DATA = {.rcData ={ROLL_PPM, PITCH_PPM, YAW_PPM, THROTTLE_PPM, FLAP_ANGLE_PPM, ARM_PPM }};
+    return TMMessage_t{TMMessage_t::RC_DATA, DATA, TIME_BOOT_MS};
 }
 
-inline TMMessage_t bmDataPack(uint32_t time_boot_ms, int16_t temperature, float *voltages, uint8_t voltage_len, int16_t current_battery, int32_t current_consumed,
-    int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state) {
+inline TMMessage_t bmDataPack(const uint32_t TIME_BOOT_MS, int16_t temperature, const float *const VOLTAGES, const uint8_t VOLTAGE_LEN, const int16_t CURRENT_BATTERY, const int32_t CURRENT_CONSUMED,
+    const int32_t ENERGY_CONSUMED, const int8_t BATTERY_REMAINING, const int32_t TIME_REMAINING, const uint8_t CHARGE_STATE) {
     uint16_t mavlinkVoltageArray[16] = {UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX};
-    for (int i = 0; i < voltage_len; i++) {
-    	mavlinkVoltageArray[i] = static_cast<uint16_t>(voltages[i]);
+    for (int i = 0; i < VOLTAGE_LEN; i++) {
+    	mavlinkVoltageArray[i] = static_cast<uint16_t>(VOLTAGES[i]);
     }
     if (temperature == -1) {
         temperature = INT16_MAX;
     }
-    const TMMessageData_t DATA = {.bmData ={temperature, mavlinkVoltageArray, current_battery,
-    current_consumed, energy_consumed, battery_remaining, time_remaining, charge_state}};
-    return TMMessage_t{TMMessage_t::BM_DATA, DATA, time_boot_ms};
+    const TMMessageData_t DATA = {.bmData ={temperature, mavlinkVoltageArray, CURRENT_BATTERY,
+    CURRENT_CONSUMED, ENERGY_CONSUMED, BATTERY_REMAINING, TIME_REMAINING, CHARGE_STATE}};
+    return TMMessage_t{TMMessage_t::BM_DATA, DATA, TIME_BOOT_MS};
 }

zeropilot3.5/src/attitude_manager/attitude_manager.cpp

@@ -36,12 +36,12 @@ AttitudeManager::AttitudeManager(
 
 void AttitudeManager::runControlLoopIteration() {
     // Get data from Queue and motor outputs
-    bool controlRes = getControlInputs(&controlMsg);
+    const bool CONTROL_RES = getControlInputs(&controlMsg);
 
     // Failsafe
     static bool failsafeTriggered = false;
 
-    if (controlRes != true) {
+    if (CONTROL_RES != true) {
         ++noDataCount;
 
         if (noDataCount * AM_MAIN_DELAY > 1000) {
@@ -75,25 +75,25 @@ void AttitudeManager::runControlLoopIteration() {
         controlMsg.throttle = 0;
     }
 
-    RCMotorControlMessage_t motorOutputs = controlAlgorithm->runControl(controlMsg);
+    const RCMotorControlMessage_t MOTOR_OUTPUTS = controlAlgorithm->runControl(controlMsg);
 
-    outputToMotor(YAW, motorOutputs.yaw);
-    outputToMotor(PITCH, motorOutputs.pitch);
-    outputToMotor(ROLL, motorOutputs.roll);
-    outputToMotor(THROTTLE, motorOutputs.throttle);
-    outputToMotor(FLAP_ANGLE, motorOutputs.flapAngle);
-    outputToMotor(STEERING, motorOutputs.yaw);
+    outputToMotor(YAW, MOTOR_OUTPUTS.yaw);
+    outputToMotor(PITCH, MOTOR_OUTPUTS.pitch);
+    outputToMotor(ROLL, MOTOR_OUTPUTS.roll);
+    outputToMotor(THROTTLE, MOTOR_OUTPUTS.throttle);
+    outputToMotor(FLAP_ANGLE, MOTOR_OUTPUTS.flapAngle);
+    outputToMotor(STEERING, MOTOR_OUTPUTS.yaw);
 
     // Send GPS data to telemetry manager
-    GpsData_t gpsData = gpsDriver->readData();
+    const GpsData_t GPS_DATA = gpsDriver->readData();
     if (amSchedulingCounter % (AM_SCHEDULING_RATE_HZ / AM_TELEMETRY_GPS_DATA_RATE_HZ) == 0) {
-        sendGPSDataToTelemetryManager(gpsData, controlMsg.arm > 0);
+        sendGPSDataToTelemetryManager(GPS_DATA, controlMsg.arm > 0);
     }
 
     amSchedulingCounter = (amSchedulingCounter + 1) % AM_SCHEDULING_RATE_HZ;
 }
 
-bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) {
+bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) const {
     if (amQueue->count() == 0) {
         return false;
     }
@@ -102,10 +102,10 @@ bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) {
     return true;
 }
 
-void AttitudeManager::outputToMotor(ControlAxis_t axis, uint8_t percent) {
-    MotorGroupInstance_t *motorGroup = nullptr;
+void AttitudeManager::outputToMotor(const ControlAxis_t AXIS, const uint8_t PERCENT) const {
+    const MotorGroupInstance_t *motorGroup = nullptr;
 
-    switch (axis) {
+    switch (AXIS) {
         case ROLL:
             motorGroup = rollMotors;
             break;
@@ -124,18 +124,16 @@ void AttitudeManager::outputToMotor(ControlAxis_t axis, uint8_t percent) {
         case STEERING:
             motorGroup = steeringMotors;
             break;
-        default:
-            return;
     }
 
     for (uint8_t i = 0; i < motorGroup->motorCount; i++) {
-        MotorInstance_t *motor = (motorGroup->motors + i);
+        const MotorInstance_t *motor = motorGroup->motors + i;
 
         if (motor->isInverted) {
-            motor->motorInstance->set(100 - percent);
+            motor->motorInstance->set(100 - PERCENT);
         }
         else {
-            motor->motorInstance->set(percent);
+            motor->motorInstance->set(PERCENT);
         }
     }
 }
@@ -155,14 +153,14 @@ void AttitudeManager::sendGPSDataToTelemetryManager(const GpsData_t &gpsData, co
     }
 
     // calculate relative altitude
-    float relativeAltitude = previouslyArmed ? (gpsData.altitude - armAltitude) : 0.0f;
+    const float RELATIVE_ALTITUDE = previouslyArmed ? gpsData.altitude - armAltitude : 0.0f;
 
     TMMessage_t gpsDataMsg = gposDataPack(
         systemUtilsDriver->getCurrentTimestampMs(), // time_boot_ms
         gpsData.altitude * 1000, // altitude in mm
         gpsData.latitude * 1e7,
         gpsData.longitude * 1e7,
-        relativeAltitude * 1000, // relative altitude in mm
+        RELATIVE_ALTITUDE * 1000, // relative altitude in mm
         gpsData.vx,
         gpsData.vy,
         gpsData.vz,

zeropilot3.5/src/attitude_manager/direct_mapping.cpp

@@ -1,5 +1,5 @@
 #include "direct_mapping.hpp"
 
-RCMotorControlMessage_t DirectMapping::runControl(RCMotorControlMessage_t controlInputs){
-    return controlInputs;
+RCMotorControlMessage_t DirectMapping::runControl(const RCMotorControlMessage_t CONTROL_INPUTS){
+    return CONTROL_INPUTS;
 }

zeropilot3.5/src/system_manager/system_manager.cpp

@@ -29,10 +29,10 @@ void SystemManager::smUpdate() {
     static int oldDataCount = 0;
     static bool rcConnected = true;
 
-    RCControl rcData = rcDriver->getRCData();
-    if (rcData.isDataNew) {
+    const RCControl RC_DATA = rcDriver->getRCData();
+    if (RC_DATA.isDataNew) {
         oldDataCount = 0;
-        sendRCDataToAttitudeManager(rcData);
+        sendRCDataToAttitudeManager(RC_DATA);
 
         if (!rcConnected) {
             loggerDriver->log("RC Reconnected");
@@ -41,37 +41,37 @@ void SystemManager::smUpdate() {
     } else {
         oldDataCount += 1;
 
-        if ((oldDataCount * SM_MAIN_DELAY > 500) && rcConnected) {
+        if (oldDataCount * SM_MAIN_DELAY > 500 && rcConnected) {
             loggerDriver->log("RC Disconnected");
             rcConnected = false;
         }
     }
 
     // Send RC data to TM
     if (smSchedulingCounter % (SM_SCHEDULING_RATE_HZ / SM_TELEMETRY_RC_DATA_RATE_HZ) == 0) {
-        sendRCDataToTelemetryManager(rcData);
+        sendRCDataToTelemetryManager(RC_DATA);
     }
 
     // Populate baseMode based on arm state
     uint8_t baseMode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-    if (rcData.arm) {
+    if (RC_DATA.arm) {
         baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
 
     // Determine system status based on RC connection and arm state
     MAV_STATE systemStatus = MAV_STATE_ACTIVE;
     if (!rcConnected) {
         systemStatus = MAV_STATE_CRITICAL;
-    } else if (!rcData.arm) {
+    } else if (!RC_DATA.arm) {
         systemStatus = MAV_STATE_STANDBY;
     }
 
-    // Custom mode not used, set to 0
-    uint32_t customMode = 0;
-
     // Send Heartbeat data to TM at a 1Hz rate
     if (smSchedulingCounter % (SM_SCHEDULING_RATE_HZ / SM_TELEMETRY_HEARTBEAT_RATE_HZ) == 0) {
-        sendHeartbeatDataToTelemetryManager(baseMode, customMode, systemStatus);
+        // Custom mode not used, set to 0
+        constexpr uint32_t CUSTOM_MODE = 0;
+
+        sendHeartbeatDataToTelemetryManager(baseMode, CUSTOM_MODE, systemStatus);
     }
 
     // Log if new messages
@@ -83,17 +83,17 @@ void SystemManager::smUpdate() {
     smSchedulingCounter = (smSchedulingCounter + 1) % SM_SCHEDULING_RATE_HZ;
 }
 
-void SystemManager::sendRCDataToTelemetryManager(const RCControl &rcData) {
+void SystemManager::sendRCDataToTelemetryManager(const RCControl &rcData) const {
     TMMessage_t rcDataMsg =  rcDataPack(systemUtilsDriver->getCurrentTimestampMs(), rcData.roll, rcData.pitch, rcData.yaw, rcData.throttle, rcData.aux2, rcData.arm);
     tmQueue->push(&rcDataMsg);
 }
 
-void SystemManager::sendHeartbeatDataToTelemetryManager(uint8_t baseMode, uint32_t customMode, MAV_STATE systemStatus) {
-    TMMessage_t hbDataMsg = heartbeatPack(systemUtilsDriver->getCurrentTimestampMs(), baseMode, customMode, systemStatus);
+void SystemManager::sendHeartbeatDataToTelemetryManager(const uint8_t BASE_MODE, uint32_t customMode, const MAV_STATE SYSTEM_STATUS) const {
+    TMMessage_t hbDataMsg = heartbeatPack(systemUtilsDriver->getCurrentTimestampMs(), BASE_MODE, customMode, SYSTEM_STATUS);
     tmQueue->push(&hbDataMsg);
 }
 
-void SystemManager::sendRCDataToAttitudeManager(const RCControl &rcData) {
+void SystemManager::sendRCDataToAttitudeManager(const RCControl &rcData) const {
     RCMotorControlMessage_t rcDataMessage;
 
     rcDataMessage.roll = rcData.roll;
@@ -106,7 +106,7 @@ void SystemManager::sendRCDataToAttitudeManager(const RCControl &rcData) {
     amRCQueue->push(&rcDataMessage);
 }
 
-void SystemManager::sendMessagesToLogger() {
+void SystemManager::sendMessagesToLogger() const {
     static char messages[16][100];
     int msgCount = 0;