Feature/drivebrain integration (#105)

* tuning pedals and updated to latest can definition

* drivebrain controller creation

* added receiving of drivebrain messages

* fixing pedals tests

* adding drivebrain interface empty mock header

* feat(drivebrain_controller): added tests for drivebrain controller latency and jitter

* adding in latching logic for the db controller

* added in drivebrain interface into main

* ensuring that the regen and torque limits dont get applied to the drivebrain controller

* fix(drivebrain_controller): fixed drivebrain_controller failure and reset logic

* adjusting logic; removing interaction with tc and giving drivebrain more responsbility

* fixing oopsie in drivebrain control mode and adding it to main

* ready to test on car

* fixing header

* seemingly working ethernet integration

* switching ethernet driver

* removing CAN based drivebrain interface, addressing naming for inverter interface updates, updating ethernet interface since only one message is received, and fixing tests for new interface

* making the veh_vec constuctor actually a constructor

* addressing comments

* a little cleanup

* added compiler macro flag for debug prints

* added in type alias

* adding in better handling of timing for drivebrain message latency

* updated to latest HT-prot

* updating pedal params and to latest HT_proto

---------

Co-authored-by: sreekara <[email protected]>

include/InterfaceParams.h

@@ -1,14 +1,17 @@
 #ifndef INTERFACEPARAMS
 #define INTERFACEPARAMS
-#include "NativeEthernet.h"
+
+#include <QNEthernet.h>
+
+using namespace qindesign::network;
 
 namespace EthParams
 {
     uint8_t default_MCU_MAC_address[6] = 
     {0x04, 0xe9, 0xe5, 0x10, 0x1f, 0x22};
 
     const IPAddress default_MCU_ip(192, 168, 1, 30);
-    const IPAddress default_TCU_ip(192, 168, 1, 68);
+    const IPAddress default_TCU_ip(192, 168, 1, 69);
 
     const uint16_t default_protobuf_send_port = 2001;
     const uint16_t default_protobuf_recv_port = 2000;

include/MCU_rev15_defs.h

@@ -52,14 +52,14 @@ 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 = 3491;  
+const int ACCEL1_PEDAL_MAX = 3490;  
 const int ACCEL2_PEDAL_MAX = 189;   
 
-const int ACCEL1_PEDAL_MIN = 2244;
-const int ACCEL2_PEDAL_MIN = 1405;
+const int ACCEL1_PEDAL_MIN = 2646;
+const int ACCEL2_PEDAL_MIN = 1030;
 
-const int BRAKE1_PEDAL_MAX = 1945;  
-const int BRAKE2_PEDAL_MAX = 1742;  
+const int BRAKE1_PEDAL_MAX = 2022;  
+const int BRAKE2_PEDAL_MAX = 1668;  
 
 const int BRAKE1_PEDAL_MIN = 1192;  
 const int BRAKE2_PEDAL_MIN = 2476;

lib/interfaces/include/DrivebrainETHInterface.h

@@ -0,0 +1,24 @@
+#ifndef __DRIVEBRAINETHINTERFACE_H__
+#define __DRIVEBRAINETHINTERFACE_H__
+#include "hytech_msgs.pb.h"
+#include "DrivebrainData.h"
+#include "SharedDataTypes.h"
+class DrivebrainETHInterface
+{
+public:
+    DrivebrainETHInterface(){
+        _latest_data.last_receive_time_millis = -1;
+        _latest_data.DB_prev_MCU_recv_millis = -1;
+    };
+
+    void receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis);
+
+    hytech_msgs_MCUOutputData make_db_msg(const SharedCarState_s &shared_state);
+    DrivebrainData_s get_latest_data() { return _latest_data; }
+
+private:
+
+    DrivebrainData_s _latest_data = {};
+};
+
+#endif // __DRIVEBRAINETHINTERFACE_H__

lib/interfaces/include/HytechCANInterface.h

@@ -166,7 +166,6 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces,
         case VN_YPR_CANID:
             interfaces.vn_interface->retrieve_ypr_CAN(recvd_msg);
             break;
-
         case VN_LAT_LON_CANID:
             interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg);
             break;
@@ -179,6 +178,8 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces,
         case VN_ANGULAR_RATE_CANID:
             interfaces.vn_interface->receive_ang_rates_CAN(recvd_msg);
             break;
+        default:
+            break;
         }
     }
 }

lib/interfaces/include/InverterInterface.h

@@ -119,16 +119,16 @@ class InverterInterface
     }
 
     int16_t get_speed() { return speed_; }
-    float get_torque_current() { return torque_current_; }
-    float get_mag_current() { return magnetizing_current_; }
+    float get_motor_torque() { return actual_motor_torque_; }
+    int16_t get_commanded_torque() { return commanded_torque_; }
     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 actual_motor_torque_, commanded_torque_;
     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

@@ -131,9 +131,10 @@ void InverterInterface<message_queue>::receive_status_msg(CAN_message_t &msg)
     // https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf#page=83&zoom=100,76,82
     // for the actual conversion. requires looking at the current params of the inverter
     // 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
+    // actual_motor_torque_ = ((float)mc_status.get_motor_torque() * id110_val_) / 16384.0;           // iq
+    actual_motor_torque_ = mc_status.get_torque_current();
+    commanded_torque_ = 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/LoadCellInterface.h

@@ -19,6 +19,14 @@ struct LoadCellInterfaceTick_s
 
 class LoadCellInterface
 {
+public:
+    LoadCellInterface() {
+        loadCellConversions_ = veh_vec<AnalogConversion_s>();
+        loadCellForcesUnfiltered_ = veh_vec<float[numFIRTaps_]>();
+        loadCellForcesFiltered_ = veh_vec<float>();
+    }
+    void tick(const LoadCellInterfaceTick_s &intake);
+    LoadCellInterfaceOutput_s getLoadCellForces();
 private:
     /*
     FIR filter designed with
@@ -50,10 +58,7 @@ class LoadCellInterface
     veh_vec<float[numFIRTaps_]> loadCellForcesUnfiltered_;
     veh_vec<float> loadCellForcesFiltered_;
     bool FIRSaturated_ = false;
-public:
-    LoadCellInterface() {}
-    void tick(const LoadCellInterfaceTick_s &intake);
-    LoadCellInterfaceOutput_s getLoadCellForces();
+
 };
 
 #endif

lib/interfaces/include/ProtobufMsgInterface.h

@@ -5,39 +5,36 @@
 #include "pb_decode.h"
 #include "pb_common.h"
 #include "circular_buffer.h"
-#include "NativeEthernet.h"
-#include "MCU_rev15_defs.h"
-#include "InterfaceParams.h"
+#include <QNEthernet.h>
+
+#include <functional>
 
 struct ETHInterfaces
 {
 };
 
-using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces);
-
 // this should be usable with arbitrary functions idk something
-void handle_ethernet_socket_receive(EthernetUDP* socket, recv_function_t recv_function, ETHInterfaces& interfaces)
+template <size_t buffer_size, typename pb_msg_type, class eth_interface>
+void handle_ethernet_socket_receive(const SysTick_s& tick, qindesign::network::EthernetUDP *socket, std::function<void(unsigned long, const uint8_t *, size_t, eth_interface &, const pb_msgdesc_t *)> recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer)
 {
     int packet_size = socket->parsePacket();
-    if(packet_size > 0)
-    {
-        Serial.println("packet size");
-        Serial.println(packet_size);
-        uint8_t buffer[EthParams::default_buffer_size];
+    if (packet_size > 0)
+        {
+        uint8_t buffer[buffer_size];
         size_t read_bytes = socket->read(buffer, sizeof(buffer));
-        socket->read(buffer, UDP_TX_PACKET_MAX_SIZE);
-        recv_function(buffer, read_bytes, interfaces);
+        socket->read(buffer, buffer_size);
+        recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer);
     }
 }
 
-template <typename pb_struct>
-bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, const pb_msgdesc_t* msg_desc)
+template <typename pb_struct, size_t buffer_size>
+bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, qindesign::network::EthernetUDP *socket, const pb_struct &msg, const pb_msgdesc_t *msg_desc)
 {
-    socket->beginPacket(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port);
-
-    uint8_t buffer[EthParams::default_buffer_size];
+    socket->beginPacket(addr, port);
+    uint8_t buffer[buffer_size];
     pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
-    if (!pb_encode(&stream, msg_desc, &msg)) {
+    if (!pb_encode(&stream, msg_desc, &msg))
+    {
         // You can handle error more explicitly by looking at stream.errmsg
         return false;
     }
@@ -47,19 +44,15 @@ bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, c
     return true;
 }
 
-
-template <typename pb_msg_type>
-std::pair<pb_msg_type, bool> recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces, const pb_msgdesc_t * desc_pointer)
+template <typename pb_msg_type, class eth_interface>
+void recv_pb_stream_msg(unsigned long curr_millis, const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer)
 {
     pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size);
     pb_msg_type msg = {};
     if (pb_decode(&stream, desc_pointer, &msg))
     {
-        return {msg, true};
+        interface.receive_pb_msg(msg, curr_millis);
     }
-    return {msg, };
 }
 
-
-
 #endif

lib/interfaces/include/TelemetryInterface.h

@@ -100,7 +100,14 @@ class TelemetryInterface
         InvInt_t *fl,
         InvInt_t *fr,
         InvInt_t *rl,
-        InvInt_t *rr);
+        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/library.json

@@ -5,7 +5,8 @@
     "dependencies": {
         "can_lib": "*",
         "shared_data": "*",
-        "CASE_lib": "*"
+        "CASE_lib": "*",
+        "nanopb": "*"
       },
     "frameworks": "*",
     "platforms": "*"

lib/interfaces/src/DrivebrainETHInterface.cpp

@@ -0,0 +1,31 @@
+#include "DrivebrainETHInterface.h"
+#include "SharedDataTypes.h"
+#include <Arduino.h>
+
+hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarState_s &shared_state)
+{
+    hytech_msgs_MCUOutputData out;
+    out.accel_percent = shared_state.pedals_data.accelPercent;
+    out.brake_percent = shared_state.pedals_data.brakePercent;
+    out.rpm_data = {shared_state.drivetrain_data.measuredSpeeds[0],
+                    shared_state.drivetrain_data.measuredSpeeds[1],
+                    shared_state.drivetrain_data.measuredSpeeds[2],
+                    shared_state.drivetrain_data.measuredSpeeds[3]};
+
+    out.steering_angle_deg = shared_state.steering_data.angle;
+    out.MCU_recv_millis = _latest_data.last_receive_time_millis; 
+    return out;
+}
+
+void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis)
+{
+    veh_vec<float> nm_lim(msg_in.torque_limit_nm.FL, msg_in.torque_limit_nm.FR, msg_in.torque_limit_nm.RL, msg_in.torque_limit_nm.RR);
+
+    veh_vec<float> speed_set(msg_in.desired_rpms.FL, msg_in.desired_rpms.FR, msg_in.desired_rpms.RL, msg_in.desired_rpms.RR);
+
+    _latest_data.torque_limits_nm = nm_lim;
+    _latest_data.speed_setpoints_rpm = speed_set;
+    _latest_data.DB_prev_MCU_recv_millis = msg_in.prev_MCU_recv_millis;
+    _latest_data.last_receive_time_millis = curr_millis; // current tick millis
+
+}

lib/interfaces/src/TelemetryInterface.cpp

@@ -167,14 +167,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 = 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();
+    torque.fl_motor_torque = fl->get_motor_torque();
+    torque.fr_motor_torque = fr->get_motor_torque();
+    torque.rl_motor_torque = rl->get_motor_torque();
+    torque.rr_motor_torque = rr->get_motor_torque();
 
     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_commanded_torque();
+    torque.fr_motor_torque = fr->get_commanded_torque();
+    torque.rl_motor_torque = rl->get_commanded_torque();
+    torque.rr_motor_torque = rr->get_commanded_torque();
+
+    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)
 {
@@ -281,6 +298,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,16 @@
+#ifndef __DRIVEBRAINDATA_H__
+#define __DRIVEBRAINDATA_H__
+
+#include "Utility.h"
+
+struct DrivebrainData_s
+{
+    /// @brief the latest time that the MCU received a message w.r.t the current tick's millis
+    int64_t last_receive_time_millis = -1;
+    /// @brief the latest MCU last_receive_time_millis that the drivebrain received
+    int64_t DB_prev_MCU_recv_millis = -1;
+    veh_vec<float> torque_limits_nm;
+    veh_vec<float> speed_setpoints_rpm;
+};
+
+#endif // __DRIVEBRAINDATA_H__