removing CAN based drivebrain interface, addressing naming for invert…

…er interface updates, updating ethernet interface since only one message is received, and fixing tests for new interface

lib/interfaces/include/HytechCANInterface.h

@@ -33,7 +33,6 @@ 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
@@ -168,7 +167,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;
@@ -181,11 +179,7 @@ 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;
-        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);
+        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_; }
-    int16_t 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
-    int16_t 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,9 @@ 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_ = mc_status.get_magnetizing_current(); // 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

lib/interfaces/include/ProtobufMsgInterface.h

@@ -13,24 +13,22 @@ struct ETHInterfaces
 {
 };
 
-using namespace qindesign::network;
-
 // this should be usable with arbitrary functions idk something
 template <size_t buffer_size, typename pb_msg_type, class eth_interface>
-void handle_ethernet_socket_receive(const SysTick_s& tick, 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)
+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)
         {
         uint8_t buffer[buffer_size];
         size_t read_bytes = socket->read(buffer, sizeof(buffer));
-        socket->read(buffer, 1024);
+        socket->read(buffer, buffer_size);
         recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer);
     }
 }
 
 template <typename pb_struct, size_t buffer_size>
-bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, EthernetUDP *socket, const pb_struct &msg, const pb_msgdesc_t *msg_desc)
+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(addr, port);
     uint8_t buffer[buffer_size];

lib/interfaces/src/DrivebrainETHInterface.cpp

@@ -26,6 +26,5 @@ void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &ms
 
     _latest_data.torque_limits_nm = nm_lim;
     _latest_data.speed_setpoints_rpm = speed_set;
-    _latest_data.last_torque_lim_receive_time_millis = curr_millis;
-    _latest_data.last_speed_setpoint_receive_time_millis = curr_millis;
+    _latest_data.last_receive_time_millis = curr_millis;
 }

lib/interfaces/src/TelemetryInterface.cpp

@@ -167,10 +167,10 @@ 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);
 }
@@ -184,10 +184,10 @@ void TelemetryInterface::update_drivetrain_torque_filter_out_telem_CAN_msg(
     // 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();
+    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);
 }

lib/shared_data/include/DrivebrainData.h

@@ -5,8 +5,7 @@
 
 struct DrivebrainData_s
 {
-    unsigned long last_torque_lim_receive_time_millis;
-    unsigned long last_speed_setpoint_receive_time_millis;
+    unsigned long last_receive_time_millis;
     veh_vec<float> torque_limits_nm;
     veh_vec<float> speed_setpoints_rpm;
 };

lib/systems/include/DrivebrainController.h

@@ -23,27 +23,25 @@ class DrivebrainController : public Controller
 {
 public:
 
-    DrivebrainController(unsigned long allowed_latency, unsigned long allowed_jitter,
+    DrivebrainController(unsigned long allowed_latency,
                          float max_fault_clear_speed_m_s = 1.0,
                          ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4)
     {
-        _params = {allowed_jitter, allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode};
+        _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode};
     }
 
     TorqueControllerOutput_s evaluate(const SharedCarState_s &state);
 
 private:
     struct
     {
-        unsigned long allowed_jitter;
         unsigned long allowed_latency;
         float max_fault_clear_speed_m_s;
         ControllerMode_e assigned_controller_mode;
     } _params;
 
     bool _timing_failure = false;
-    unsigned long _last_sent_torque_lim_millis = -1;
-    unsigned long _last_sent_speed_setpoint_millis = -1;
+    unsigned long _last_setpoint_millis = -1;
 };
 
 #endif // __DRIVEBRAINCONTROLLER_H__

lib/systems/include/DrivetrainSystem.tpp

@@ -233,8 +233,8 @@ DrivetrainDynamicReport_s DrivetrainSystem<InverterType>::get_dynamic_data()
     int inverter_ind = 0;
     for (auto inv_pointer : inverters_)
     {
-        auto iq = inv_pointer->get_torque_current(); // iq in A
-        auto id = inv_pointer->get_mag_current();    // id in A
+        auto iq = inv_pointer->get_motor_torque(); // iq in A
+        auto id = inv_pointer->get_commanded_torque();    // id in A
         dynamic_data_.measuredSpeeds[inverter_ind] = inv_pointer->get_speed();
         dynamic_data_.measuredTorqueCurrents[inverter_ind] = iq;
         dynamic_data_.measuredMagnetizingCurrents[inverter_ind] = id;

lib/systems/src/DrivebrainController.cpp

@@ -8,31 +8,13 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
     auto sys_tick = state.systick;
     auto db_input = state.db_data;     
 
-    bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency);
-    bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency);
-
-    auto speed_sp_recv = db_input.last_speed_setpoint_receive_time_millis;
-    auto t_sp_recv = db_input.last_torque_lim_receive_time_millis;
+    bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.last_receive_time_millis)) > (int)_params.allowed_latency);
 
-    bool msg_jitter_too_high;
 
-    int diff = 0;
-    if(t_sp_recv > speed_sp_recv)
-    {
-        msg_jitter_too_high = ( (t_sp_recv - speed_sp_recv) > _params.allowed_jitter);
-        diff = (t_sp_recv - speed_sp_recv);
-    } else if (speed_sp_recv > t_sp_recv){
-        diff = (speed_sp_recv - t_sp_recv);
-        msg_jitter_too_high = ( (speed_sp_recv - t_sp_recv) > _params.allowed_jitter);
-    } else {
-        msg_jitter_too_high = false;
-    }
-
-    bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high);
+    bool timing_failure = (message_too_latent);
 
     // 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.tc_mux_status.current_controller_mode_ == _params.assigned_controller_mode;
 
     if ((!is_active_controller) && (!timing_failure))
@@ -44,10 +26,7 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
     TorqueControllerOutput_s output;
     if (!timing_failure && (!_timing_failure))
     {
-        _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis;
-        _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis;
-
-
+        _last_setpoint_millis = db_input.last_receive_time_millis;
 
         db_input.speed_setpoints_rpm.copy_to_arr(output.command.speeds_rpm); 
         db_input.torque_limits_nm.copy_to_arr(output.command.torqueSetpoints); 

src/main.cpp

@@ -325,9 +325,8 @@ MCUStateMachine<DriveSys_t> fsm(&buzzer, &drivetrain, &dashboard, &pedals_system
 //     GROUPING STRUCTS (To limit parameter count in utilizing functions)
 // */
 
-DrivebrainInterface db_interface(&CAN3_txBuffer);
 DrivebrainETHInterface db_eth_interface;
-CANInterfaces<CircularBufferType> CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface, &db_interface};
+CANInterfaces<CircularBufferType> CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface};
 
 /*
     FUNCTION DEFINITIONS
@@ -505,7 +504,6 @@ void loop()
         Serial.println();
         Serial.print("dial state: ");
         Serial.println(static_cast<int>(dashboard.getDialMode()));
-        Serial.println(db_interface.get_latest_db_data().torque_limits_nm.FL);
         Serial.println();
         Serial.println();
     }