From 3ca6d5364b934a505ff9db7ee92fea3251ffa8e5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 13 Oct 2024 12:50:16 -0400 Subject: [PATCH] added docs for the drivebrain controller --- lib/interfaces/src/DrivebrainETHInterface.cpp | 6 +-- lib/systems/include/DrivebrainController.h | 49 ++++++++++++++++--- lib/systems/src/DrivebrainController.cpp | 18 +++---- src/main.cpp | 1 + 4 files changed, 52 insertions(+), 22 deletions(-) diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index f948da2d..0a868481 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -7,13 +7,13 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta hytech_msgs_MCUOutputData out; out.accel_percent = shared_state.pedals_data.accelPercent; out.brake_percent = shared_state.pedals_data.regenPercent; - + out.has_rpm_data = true; out.rpm_data.FL = shared_state.drivetrain_data.measuredSpeeds[0]; out.rpm_data.FR = shared_state.drivetrain_data.measuredSpeeds[1]; out.rpm_data.RL = shared_state.drivetrain_data.measuredSpeeds[2]; out.rpm_data.RR = 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; out.load_cell_data = {shared_state.raw_loadcell_data.raw_load_cell_data.FL, @@ -29,9 +29,7 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis) { veh_vec 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 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; diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 9b9cefe3..d418ee10 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -2,15 +2,15 @@ #define __DRIVEBRAINCONTROLLER_H__ #include -// #include "TorqueControllers.h" #include "DrivebrainData.h" #include "BaseController.h" #include "SharedDataTypes.h" #include + // TODO - [x] 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, +// TODO - [x] 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 @@ -19,28 +19,65 @@ // 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 - [x] correctly measure latency between drivebrain and MCU +// +// measure latency by subtracting the latest MCU millis stamped message from the drivebrain from the +// current system millis to + + +/*! \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 comparing the mcu systick's millis stamp within the most recent packet received from the + drivebrain to the current systick millis. if the difference is too great, we swap to MODE0 control. The drivebrain's + MCU timestamp comes from the most recent packet it received from the MCU as it contains the MCU systick's millis + stamp at that packet's creation. + + 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 systick + */ class DrivebrainController : public Controller { 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, - float max_fault_clear_speed_m_s = 1.0, ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4) : _emergency_control(1.0f, 1.0f) { - _last_worst_latency_rec_time =0; + _last_worst_latency_rec_time = 0; _worst_latency_so_far = -1; - _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode}; + _params = {allowed_latency, assigned_controller_mode}; } + /// @brief evaluate function for running the business logic + /// @param state the current state of the car + /// @return torque controller output that gets passed through the TC MUX TorqueControllerOutput_s evaluate(const SharedCarState_s &state); + + /// @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; - float max_fault_clear_speed_m_s; ControllerMode_e assigned_controller_mode; } _params; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index ec3c040c..06229463 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,5 +1,5 @@ #include "DrivebrainController.h" -// #include + TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state) { @@ -12,32 +12,26 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & // 2 if the DB_prev_MCU_recv_millis < 0, then the drivebrain has not received a time from the MCU // (meaning that the MCU is not sending properly or the drivebrain is not receiving properly or it has - // yet to receive from the MCU yet) + // yet to receive from the MCU yet) bool drivebrain_has_not_received_time = (db_input.DB_prev_MCU_recv_millis < 0); - // Serial.println("uh"); + // 3 if the time between the current MCU sys_tick.millis time and the last millis time that the drivebrain received is too high bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.DB_prev_MCU_recv_millis)) > (int)_params.allowed_latency); if((sys_tick.millis - _last_worst_latency_rec_time) > 5000) - { - // Serial.print("_worst_latency_so_far "); - // Serial.println(_worst_latency_so_far); - // Serial.print("errord:"); - // Serial.println(_timing_failure); + { _last_worst_latency_rec_time = sys_tick.millis; _worst_latency_so_far = -1; } if( (sys_tick.millis - db_input.DB_prev_MCU_recv_millis) > _worst_latency_so_far) { - - _worst_latency_so_far = (sys_tick.millis - db_input.DB_prev_MCU_recv_millis); - + _worst_latency_so_far = (sys_tick.millis - db_input.DB_prev_MCU_recv_millis); } bool timing_failure = (message_too_latent || no_messages_received || drivebrain_has_not_received_time); - // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault + // only in the case that we are not the active controller yet do we want to clear the fault bool is_active_controller = state.tc_mux_status.current_controller_mode_ == _params.assigned_controller_mode; diff --git a/src/main.cpp b/src/main.cpp index f4112065..77d93603 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -442,6 +442,7 @@ void loop() torque_controller_mux.get_tc_mux_status(), db_controller.get_timing_failure_status()); + car_state_inst.drivebrain_timing_failure = db_controller.get_timing_failure_status(); hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst); handle_ethernet_interface_comms(curr_tick, out_eth_msg);