Skip to content

Commit

Permalink
adding in better handling of timing for drivebrain message latency
Browse files Browse the repository at this point in the history
  • Loading branch information
RCMast3r committed Oct 4, 2024
1 parent f858024 commit c0201ef
Show file tree
Hide file tree
Showing 6 changed files with 38 additions and 15 deletions.
6 changes: 5 additions & 1 deletion lib/interfaces/include/DrivebrainETHInterface.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,18 @@
class DrivebrainETHInterface
{
public:
DrivebrainETHInterface() = default;
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 = {};
};

Expand Down
7 changes: 6 additions & 1 deletion lib/interfaces/src/DrivebrainETHInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,9 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta
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;
}

Expand All @@ -22,5 +25,7 @@ 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_receive_time_millis = curr_millis;
_latest_data.DB_prev_MCU_recv_millis = msg_in.prev_MCU_recv_millis;
_latest_data.last_receive_time_millis = curr_millis; // current tick millis

}
5 changes: 4 additions & 1 deletion lib/shared_data/include/DrivebrainData.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,10 @@

struct DrivebrainData_s
{
unsigned long last_receive_time_millis;
/// @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;
};
Expand Down
14 changes: 12 additions & 2 deletions lib/systems/src/DrivebrainController.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,20 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &
auto sys_tick = state.systick;
auto db_input = state.db_data;

bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.last_receive_time_millis)) > (int)_params.allowed_latency);
// cases for timing_failure:
// 1 if the last_receive_time_millis < 0, then we have not received any messages from the db (initialized at -1 in constructor)
bool no_messages_received = db_input.last_receive_time_millis < 0;

// 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)
bool drivebrain_has_not_received_time = (db_input.DB_prev_MCU_recv_millis < 0);

// 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);


bool timing_failure = (message_too_latent);
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

Expand Down
2 changes: 1 addition & 1 deletion platformio.ini
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ lib_deps_shared =
git+ssh://[email protected]/hytech-racing/CASE_lib.git#v49
https://github.com/hytech-racing/shared_firmware_types.git#feb3b83
https://github.com/hytech-racing/shared_firmware_systems.git#af96a63
https://github.com/hytech-racing/HT_proto/releases/download/2024-09-17T03_08_13/hytech_msgs_pb_lib.tar.gz
https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T01_14_21/hytech_msgs_pb_lib.tar.gz

[env:test_env]
platform = native
Expand Down
19 changes: 10 additions & 9 deletions test/test_systems/drivebrain_controller_test.h
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,11 @@
#include <DrivebrainController.h>
#include <DrivebrainData.h>

auto runTick(DrivebrainController *controller, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis)
auto runTick(DrivebrainController *controller, float last_DB_receive_time_millis, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis)
{
DrivebrainData_s data;
data.last_receive_time_millis = last_receive_time_millis;
data.DB_prev_MCU_recv_millis = last_DB_receive_time_millis;
data.torque_limits_nm = {1, 1, 1, 1};
data.speed_setpoints_rpm = {1, 1, 1, 1};

Expand All @@ -24,38 +25,38 @@ auto runTick(DrivebrainController *controller, float last_receive_time_millis, C
TEST(DrivebrainControllerTesting, signals_sent_within_range)
{
DrivebrainController controller(10);
auto torque_controller_output_s = runTick(&controller, 1001, ControllerMode_e::MODE_4, 1002);
auto torque_controller_output_s = runTick(&controller, 998, 1001, ControllerMode_e::MODE_4, 1002);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1);
}

TEST(DrivebrainControllerTesting, setpoint_too_latent)
{
DrivebrainController controller(5);
auto torque_controller_output_s = runTick(&controller, 1006, ControllerMode_e::MODE_4, 1012);
auto torque_controller_output_s = runTick(&controller, 800, 1006, ControllerMode_e::MODE_4, 1012);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, failing_stay_failing)
{
DrivebrainController controller(10);
auto torque_controller_output_s = runTick(&controller, 1011, ControllerMode_e::MODE_4, 1032);
auto torque_controller_output_s = runTick(&controller, 200, 1011, ControllerMode_e::MODE_4, 1032);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);

torque_controller_output_s = runTick(&controller, 1033, ControllerMode_e::MODE_4, 1033);
torque_controller_output_s = runTick(&controller, 200, 1033, ControllerMode_e::MODE_4, 1033);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);

torque_controller_output_s = runTick(&controller, 1034, ControllerMode_e::MODE_4, 1034);
torque_controller_output_s = runTick(&controller, 400, 1034, ControllerMode_e::MODE_4, 1034);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, failing_reset_success)
{
DrivebrainController controller(10);
auto torque_controller_output_s = runTick(&controller, 1011, ControllerMode_e::MODE_4, 1022);
auto torque_controller_output_s = runTick(&controller, 300, 1011, ControllerMode_e::MODE_4, 1022);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);

torque_controller_output_s = runTick(&controller, 1021, ControllerMode_e::MODE_3, 1023);
torque_controller_output_s = runTick(&controller, 1022, ControllerMode_e::MODE_4, 1024);
torque_controller_output_s = runTick(&controller, 1020, 1021, ControllerMode_e::MODE_3, 1023);
torque_controller_output_s = runTick(&controller, 1022, 1022, ControllerMode_e::MODE_4, 1024);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1);
}

Expand Down

0 comments on commit c0201ef

Please sign in to comment.