Added steering status report

lib/interfaces/include/TelemetryInterface.h

@@ -112,6 +112,16 @@ class TelemetryInterface
 
     void enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data);
 
+    void update_TCMux_status_CAN_msg(const TCMuxStatus_s &tcMuxStatus);
+
+    void update_steering_status_CAN_msg(
+        const float steering_system_angle,
+        const float filtered_angle_encoder,
+        const float filtered_angle_analog,
+        const uint8_t steering_system_status,
+        const uint8_t steering_encoder_status,
+        const uint8_t steering_analog_status);
+
     /* Enqueue outbound telemetry CAN messages */
     // void enqueue_CAN_mcu_pedal_readings();
     // void enqueue_CAN_mcu_load_cells();

lib/interfaces/src/TelemetryInterface.cpp

@@ -237,6 +237,21 @@ void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerDa
 
 }
 
+void TelemetryInterface::update_TCMux_status_CAN_msg(const TCMuxStatus_s &tcMuxStatus)
+{
+
+}
+
+void TelemetryInterface::update_steering_status_CAN_msg(const float steering_system_angle,
+                                                        const float filtered_angle_encoder,
+                                                        const float filtered_angle_analog,
+                                                        const uint8_t steering_system_status,
+                                                        const uint8_t steering_encoder_status,
+                                                        const uint8_t steering_analog_status)
+{
+
+}
+
 
 /* Tick SysClock */
 void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1,

lib/shared_data/include/TorqueControllersData.h

@@ -1,19 +1,21 @@
 #ifndef __TORQUE_CONTROLLERS_DATA__
 #define __TORQUE_CONTROLLERS_DATA__
 
-#include "DashboardInterface.h"
-#include "TorqueControllers.h"
+#include <stdint.h>
 
 struct TCMuxStatus_s
 {
     bool speedPreventsModeChange;
     bool torqueDeltaPreventsModeChange;
     bool controllerNotReadyPreventsModeChange;
+    bool steeringSystemError;
+
     uint8_t modeIntended;
     uint8_t modeActual;
     uint8_t dialMode;
 
-
+    uint8_t torqueMode;
+    float maxTorque;
 };
 
 struct PIDTVTorqueControllerData

lib/systems/include/SteeringSystem.h

@@ -5,6 +5,8 @@
 #include "AnalogSensorsInterface.h"
 #include "Filter_IIR.h"
 #include "SysClock.h"
+#include "TelemetryInterface.h"
+
 // Digital Encoder = Primary Sensor
 // Analog Encoder = Secondary Sensor
 
@@ -52,17 +54,20 @@ class SteeringSystem
     Filter_IIR<float> steeringFilters_[NUM_SENSORS];
     float filteredAnglePrimary_;
     float filteredAngleSecondary_;
+
+    TelemetryInterface *telemHandle_;
 public:
-    SteeringSystem(SteeringEncoderInterface *primarySensor)
-    : SteeringSystem(primarySensor, DEFAULT_STEERING_ALPHA)
+    SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface)
+    : SteeringSystem(primarySensor, telemInterface, DEFAULT_STEERING_ALPHA)
     {}
 
-    SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlpha)
-    : SteeringSystem(primarySensor, filterAlpha, filterAlpha)
+    SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface, float filterAlpha)
+    : SteeringSystem(primarySensor, telemInterface, filterAlpha, filterAlpha)
     {}
 
-    SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlphaPrimary, float filterAlphaSecondary)
+    SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface, float filterAlphaPrimary, float filterAlphaSecondary)
     : primarySensor_(primarySensor)
+    , telemHandle_(telemInterface)
     {
         steeringFilters_[0] = Filter_IIR<float>(filterAlphaPrimary);
         steeringFilters_[1] = Filter_IIR<float>(filterAlphaSecondary);
@@ -80,6 +85,14 @@ class SteeringSystem
     {
         return steeringData_;
     }
+
+    void reportSteeringStatus(
+        const float angle,
+        const float filteredAngleEncoder,
+        const float filteredAngleAnalog,
+        const uint8_t systemStatus,
+        const uint8_t encoderStatus,
+        const uint8_t analogSensorStatus);
 };
 
 #endif /* __STEERINGSYSTEM_H__ */

lib/systems/include/TorqueControllerMux.h

@@ -43,7 +43,7 @@ class TorqueControllerMux
     };
 
     TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER;
-    DialMode_e cur_dial_mode_ = DialMode_e::MODE_0;
+    DialMode_e currDialMode_ = DialMode_e::MODE_0;
 
     TorqueControllerOutput_s controllerOutputs_[static_cast<int>(TorqueController_e::TC_NUM_CONTROLLERS)];
 
@@ -57,6 +57,8 @@ class TorqueControllerMux
         static_cast<TorqueControllerBase*>(&tcCASEWrapper_)
     };
 
+    TCMuxStatus_s tcMuxStatus_;
+
     DrivetrainCommand_s drivetrainCommand_;
     TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE;
     bool torqueLimitButtonPressed_ = false;
@@ -103,7 +105,7 @@ class TorqueControllerMux
         return drivetrainCommand_;
     };
 
-    const TorqueLimit_e &getTorqueLimit()
+    const TorqueLimit_e getTorqueLimit()
     {
         return torqueLimit_;
     };
@@ -125,7 +127,7 @@ class TorqueControllerMux
 
     const DialMode_e getDialMode()
     {
-        return cur_dial_mode_;
+        return currDialMode_;
     }
 
     const TorqueController_e getDriveMode()
@@ -144,7 +146,7 @@ class TorqueControllerMux
         }
     }
 
-    void updateTCMuxStatus();
+    void reportTCMuxStatus();
 };
 
 #endif /* __TORQUECTRLMUX_H__ */

lib/systems/include/TorqueControllers.h

@@ -4,7 +4,6 @@
 #include <Utility.h>
 #include <DrivetrainSystem.h>
 #include <PedalsSystem.h>
-#include <SteeringSystem.h>
 #include "AnalogSensorsInterface.h"
 #include "DashboardInterface.h"
 #include "PhysicalParameters.h"

lib/systems/src/SteeringSystem.cpp

@@ -61,11 +61,36 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
                 .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR
             };
         }
+
+    }
 
-        // For possible bottom sensor recalibration
-        // TODO: Remove me once done!
-        // Serial.print("Secondary sensor raw: ");
-        // Serial.println(intake.secondaryConversion.raw);
-        // Serial.println();
+    // Report at 50Hz
+    if (intake.tick.triggers.trigger50)
+    {
+        reportSteeringStatus(
+            steeringData_.angle,
+            filteredAnglePrimary_,
+            filteredAngleSecondary_,
+            static_cast<uint8_t>(steeringData_.status),
+            static_cast<uint8_t>(primaryConversion_.status),
+            static_cast<uint8_t>(intake.secondaryConversion.status));
     }
+
+
+}
+
+void SteeringSystem::reportSteeringStatus(const float angle,
+                                          const float filteredAngleEncoder,
+                                          const float filteredAngleAnalog,
+                                          const uint8_t systemStatus,
+                                          const uint8_t encoderStatus,
+                                          const uint8_t analogSensorStatus)
+{
+    telemHandle_->update_steering_status_CAN_msg(
+        angle,
+        filteredAngleEncoder,
+        filteredAngleAnalog,
+        systemStatus,
+        encoderStatus,
+        analogSensorStatus);
 }

lib/systems/src/TorqueControllerMux.cpp

@@ -73,8 +73,13 @@ void TorqueControllerMux::tick(
             // if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange))
             // {
             muxMode_ = dialModeMap_[dashboardDialMode];
-            cur_dial_mode_ = dashboardDialMode;
+            currDialMode_ = dashboardDialMode;
             // }
+
+            // Update TCMux status
+            tcMuxStatus_.speedPreventsModeChange = speedPreventsModeChange;
+            tcMuxStatus_.torqueDeltaPreventsModeChange = torqueDeltaPreventsModeChange;
+            tcMuxStatus_.controllerNotReadyPreventsModeChange = controllerNotReadyPreventsModeChange;
         }
 
         // Check if the current controller is ready. If it has faulted, revert to safe mode
@@ -85,8 +90,18 @@ void TorqueControllerMux::tick(
             muxMode_ = TorqueController_e::TC_SAFE_MODE;
         }
 
+        // Update TCMux status
+        tcMuxStatus_.steeringSystemError = steeringData.status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR;
+        tcMuxStatus_.modeIntended = static_cast<uint8_t>(dialModeMap_[dashboardDialMode]);
+        tcMuxStatus_.modeActual = static_cast<uint8_t>(getDriveMode());
+        tcMuxStatus_.dialMode = static_cast<uint8_t>(getDialMode());
+
         drivetrainCommand_ = controllerOutputs_[static_cast<int>(muxMode_)].command;
 
+        // Update TCMux status
+        tcMuxStatus_.torqueMode = static_cast<uint8_t>(getTorqueLimit());
+        tcMuxStatus_.maxTorque = getMaxTorque();
+
         // Apply setpoints value limits
         // Safety checks for CASE: CASE handles regen, torque, and power limit internally
         applyRegenLimit(&drivetrainCommand_, &drivetrainData);
@@ -97,7 +112,7 @@ void TorqueControllerMux::tick(
         applyPosSpeedLimit(&drivetrainCommand_);
 
         // Update controller status report
-        updateTCMuxStatus();
+        reportTCMuxStatus();
     }
 }
 
@@ -239,7 +254,7 @@ void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command)
     }
 }
 
-void TorqueControllerMux::updateTCMuxStatus()
+void TorqueControllerMux::reportTCMuxStatus()
 {
-
+    telemHandle_->update_TCMux_status_CAN_msg(tcMuxStatus_);
 }

src/main.cpp

@@ -142,7 +142,7 @@ struct inverters
 // */
 
 SysClock sys_clock;
-SteeringSystem steering_system(&steering1, STEERING_IIR_ALPHA);
+SteeringSystem steering_system(&steering1, &telem_interface, STEERING_IIR_ALPHA);
 BuzzerController buzzer(BUZZER_ON_INTERVAL);
 
 SafetySystem safety_system(&ams_interface, &wd_interface);