Feature/drivebrain can integrity (#64)

* break out latencies

* mid testing

* tested on the car

* created a seperate task for this

* pluh

* define shared interfaces version

* updated firmware interface

* saving

* passes unit tests

* fix linter issues

* fix shared_firmware_types

* pluh

Author: KrishKittur

include/VCR_Constants.h

@@ -111,6 +111,8 @@ constexpr unsigned long ams_update_period_us = 5000;                 // 5 000 us
 constexpr unsigned long ams_priority = 2;
 constexpr unsigned long suspension_can_period_us = 4000;             // 4 000 us = 250 Hz
 constexpr unsigned long suspension_priority = 4;
+constexpr unsigned long controls_can_period_us = 4000;             // 4 000 us = 250 Hz
+constexpr unsigned long controls_priority = 8;
 constexpr unsigned long ethernet_update_period = 100000;             // 100 000 us = 10 Hz
 constexpr unsigned long ethernet_send_priority = 6;
 constexpr unsigned long inv_send_period = 5000;                      // 5 000 us = 200 Hz

include/VCR_InterfaceTasks.h

@@ -55,6 +55,11 @@ HT_TASK::TaskResponse read_ioexpander(const unsigned long& sysMicros, const HT_T
  */
 HT_TASK::TaskResponse enqueue_suspension_CAN_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); // NOLINT (capitalized CAN)
 
+/**
+ * Handles sending controls info for drivebrain (latencies and stuff)
+ */
+HT_TASK::TaskResponse enqueue_controls_CAN_data(const unsigned long& sysMicro, const HT_TASK::TaskInfo& taskInfo);
+
 /**
  * Handles sending of coolant temperature data
 */

include/controls.h

@@ -48,6 +48,9 @@ class VCRControls
 
         bool drivebrain_is_in_control();
         bool drivebrain_timing_failure();
+
+        void send_controls_can_messages(); 
+        
     private:
         TorqueControllerSimple _mode0; // this needs to be first for tc_mux to have a valid capture
         LoadCellVectoringTorqueController _mode1;

lib/interfaces/include/DrivebrainInterface.h

@@ -24,15 +24,22 @@ class DrivebrainInterface {
                         const ThermistorData_s &flowmeter_data,
                         IPAddress drivebrain_ip,
                         uint16_t vcr_data_port, qindesign::network::EthernetUDP *udp_socket);
-    void receive_drivebrain_speed_command(const CAN_message_t &msg, unsigned long curr_millis);
-    void receive_drivebrain_torque_lim_command(const CAN_message_t &msg, unsigned long curr_millis);
+    void receive_drivebrain_speed_command_telem(const CAN_message_t &msg, unsigned long curr_millis);
+
+    void receive_drivebrain_torque_lim_command_telem(const CAN_message_t &msg, unsigned long curr_millis);
+
+    void receive_drivebrain_speed_command_auxillary(const CAN_message_t &msg, unsigned long curr_millis); 
+
+    void receive_drivebrain_torque_lim_command_auxillary(const CAN_message_t &msg, unsigned long curr_millis);
 
     void handle_enqueue_suspension_CAN_data();
 
     void handle_enqueue_coolant_temp_CAN_data();
 
     void handle_send_ethernet_data(const hytech_msgs_VCRData_s &data);
-    StampedDrivetrainCommand_s get_latest_data();
+
+    StampedDrivetrainCommand_s get_latest_telem_drivebrain_command();
+    StampedDrivetrainCommand_s get_latest_auxillary_drivebrain_command(); 
 
   private:
     struct {
@@ -49,7 +56,9 @@ class DrivebrainInterface {
     IPAddress _drivebrain_ip;
     uint16_t _vcr_data_port;
     qindesign::network::EthernetUDP *_udp_socket;
-    StampedDrivetrainCommand_s _latest_drivebrain_command = {};
+
+    StampedDrivetrainCommand_s _latest_drivebrain_command_telem = {};
+    StampedDrivetrainCommand_s _latest_drivebrain_command_auxillary = {};
 };
 
 using DrivebrainInterfaceInstance = etl::singleton<DrivebrainInterface>;

lib/interfaces/include/VCRCANInterfaceImpl.h

@@ -66,26 +66,27 @@ using CANInterfacesInstance = etl::singleton<CANInterfaces>;
 
 namespace VCRCANInterfaceImpl {
 
-    extern CANRXBufferType CAN1_rxBuffer;
+    extern CANRXBufferType auxillary_can_rx_buffer;
     extern CANRXBufferType inverter_can_rx_buffer;
     extern CANRXBufferType telem_can_rx_buffer;
 
     /* TX buffer for CAN1 */
-    extern CANTXBufferType CAN1_txBuffer;
+    extern CANTXBufferType auxillary_can_tx_buffer;
     /* TX buffer for CAN2 */
     extern CANTXBufferType inverter_can_tx_buffer;
     /* TX buffer for CAN3 */
     extern CANTXBufferType telem_can_tx_buffer;
 
+    extern FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> AUXILLARY_CAN;
     extern FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_16> TELEM_CAN; // gets defined in main as of right now
     extern FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> INVERTER_CAN; // gets defined in main as of right now
     //extern FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> CAN_1; 
 
 
-    void on_can1_receive(const CAN_message_t &msg);
+    void on_auxillary_can_receive(const CAN_message_t &msg);
     void on_inverter_can_receive(const CAN_message_t &msg);
     void on_telem_can_receive(const CAN_message_t &msg);
-    void vcr_CAN_recv(CANInterfaces &interfaces, const CAN_message_t &msg, unsigned long millis);
+    void vcr_CAN_recv(CANInterfaces &interfaces, const CAN_message_t &msg, unsigned long millis, CANInterfaceType_e interface_type);
 
     void send_all_CAN_msgs(CANTXBufferType &buffer, FlexCAN_T4_Base *can_interface);
 

lib/interfaces/src/DrivebrainInterface.cpp

@@ -10,6 +10,7 @@
 #include "hytech_msgs.pb.h"
 #include <cstdint>
 
+
 DrivebrainInterface::DrivebrainInterface(const RearLoadCellData_s &rear_load_cell_data,
                                          const RearSusPotData_s &rear_suspot_data,
                                          const ThermistorData_s &coolant_temperature_data_0,
@@ -27,36 +28,78 @@ DrivebrainInterface::DrivebrainInterface(const RearLoadCellData_s &rear_load_cel
       _vcr_data_port(vcr_data_port),
       _udp_socket(udp_socket) { };
 
-StampedDrivetrainCommand_s DrivebrainInterface::get_latest_data() {
-    return _latest_drivebrain_command;
+StampedDrivetrainCommand_s DrivebrainInterface::get_latest_telem_drivebrain_command() {
+    return _latest_drivebrain_command_telem;
+}
+
+StampedDrivetrainCommand_s DrivebrainInterface::get_latest_auxillary_drivebrain_command() {
+    return _latest_drivebrain_command_auxillary;
+}
+
+
+void DrivebrainInterface::receive_drivebrain_speed_command_telem(const CAN_message_t &msg,
+                                                           unsigned long curr_millis) {
+    DRIVEBRAIN_SPEED_SET_INPUT_t drivebrain_msg;
+
+    Unpack_DRIVEBRAIN_SPEED_SET_INPUT_hytech(&drivebrain_msg, &msg.buf[0], msg.len);
+
+    _latest_drivebrain_command_telem.desired_speeds.recvd = true;
+    _latest_drivebrain_command_telem.desired_speeds.last_recv_millis = curr_millis;
+
+    _latest_drivebrain_command_telem.desired_speeds.veh_vec_data = {
+        static_cast<float>(drivebrain_msg.drivebrain_set_rpm_fl),
+        static_cast<float>(drivebrain_msg.drivebrain_set_rpm_fr),
+        static_cast<float>(drivebrain_msg.drivebrain_set_rpm_rl),
+        static_cast<float>(drivebrain_msg.drivebrain_set_rpm_rr)};
+};
+
+void DrivebrainInterface::receive_drivebrain_torque_lim_command_telem(const CAN_message_t &msg,
+                                                                unsigned long curr_millis) {
+    DRIVEBRAIN_TORQUE_LIM_INPUT_t drivebrain_msg;
+
+    Unpack_DRIVEBRAIN_TORQUE_LIM_INPUT_hytech(&drivebrain_msg, &msg.buf[0], msg.len);
+
+    _latest_drivebrain_command_telem.torque_limits.recvd = true;
+    _latest_drivebrain_command_telem.torque_limits.last_recv_millis = curr_millis;
+
+    _latest_drivebrain_command_telem.torque_limits.veh_vec_data = {
+        static_cast<float>(
+            HYTECH_drivebrain_torque_fl_ro_fromS(drivebrain_msg.drivebrain_torque_fl_ro)),
+        static_cast<float>(
+            HYTECH_drivebrain_torque_fr_ro_fromS(drivebrain_msg.drivebrain_torque_fr_ro)),
+        static_cast<float>(
+            HYTECH_drivebrain_torque_rl_ro_fromS(drivebrain_msg.drivebrain_torque_rl_ro)),
+        static_cast<float>(
+            HYTECH_drivebrain_torque_rr_ro_fromS(drivebrain_msg.drivebrain_torque_rr_ro))};
 }
 
-void DrivebrainInterface::receive_drivebrain_speed_command(const CAN_message_t &msg,
+
+void DrivebrainInterface::receive_drivebrain_speed_command_auxillary(const CAN_message_t &msg,
                                                            unsigned long curr_millis) {
     DRIVEBRAIN_SPEED_SET_INPUT_t drivebrain_msg;
 
     Unpack_DRIVEBRAIN_SPEED_SET_INPUT_hytech(&drivebrain_msg, &msg.buf[0], msg.len);
 
-    _latest_drivebrain_command.desired_speeds.recvd = true;
-    _latest_drivebrain_command.desired_speeds.last_recv_millis = curr_millis;
+    _latest_drivebrain_command_auxillary.desired_speeds.recvd = true;
+    _latest_drivebrain_command_auxillary.desired_speeds.last_recv_millis = curr_millis;
 
-    _latest_drivebrain_command.desired_speeds.veh_vec_data = {
+    _latest_drivebrain_command_auxillary.desired_speeds.veh_vec_data = {
         static_cast<float>(drivebrain_msg.drivebrain_set_rpm_fl),
         static_cast<float>(drivebrain_msg.drivebrain_set_rpm_fr),
         static_cast<float>(drivebrain_msg.drivebrain_set_rpm_rl),
         static_cast<float>(drivebrain_msg.drivebrain_set_rpm_rr)};
 };
 
-void DrivebrainInterface::receive_drivebrain_torque_lim_command(const CAN_message_t &msg,
+void DrivebrainInterface::receive_drivebrain_torque_lim_command_auxillary(const CAN_message_t &msg,
                                                                 unsigned long curr_millis) {
     DRIVEBRAIN_TORQUE_LIM_INPUT_t drivebrain_msg;
 
     Unpack_DRIVEBRAIN_TORQUE_LIM_INPUT_hytech(&drivebrain_msg, &msg.buf[0], msg.len);
 
-    _latest_drivebrain_command.torque_limits.recvd = true;
-    _latest_drivebrain_command.torque_limits.last_recv_millis = curr_millis;
+    _latest_drivebrain_command_auxillary.torque_limits.recvd = true;
+    _latest_drivebrain_command_auxillary.torque_limits.last_recv_millis = curr_millis;
 
-    _latest_drivebrain_command.torque_limits.veh_vec_data = {
+    _latest_drivebrain_command_auxillary.torque_limits.veh_vec_data = {
         static_cast<float>(
             HYTECH_drivebrain_torque_fl_ro_fromS(drivebrain_msg.drivebrain_torque_fl_ro)),
         static_cast<float>(
@@ -69,11 +112,12 @@ void DrivebrainInterface::receive_drivebrain_torque_lim_command(const CAN_messag
 
 void DrivebrainInterface::handle_enqueue_suspension_CAN_data() {
     REAR_SUSPENSION_t rear_sus_msg;
+    
     rear_sus_msg.rl_load_cell = _suspension_data.rear_load_cell_data.RL_loadcell_analog;
     rear_sus_msg.rr_load_cell = _suspension_data.rear_load_cell_data.RR_loadcell_analog;
     rear_sus_msg.rl_shock_pot = _suspension_data.rear_suspot_data.RL_sus_pot_analog;
     rear_sus_msg.rr_shock_pot = _suspension_data.rear_suspot_data.RR_sus_pot_analog;
-
+    
     CAN_util::enqueue_msg(&rear_sus_msg, &Pack_REAR_SUSPENSION_hytech,
                           VCRCANInterfaceImpl::telem_can_tx_buffer); 
 }

lib/interfaces/src/VCRCANInterfaceImpl.cpp

@@ -6,20 +6,20 @@
 namespace VCRCANInterfaceImpl {
 
 // global forwards
-CANRXBufferType CAN1_rxBuffer; // can1 = aux can
+CANRXBufferType auxillary_can_rx_buffer;
 CANRXBufferType inverter_can_rx_buffer;
 CANRXBufferType telem_can_rx_buffer;
 
-CANTXBufferType CAN1_txBuffer;
+CANTXBufferType auxillary_can_tx_buffer;
 CANTXBufferType inverter_can_tx_buffer;
 CANTXBufferType telem_can_tx_buffer;
 
 
 
-void on_can1_receive(const CAN_message_t &msg) {
+void on_auxillary_can_receive(const CAN_message_t &msg) {
     uint8_t buf[sizeof(CAN_message_t)];
     memmove(buf, &msg, sizeof(msg));
-    CAN1_rxBuffer.push_back(buf, sizeof(CAN_message_t));
+    auxillary_can_rx_buffer.push_back(buf, sizeof(CAN_message_t));
 }
 
 void on_inverter_can_receive(const CAN_message_t &msg) {
@@ -35,7 +35,7 @@ void on_telem_can_receive(const CAN_message_t &msg) {
     telem_can_rx_buffer.push_back(buf, sizeof(CAN_message_t));
 }
 
-void vcr_CAN_recv(CANInterfaces &interfaces, const CAN_message_t &msg, unsigned long millis) {
+void vcr_CAN_recv(CANInterfaces &interfaces, const CAN_message_t &msg, unsigned long millis, CANInterfaceType_e interface_type) {
     switch (msg.id) {
 
     case PEDALS_SYSTEM_DATA_CANID: 
@@ -60,11 +60,20 @@ void vcr_CAN_recv(CANInterfaces &interfaces, const CAN_message_t &msg, unsigned
     }
     case DRIVEBRAIN_TORQUE_LIM_INPUT_CANID: 
     {
-        interfaces.db_interface.receive_drivebrain_torque_lim_command(msg, millis);
+        if (interface_type == CANInterfaceType_e::AUX) {
+            interfaces.db_interface.receive_drivebrain_torque_lim_command_auxillary(msg, millis);
+        } else if (interface_type == CANInterfaceType_e::TELEM) {
+            interfaces.db_interface.receive_drivebrain_torque_lim_command_telem(msg, millis);
+        }
         break;
     }
     case DRIVEBRAIN_SPEED_SET_INPUT_CANID: {
-        interfaces.db_interface.receive_drivebrain_speed_command(msg, millis);
+        if (interface_type == CANInterfaceType_e::AUX) {
+            interfaces.db_interface.receive_drivebrain_speed_command_auxillary(msg, millis);
+        } else if (interface_type == CANInterfaceType_e::TELEM) {
+            interfaces.db_interface.receive_drivebrain_speed_command_telem(msg, millis);
+        }
+         interfaces.db_interface.receive_drivebrain_speed_command_telem(msg, millis);
         break;
     }
 

lib/systems/include/DrivetrainSystem.h

@@ -14,18 +14,7 @@
 #include <shared_types.h>
 #include "SystemTimeInterface.h"
 
-// requirements:
-// - [x] must support ability to initialize the drivetrain 
-// - [x] ability to command inverters individually and be able to return a failure status when attempting to send invalid command for a certain state
-// - [x] contain a state machine for managing the state of the drivetrain as a whole (aka: all inverters have the same state)
-//  - [x] initialization states included
-//  - [ ] different control mode states
-// - [x] single point of interaction / control of the drivetrain that can receive "commands"
-//      (at least for now, need to see how this works out once we start using it)
-// - [x] be decoupled from the inverter class
-//      std::function / etl::delegate registered functions for the inverter interface. mostly for ease of testing.
-// - [x] be able to reset drivetrain
-    // - [ ] 
+
 
 /**
  * When user calls evaluate_drivetrain(), this is part of the returned status to