Idea outline for reporting TCMux status

hytech-racing · May 19, 2024 · 1284ebc · 1284ebc
1 parent c1710c2
commit 1284ebc
Show file tree

Hide file tree

Showing 5 changed files with 42 additions and 10 deletions.
diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h
@@ -105,4 +105,8 @@ const int SECONDARY_STEERING_SENSE_CENTER = 1960;           // 1970
 const float STEERING_RANGE_DEGREES = 253.0f;                // 256.05f;
 const float STEERING_IIR_ALPHA = 0.7f;  // shaves off around 1 deg of max discrepancy
 
+// TC parameters
+const float SIMPLE_TC_REAR_TORQUE_SCALE  = 0.4;
+const float SIMPLE_TC_REGEN_TORQUE_SCALE = 1.0;
+
 #endif /* __MCU15_H__ */
diff --git a/lib/shared_data/include/TorqueControllersData.h b/lib/shared_data/include/TorqueControllersData.h
@@ -1,5 +1,20 @@
-#ifndef TORQUECONTROLLERSDATA
-#define TORQUECONTROLLERSDATA
+#ifndef __TORQUE_CONTROLLERS_DATA__
+#define __TORQUE_CONTROLLERS_DATA__
+
+#include "DashboardInterface.h"
+#include "TorqueControllers.h"
+
+struct TCMuxStatus_s
+{
+    bool speedPreventsModeChange;
+    bool torqueDeltaPreventsModeChange;
+    bool controllerNotReadyPreventsModeChange;
+    uint8_t modeIntended;
+    uint8_t modeActual;
+    uint8_t dialMode;
+
+
+};
 
 struct PIDTVTorqueControllerData
 {
@@ -11,4 +26,4 @@ struct PIDTVTorqueControllerData
     float rr_torque_delta;
 };
 
-#endif
+#endif  /* __TORQUE_CONTROLLERS_DATA__ */
diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h
@@ -11,6 +11,7 @@
 #include "DashboardInterface.h"
 #include "VectornavInterface.h"
 #include "LoadCellInterface.h"
+#include "TelemetryInterface.h"
 
 const float MAX_SPEED_FOR_MODE_CHANGE = 5.0;        // m/s
 const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm
@@ -60,28 +61,31 @@ class TorqueControllerMux
     TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE;
     bool torqueLimitButtonPressed_ = false;
     unsigned long torqueLimitButtonPressedTime_ = 0;
+    TelemetryInterface *telemHandle_;
 
 public:
     /// @brief torque controller mux in which default instances of all torque controllers are created for use
-    TorqueControllerMux()
+    TorqueControllerMux(TelemetryInterface *telemInterface)
     : torqueControllerNone_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_NO_CONTROLLER)])
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)])
     , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)])
     , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
     , torqueControllerSlipLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SLIP_LAUNCH)])
-    , tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)]) {}
+    , tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)])
+    , telemHandle_(telemInterface) {}
 
 
     /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC
     /// @param simpleTCRearTorqueScale the scaling from 0 to 2 in which 2 is full rear torque allocation, 0 is full front, 1 = balanced
     /// @param simpleTCRegenTorqueScale scaling from 0 to 2 in which 0 is full rear regen and 2 is full front regen, 1 = balanced
-    TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale)
+    TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale, TelemetryInterface *telemInterface)
     : torqueControllerNone_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_NO_CONTROLLER)])
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale)
     , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale)
     , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
     , torqueControllerSlipLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SLIP_LAUNCH)])
-    , tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)]) {}
+    , tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)])
+    , telemHandle_(telemInterface) {}
 // Functions
     void tick(
         const SysTick_s &tick,
@@ -139,6 +143,8 @@ class TorqueControllerMux
             return static_cast<TorqueControllerBase*>(&torqueControllerNone_);
         }
     }
+
+    void updateTCMuxStatus();
 };
 
 #endif /* __TORQUECTRLMUX_H__ */
diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp
@@ -27,8 +27,7 @@ void TorqueControllerMux::tick(
     if (tick.triggers.trigger50)
     {
         // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms
-        if (
-            torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS)
+        if (torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS)
         {
             // WOW C++ is ass
             torqueLimit_ = static_cast<TorqueLimit_e>((static_cast<int>(torqueLimit_) + 1) % (static_cast<int>(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS)));
@@ -96,6 +95,9 @@ void TorqueControllerMux::tick(
         applyPowerLimit(&drivetrainCommand_, &drivetrainData);
         // Uniformly apply speed limit to all controller modes
         applyPosSpeedLimit(&drivetrainCommand_);
+
+        // Update controller status report
+        updateTCMuxStatus();
     }
 }
 
@@ -235,4 +237,9 @@ void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command)
     {
         command->speeds_rpm[i] = std::max(0.0f, command->speeds_rpm[i]);
     }
+}
+
+void TorqueControllerMux::updateTCMuxStatus()
+{
+
 }
diff --git a/src/main.cpp b/src/main.cpp
@@ -150,7 +150,7 @@ SafetySystem safety_system(&ams_interface, &wd_interface);
 PedalsSystem pedals_system(accel_params, brake_params);
 using DriveSys_t = DrivetrainSystem<InvInt_t>;
 DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL);
-TorqueControllerMux torque_controller_mux(1.0, 0.4);
+TorqueControllerMux torque_controller_mux(SIMPLE_TC_REAR_TORQUE_SCALE, SIMPLE_TC_REGEN_TORQUE_SCALE, &telem_interface);
 // TODO ensure that case uses max regen torque, right now its not
 CASEConfiguration case_config = {
     // Following used for generated code