Feature/drivebrain controller test

include/MCU_rev15_defs.h

@@ -54,17 +54,17 @@ const unsigned long TELEM_CAN_BAUDRATE  = 500000;
 // All of these values are the PEDAL min/max
 // the sensor min/max that trip implaus are calculated
 // in the PedalsSystem constructor
-const int ACCEL1_PEDAL_MAX = 3409;  // 3330
-const int ACCEL2_PEDAL_MAX = 259;   // 388
+const int ACCEL1_PEDAL_MAX = 3491;  // 3330
+const int ACCEL2_PEDAL_MAX = 189;   // 388
 
-const int ACCEL1_PEDAL_MIN = 2129;
-const int ACCEL2_PEDAL_MIN = 1517;
+const int ACCEL1_PEDAL_MIN = 2244;
+const int ACCEL2_PEDAL_MIN = 1405;
 
 const int BRAKE1_PEDAL_MAX = 1945;  // 2200;
 const int BRAKE2_PEDAL_MAX = 1742;  // 2200;
 
-const int BRAKE1_PEDAL_MIN = 1230;  // 785;    // 1230 to 1750
-const int BRAKE2_PEDAL_MIN = 2437;  // 785;    // 2450 to 1930
+const int BRAKE1_PEDAL_MIN = 1192;  // 785;    // 1230 to 1750
+const int BRAKE2_PEDAL_MIN = 2476;  // 785;    // 2450 to 1930
 
 const int ACCEL1_PEDAL_OOR_MIN = 90;
 const int ACCEL2_PEDAL_OOR_MIN = 90;

lib/interfaces/include/DrivebrainInterface.h

@@ -0,0 +1,31 @@
+#ifndef __DRIVEBRAININTERFACE_H__
+#define __DRIVEBRAININTERFACE_H__
+#include "DrivebrainData.h"
+#include "MessageQueueDefine.h"
+
+class DrivebrainInterface
+{
+    public:
+        DrivebrainInterface(CANBufferType * msg_output_queue)
+        {
+
+            _msg_queue = msg_output_queue;
+            _current_drivebrain_data.last_speed_setpoint_receive_time_millis= -1;
+            _current_drivebrain_data.last_torque_lim_receive_time_millis= -1;
+            _current_drivebrain_data.speed_setpoints_rpm = {};
+            _current_drivebrain_data.torque_limits_nm = {};
+        }
+
+        void receive_db_torque_lim_message(CAN_message_t &msg, unsigned long curr_millis);
+        void receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis);
+
+        DrivebrainData get_latest_db_data() {return _current_drivebrain_data; }
+
+    private:
+
+        DrivebrainData _current_drivebrain_data;
+        CANBufferType * _msg_queue;
+
+};
+
+#endif // __DRIVEBRAININTERFACE_H__

lib/interfaces/include/HytechCANInterface.h

@@ -14,6 +14,7 @@
 #include "VectornavInterface.h"
 #include "HT08_CASE_types.h"
 #include "MessageQueueDefine.h"
+#include "DrivebrainInterface.h"
 /*
     struct holding interfaces processed by process_ring_buffer()
     FL = MC1
@@ -32,6 +33,7 @@ struct CANInterfaces
     DashboardInterface *dash_interface;
     AMSInterface *ams_interface;
     SABInterface *sab_interface;
+    DrivebrainInterface *db_interface;
 };
 
 // the goal with the can interface is that there exists a receive call that appends to a circular buffer
@@ -170,15 +172,21 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces,
         case VN_LAT_LON_CANID:
             interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg);
             break;
-        case VN_GPS_TIME_CANID:
-            interfaces.vn_interface->retrieve_gps_time_CAN(recvd_msg);
-            break;
+        // case VN_GPS_TIME_MSG_CANID:
+        //     interfaces.vn_interface->retrieve_gps_time_CAN(recvd_msg);
+        //     break;
         case VN_STATUS_CANID:
             interfaces.vn_interface->retrieve_vn_status_CAN(recvd_msg); // double check this
             break;
         case VN_ANGULAR_RATE_CANID:
             interfaces.vn_interface->receive_ang_rates_CAN(recvd_msg);
             break;
+        case DRIVEBRAIN_TORQUE_LIM_INPUT_CANID:
+            interfaces.db_interface->receive_db_torque_lim_message(recvd_msg, curr_millis);
+            break;
+        case DRIVEBRAIN_SPEED_SET_INPUT_CANID:
+            interfaces.db_interface->receive_db_speed_setpoint_message(recvd_msg, curr_millis);
+            break;
         }
     }
 }

lib/interfaces/include/InverterInterface.h

@@ -120,15 +120,15 @@ class InverterInterface
 
     int16_t get_speed() { return speed_; }
     float get_torque_current() { return torque_current_; }
-    float get_mag_current() { return magnetizing_current_; }
+    int16_t get_mag_current() { return magnetizing_current_; }
     float get_actual_torque() { return actual_torque_nm_; }
     uint16_t get_error_status();
     MC_temps get_temps_msg() { return mc_temps_; }
     MC_setpoints_command get_cmd_msg() { return curr_cmd_; }
 
 private:
     float id110_val_;                            // for scaling to proper iq and id vals
-    float torque_current_, magnetizing_current_; // iq and id in A respectively
+    int16_t torque_current_, magnetizing_current_; // iq and id in A respectively
     float actual_torque_nm_;
     /* write setpoints message to the CAN buffer */
     void write_cmd_msg_to_queue_(MC_setpoints_command msg);

lib/interfaces/include/InverterInterface.tpp

@@ -133,7 +133,8 @@ void InverterInterface<message_queue>::receive_status_msg(CAN_message_t &msg)
     // to get scalar for this.
     // torque_current_ = ((float)mc_status.get_torque_current() * id110_val_) / 16384.0;           // iq
     torque_current_ = mc_status.get_torque_current();
-    magnetizing_current_ = ((float)mc_status.get_magnetizing_current() * id110_val_) / 16384.0; // id
+    magnetizing_current_ = mc_status.get_magnetizing_current(); // id
+
 
     // TODO enable this on the inverters
     // actual torque in Nm is from the signal we can add in from here:

lib/interfaces/include/TelemetryInterface.h

@@ -108,6 +108,12 @@ class TelemetryInterface
         InvInt_t *rl,
         InvInt_t *rr
     );
+    void update_drivetrain_torque_filter_out_telem_CAN_msg(
+        InvInt_t *fl,
+        InvInt_t *fr,
+        InvInt_t *rl,
+        InvInt_t *rr
+    );
     void update_penthouse_accum_CAN_msg(
         const AnalogConversion_s &current,
         const AnalogConversion_s &reference

lib/interfaces/include/VectornavInterface.h

@@ -45,10 +45,10 @@ class VNInterface
     void retrieve_uncompLinear_accel_CAN(CAN_message_t &recvd_msg);
     void retrieve_ypr_CAN(CAN_message_t &recvd_msg);
     void retrieve_lat_lon_CAN(CAN_message_t &recvd_msg);
-    void retrieve_gps_time_CAN(CAN_message_t &recvd_msg);
+    // void retrieve_gps_time_CAN(CAN_message_t &recvd_msg);
     void retrieve_vn_status_CAN(CAN_message_t &recvd_msg);
     void retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recvd_msg);
-    void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg);
+    // void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg);
     void receive_ang_rates_CAN(CAN_message_t &recvd_msg);
     // getters
     vector_nav get_vn_struct();

lib/interfaces/include/VectornavInterface.tpp

@@ -48,12 +48,12 @@ void VNInterface<message_queue>::retrieve_lat_lon_CAN(CAN_message_t &recvd_msg)
     vn_data.longitude = HYTECH_vn_gps_lon_ro_fromS(lat_lon_data.vn_gps_lon_ro);
 }
 
-template<typename message_queue>
-void VNInterface<message_queue>::retrieve_gps_time_CAN(CAN_message_t &recvd_msg) {
-    VN_GPS_TIME_t gps_time_data;
-    Unpack_VN_GPS_TIME_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len);
-    vn_data.gps_time = gps_time_data.vn_gps_time;
-}
+// template<typename message_queue>
+// void VNInterface<message_queue>::retrieve_gps_time_CAN(CAN_message_t &recvd_msg) {
+//     VN_GPS_TIME_MSG_CANID gps_time_data;
+//     Unpack_VN_GPS_TIME_MSG_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len);
+//     vn_data.gps_time = gps_time_data.vn_gps_time;
+// }
 
 template<typename message_queue>
 void VNInterface<message_queue>::retrieve_vn_status_CAN(CAN_message_t &recvd_msg) {
@@ -70,12 +70,12 @@ void VNInterface<message_queue>::retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recv
     vn_data.ecef_coords[1] = HYTECH_vn_ecef_pos_y_ro_fromS(ecef_xy.vn_ecef_pos_y_ro);
 }
 
-template <typename message_queue>
-void VNInterface<message_queue>::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) {
-    VN_ECEF_POS_Z_t ecef_z;
-    Unpack_VN_ECEF_POS_Z_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len);
-    vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro);
-}
+// template <typename message_queue>
+// void VNInterface<message_queue>::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) {
+//     VN_ECEF_POS_Z_t ecef_z;
+//     Unpack_VN_ECEF_POS_Z_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len);
+//     vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro);
+// }
 
 template <typename message_queue>
 void VNInterface<message_queue>::receive_ang_rates_CAN(CAN_message_t & recvd_msg)

lib/interfaces/src/DrivebrainInterface.cpp

@@ -0,0 +1,33 @@
+#include "DrivebrainInterface.h"
+#include "hytech.h"
+
+void DrivebrainInterface::receive_db_torque_lim_message(CAN_message_t& msg, unsigned long curr_millis)
+{
+    DRIVEBRAIN_TORQUE_LIM_INPUT_t db_data;
+    Unpack_DRIVEBRAIN_TORQUE_LIM_INPUT_hytech(&db_data, msg.buf, msg.len);
+    _current_drivebrain_data.last_torque_lim_receive_time_millis = curr_millis;
+
+    auto fl = HYTECH_drivebrain_torque_fl_ro_fromS(db_data.drivebrain_torque_fl_ro);
+    auto fr = HYTECH_drivebrain_torque_fr_ro_fromS(db_data.drivebrain_torque_fr_ro);
+    auto rl = HYTECH_drivebrain_torque_rl_ro_fromS(db_data.drivebrain_torque_rl_ro);
+    auto rr = HYTECH_drivebrain_torque_rr_ro_fromS(db_data.drivebrain_torque_rr_ro);
+
+    _current_drivebrain_data.torque_limits_nm.FL = (float)fl;
+    _current_drivebrain_data.torque_limits_nm.FR = (float)fr;
+    _current_drivebrain_data.torque_limits_nm.RL = (float)rl;
+    _current_drivebrain_data.torque_limits_nm.RR = (float)rr;
+
+}
+
+void DrivebrainInterface::receive_db_speed_setpoint_message(CAN_message_t& msg, unsigned long curr_millis)
+{
+    DRIVEBRAIN_SPEED_SET_INPUT_t db_data;
+    Unpack_DRIVEBRAIN_SPEED_SET_INPUT_hytech(&db_data, msg.buf, msg.len);
+
+    _current_drivebrain_data.last_speed_setpoint_receive_time_millis = curr_millis;
+    _current_drivebrain_data.speed_setpoints_rpm.FL = (float)db_data.drivebrain_set_rpm_fl;
+    _current_drivebrain_data.speed_setpoints_rpm.FR = (float)db_data.drivebrain_set_rpm_fr;
+    _current_drivebrain_data.speed_setpoints_rpm.RL = (float)db_data.drivebrain_set_rpm_rl;
+    _current_drivebrain_data.speed_setpoints_rpm.RR = (float)db_data.drivebrain_set_rpm_rr;
+
+}

lib/interfaces/src/TelemetryInterface.cpp

@@ -165,14 +165,31 @@ void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg(
     // TODO: change this to use actual torque values from inverter
     // Torque current just temporary for gearbox seal validation
     DRIVETRAIN_TORQUE_TELEM_t torque;
-    torque.fl_motor_torque_ro = HYTECH_fl_motor_torque_ro_toS(fl->get_torque_current());
-    torque.fr_motor_torque_ro = HYTECH_fr_motor_torque_ro_toS(fr->get_torque_current());
-    torque.rl_motor_torque_ro = HYTECH_rl_motor_torque_ro_toS(rl->get_torque_current());
-    torque.rr_motor_torque_ro = HYTECH_rr_motor_torque_ro_toS(rr->get_torque_current());
+    torque.fl_motor_torque = fl->get_torque_current();
+    torque.fr_motor_torque = fr->get_torque_current();
+    torque.rl_motor_torque = rl->get_torque_current();
+    torque.rr_motor_torque = rr->get_torque_current();
 
     enqueue_new_CAN<DRIVETRAIN_TORQUE_TELEM_t>(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech);
 }
 
+void TelemetryInterface::update_drivetrain_torque_filter_out_telem_CAN_msg(
+                                                                InvInt_t* fl,
+                                                                InvInt_t* fr,
+                                                                InvInt_t* rl,
+                                                                InvInt_t* rr)
+{
+    // TODO: change this to use actual torque values from inverter
+    // Torque current just temporary for gearbox seal validation
+    DRIVETRAIN_FILTER_OUT_TORQUE_TEL_t torque;
+    torque.fl_motor_torque = fl->get_mag_current();
+    torque.fr_motor_torque = fr->get_mag_current();
+    torque.rl_motor_torque = rl->get_mag_current();
+    torque.rr_motor_torque = rr->get_mag_current();
+
+    enqueue_new_CAN<DRIVETRAIN_FILTER_OUT_TORQUE_TEL_t>(&torque, &Pack_DRIVETRAIN_FILTER_OUT_TORQUE_TEL_hytech);
+}
+
 // Pack_PENTHOUSE_ACCUM_MSG_hytech
 void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s &current, const AnalogConversion_s &reference)
 {
@@ -307,6 +324,7 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1,
     update_drivetrain_err_status_CAN_msg(fl, fr, rl, rr);
     update_drivetrain_status_telem_CAN_msg(fl, fr, rl, rr, accel_implaus, brake_implaus, accel_per, brake_per);
     update_drivetrain_torque_telem_CAN_msg(fl, fr, rl, rr);
+    update_drivetrain_torque_filter_out_telem_CAN_msg(fl, fr, rl, rr);
 
     update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel],
                                    adc1.conversions[channels_.current_ref_channel]);

lib/mock_interfaces/DrivebrainInterface.h

@@ -0,0 +1,5 @@
+#ifndef __DRIVEBRAININTERFACE_H__
+#define __DRIVEBRAININTERFACE_H__
+
+
+#endif // __DRIVEBRAININTERFACE_H__

lib/shared_data/include/DrivebrainData.h

@@ -0,0 +1,14 @@
+#ifndef __DRIVEBRAINDATA_H__
+#define __DRIVEBRAINDATA_H__
+
+#include "Utility.h"
+
+struct DrivebrainData
+{
+    unsigned long last_torque_lim_receive_time_millis;
+    unsigned long last_speed_setpoint_receive_time_millis;
+    veh_vec<float> torque_limits_nm;
+    veh_vec<float> speed_setpoints_rpm;
+};
+
+#endif // __DRIVEBRAINDATA_H__

lib/systems/include/Utility.h → lib/shared_data/include/Utility.h

@@ -18,6 +18,23 @@ struct veh_vec
     T FR;
     T RL;
     T RR;
+
+    void construct(T FL, T FR, T RL, T RR)
+    {
+        FL = FL;
+        FR = FR;
+        RL = RL;
+        RR = RR;
+    }
+
+    /// @brief copy values to array in FL, FR, RL, RR order 
+    void copy_to_arr(T(&arr_out)[4])
+    {
+        arr_out[0]=FL;
+        arr_out[1]=FR;
+        arr_out[2]=RL;
+        arr_out[3]=RR;
+    }
 };
 
 template <typename T>