WIP: added in drivebrain system. need new time paradigm for further development

Author: RCMast3r

lib/interfaces/README.md

@@ -128,7 +128,7 @@ message 2346 'Converter temperature error' after the warning time1) (ID32943) ha
         - inverter's internal value of (TD) kd
         - AKA SERCOS parameter ID102
 
-## VCF Interfaces
+## VCF Interface
 
 The VCR is connected over CAN and Ethernet to the VCF. We will use the CAN communication for latency-sensitive communication such as the driver input and controller input sensor signals. The Ethernet link will be used for the non-timing-sensitive data.
 
@@ -183,3 +183,43 @@ VCF Outputs:
             - means that the firmware was flashed while there was changes made that had not been commited
         - `char git_short_hash[8]`
             - the git hash of the commit that was flashed to the 
+
+## Drivebrain Interface
+
+### CAN interface
+
+#### preface
+__NOTE__: the following messages that are on the bus are listened to by the drivebrain and are output from other boards (not VCR)
+- pedal data CAN packet (VCF)
+- inverter data (FL, FR, RL, RR inverters) -> these will be forwarded messages being forwarded by VCR onto the telem CAN line
+
+VCR Outputs:
+
+- VCR suspension data CAN packet:
+    - `uint16_t rl_load_cell`
+    - `uint16_t rr_load_cell`
+    - `uint16_t rl_shock_pot`
+    - `uint16_t rr_shock_pot`
+
+- VCR status CAN packet:
+    - vehicle state (oneof: RTD, tractive system enabled, etc.)
+    - control mode
+    - VCR 
+    - VCR error state word 
+        - drivebrain timeout, VCF timeout, VCR firmware error, etc.
+
+VCR inputs (sent by drivebrain):
+- desired RPMs
+    - `int16_t fl_rpm`
+    - `int16_t fr_rpm`
+    - `int16_t rl_rpm`
+    - `int16_t rr_rpm`
+
+- torque limits
+    - __note__: in units of .01nm: 2100 = 21nm, 2150 = 21.5nm
+    - `int16_t fl_torque_lim` 
+    - `int16_t fr_torque_lim`
+    - `int16_t rl_torque_lim`
+    - `int16_t rl_torque_lim`
+
+

lib/systems/README.md

@@ -101,4 +101,26 @@ stateDiagram-v2
     torq_mode --> err: incorrect user command type OR any inverter error
     err --> clear_err: on user request of error reset 
     clear_err --> not_en_hv: on successful reset of errors (either internally to the drivetrain system or of the inverters themselves)
-```
+```
+
+## drivebrain controller system
+
+Drivebrain Controller for fail-safe pass-through control of the car
+  
+this class is the "controller" that allows for pass-through control as commanded by the drivebrain. It also calculates the latency of the most recent input and checks to see if the most recent input is still valid and has not expired. If the input has expired then it switches over to the fail-safe control mode (MODE0) to allow for safe failing even while the car is driving so that we dont lose hard-braking capabilities.
+
+The controller can clear it's own fault by switching off of this operating mode and then swapping back to this operating mode. The fault clears the first time this controller gets evaluated while switch from the swapped-to mode back to this pass through mode. 
+    - all controllers get evaluated once when being evaluated as a mode to switch to. during this evaluation is when the fault clears.
+
+### latency measurement:
+- the latency is measured by the difference in times in received messages from drivebrain over CAN. if the difference is too great, we swap to MODE0 control. it is assumed that the transmission delay is negligible
+
+### config and I/O
+config: 
+    - maximum allowed latency 
+    - assigned controller mode (currently defaults to MODE4)
+inputs:
+    - the current car state from which it gets the latest drivebrain input, current controller mode and the stamped drivebrain message data
+outputs:
+    - drivetrain command either from drivebrain or mode0 depending on if an error is present
+    - getter for the internal state of the drivebrain controller (error present, etc.)

lib/systems/include/TorqueControllerMux.hpp

@@ -79,7 +79,7 @@ class TorqueControllerMux
     /// @param max_power_limit the max power limit defaults to TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT
     /// @param num_motors the number of motors. defaults to 4.
     /// @note TC Mux must be created with at least 1 controller.
-    explicit TorqueControllerMux(std::array<std::function<DrivetrainCommand_s(const VCRData_s &state)>, num_controllers> controller_evals,
+    explicit TorqueControllerMux(std::array<std::function<DrivetrainCommand_s(const VCRData_s &state, unsigned long eval_millis)>, num_controllers> controller_evals,
         std::array<bool, num_controllers> mux_bypass_limits,
         float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE,
         float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE,
@@ -106,7 +106,7 @@ class TorqueControllerMux
 
 private:
 
-    std::array<std::function<DrivetrainCommand_s(const VCRData_s &state)>, num_controllers> _controller_evals;
+    std::array<std::function<DrivetrainCommand_s(const VCRData_s &state, unsigned long eval_millis)>, num_controllers> _controller_evals;
 
     std::array<bool, num_controllers> _mux_bypass_limits;
 

lib/systems/include/controllers/DrivebrainController.h

@@ -0,0 +1,86 @@
+#ifndef __DRIVEBRAINCONTROLLER_H__
+#define __DRIVEBRAINCONTROLLER_H__
+
+#include "controllers/SimpleController.h"
+#include "SharedFirmwareTypes.h"
+#include <cmath>
+
+// TODO - [ ] need to validate that the times that are apparent in the drivebrain data
+//            and ensure that they are within tolerence to current sys-tick
+
+// TODO - [ ] if the drivebrain controller is currently the active controller,
+//            the latency fault should be latching
+
+//              meaning that if we fail any of the latency checks while active we cannot clear the timing failure fault
+//              unless we switch to another controller and back again. in reality, the CAN line or ethernet conditions
+//              probably wont improve randomly during runtime so it will keep faulting. primarily this is just to make sure
+//              we dont keep going from latent to not latent while driving and thus the driver gets jittered and the
+//              drivetrain will keep "powering up" and "powering down"
+
+// TODO - [ ] correctly measure latency between drivebrain and VCR:
+//
+
+/*! \brief Drivebrain Controller for fail-safe pass-through control of the car
+           RESPONSIBILITIES AND DOCUMENTATION
+  
+    this class is the "controller" that allows for pass-through control as commanded by the drivebrain. It also calculates the 
+    latency of the most recent input and checks to see if the most recent input is still valid and has not expired. If the 
+    input has expired then it switches over to the fail-safe control mode (MODE0) to allow for safe failing even while the car
+    is driving so that we dont lose hard-braking capabilities.
+
+    This class can clear it's own fault by switching off of this operating mode and then swapping back to this operating mode. 
+    The fault clears the first time this controller gets evaluated while switch from the swapped-to mode back to this pass 
+    through mode. 
+
+    latency measurement:
+        - the latency is measured by the difference in times in received messages from drivebrain over CAN. if the difference 
+          is too great, we swap to MODE0 control. it is assumed that the transmission delay is negligible
+
+    config: 
+        - maximum allowed latency 
+        - assigned controller mode (currently defaults to MODE4)
+    inputs:
+        - the current car state from which it gets the latest drivebrain input, current controller mode and the stamped drivebrain 
+          message data
+ */
+class DrivebrainController
+{
+public:
+
+    /// @brief constructor for the drivebrain controller class
+    /// @param allowed_latency the allowed latency in milliseconds for which if the most recent packet has a timestamp older than this measure of time we fail safe
+    /// @param assigned_controller_mode the controller mode that the drivebrain controller is assigned to. is required for evaluating whether or not we are active or not
+    DrivebrainController(unsigned long allowed_latency,
+                         ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4)
+        : _emergency_control()
+    {
+        _last_worst_latency_timestamp = 0;
+        _worst_latency_so_far = -1;
+        _params = {allowed_latency, assigned_controller_mode};
+    }
+
+    /// @brief evaluate function for running the business logic
+    /// @param state the current state of the car
+    /// @param eval_millis
+    /// @return torque controller output that gets passed through the TC MUX
+    DrivetrainCommand_s evaluate(const VCRData_s &state, unsigned long eval_millis);
+
+    /// @brief getter for the current status of whether or not the controller has had a timing failure during operation
+    /// @return bool of status
+    bool get_timing_failure_status() { return _timing_failure; }
+
+private:
+    struct
+    {
+        unsigned long allowed_latency = {};
+        ControllerMode_e assigned_controller_mode = {};
+    } _params;
+
+    unsigned long _last_worst_latency_timestamp;
+    int64_t _worst_latency_so_far;
+    bool _timing_failure = false;
+    unsigned long _last_setpoint_millis = -1;
+    TorqueControllerSimple _emergency_control;
+};
+
+#endif // __DRIVEBRAINCONTROLLER_H__

lib/systems/include/controllers/SimpleController.h

@@ -31,11 +31,11 @@ class TorqueControllerSimple
 {
 public:
     /// @brief simple TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance
-    TorqueControllerSimple(TorqueControllerSimpleParams_s params)
+    TorqueControllerSimple(TorqueControllerSimpleParams_s params = TorqueControllerSimpleParams_s())
         : _params(params)
     { }
     /// @brief calculates torque output based off max torque and simple torque scaling
-    DrivetrainCommand_s evaluate(const VCRData_s &state);
+    DrivetrainCommand_s evaluate(const VCRData_s &state, unsigned long eval_millis);
 
 private:
     TorqueControllerSimpleParams_s _params;

lib/systems/src/controllers/DrivebrainController.cpp

@@ -0,0 +1,58 @@
+#include "controllers/DrivebrainController.h"
+#include "SharedFirmwareTypes.h"
+#include <cstdint>
+
+DrivetrainCommand_s DrivebrainController::evaluate(const VCRData_s &state, unsigned long eval_millis)
+{
+
+
+    
+    auto db_input = state.interface_data.latest_drivebrain_command;
+    
+    // cases for timing_failure:
+    // 1 we have not received any messages from the db (timestamped message recvd flag initialized as false in struct def)
+    bool no_messages_received = (!db_input.recvd);
+    
+    
+    // 2 if the time between the current VCR eval_millis time and the last millis time that we recvd a drivebrain msg is too high
+    bool message_too_latent = (::abs((int)(static_cast<int64_t>(eval_millis) - static_cast<int64_t>(db_input.last_recv_millis))) > (int)_params.allowed_latency);
+    constexpr int64_t debug_timestamp_period_ms = 5000;
+    if((static_cast<int64_t>(eval_millis) - static_cast<int64_t>(_last_worst_latency_timestamp)) > debug_timestamp_period_ms)
+    {    
+        _last_worst_latency_timestamp = eval_millis;
+        _worst_latency_so_far = -1;
+    }
+
+    if( (eval_millis - db_input.last_recv_millis) > _worst_latency_so_far)
+    {
+        _worst_latency_so_far = (eval_millis - db_input.last_recv_millis);   
+    }
+    
+
+    bool timing_failure = (message_too_latent || no_messages_received);
+
+    // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault
+    
+    bool is_active_controller = state.system_data.tc_mux_status.active_controller_mode == _params.assigned_controller_mode;
+
+    if ((!is_active_controller) && (!timing_failure))
+    {
+        // timing failure should be false here
+        _timing_failure = false;
+    }
+
+    DrivetrainCommand_s output;
+    if (!timing_failure && (!_timing_failure))
+    {
+        _last_setpoint_millis = db_input.last_recv_millis;
+        
+        output = db_input.cmd_data;
+    }
+    else
+    {
+        _timing_failure = true;
+        // output.command = {{0.0f}, {0.0f}};
+        output = _emergency_control.evaluate(state, eval_millis);
+    }
+    return output;
+}

lib/systems/src/controllers/SimpleController.cpp

@@ -1,6 +1,6 @@
 #include "controllers/SimpleController.h"
 
-DrivetrainCommand_s TorqueControllerSimple::evaluate(const VCRData_s &state)
+DrivetrainCommand_s TorqueControllerSimple::evaluate(const VCRData_s &state, unsigned long eval_millis)
 {
     // Both pedals are not pressed and no implausibility has been detected
     // accelRequest goes between 1.0 and -1.0

platformio.ini

@@ -8,7 +8,7 @@
 
 lib_deps_shared =
   https://github.com/hytech-racing/shared_firmware_systems.git#af96a63
-  https://github.com/hytech-racing/shared_firmware_types.git
+  https://github.com/hytech-racing/shared_firmware_types.git#feature/db_controls_and_interface
   ; ../../shared_firmware_types
   https://github.com/ssilverman/QNEthernet#v0.26.0
   https://github.com/hytech-racing/HT_SCHED

test/test_systems/test_tcmux.h

@@ -187,7 +187,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation)
 //                                                   static_cast<Controller *>(&simple_launch),
 //                                                   static_cast<Controller *>(&slip_launch)},
 //                                                  {false, true, false, false, false});
-    TorqueControllerMux<1> torque_controller_mux({std::bind(&TorqueControllerSimple::evaluate, std::ref(tc_simple), std::placeholders::_1)}, {false});
+    TorqueControllerMux<1> torque_controller_mux({std::bind(&TorqueControllerSimple::evaluate, std::ref(tc_simple), std::placeholders::_1, std::placeholders::_2)}, {false});
 
 
     VCRData_s mode_0_input_state;