Skip to content

Commit

Permalink
adjusting logic; removing interaction with tc and giving drivebrain m…
Browse files Browse the repository at this point in the history 
…ore responsbility
  • Loading branch information
@RCMast3r
RCMast3r committed Sep 7, 2024
1 parent 5310548 commit 57c2a96
Show file tree
Hide file tree
Showing 8 changed files with 118 additions and 92 deletions.
1 change: 1 addition & 0 deletions lib/interfaces/include/DrivebrainInterface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ 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;
Expand Down
12 changes: 10 additions & 2 deletions lib/interfaces/src/DrivebrainInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,12 @@ void DrivebrainInterface::receive_db_torque_lim_message(CAN_message_t& msg, unsi
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.construct(fl, fr, rl, rr);

_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)
Expand All @@ -20,6 +25,9 @@ void DrivebrainInterface::receive_db_speed_setpoint_message(CAN_message_t& msg,
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.construct(db_data.drivebrain_set_rpm_fl, db_data.drivebrain_set_rpm_fr, db_data.drivebrain_set_rpm_rl, db_data.drivebrain_set_rpm_rr);
_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;

}
118 changes: 59 additions & 59 deletions lib/systems/include/CASESystem.tpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,71 +200,71 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(

// send these out at the send period

if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_))
{
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p);

last_controller_pt1_send_time_ = tick.millis;
}
// if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_))
// {
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p);

// last_controller_pt1_send_time_ = tick.millis;
// }

if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_))
{
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn);

last_controller_pt2_send_time_ = tick.millis;
}
// if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
// ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_))
// {
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn);

// last_controller_pt2_send_time_ = tick.millis;
// }

if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_))
{
// if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
// ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_))
// {

enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p);

last_controller_pt3_send_time_ = tick.millis;
}
// last_controller_pt3_send_time_ = tick.millis;
// }

if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) &&
((tick.millis - last_vehm_send_time_) > controller_send_period_ms_))
{
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg);
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl);
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_);
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_);
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_);
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg);
enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve);

last_vehm_send_time_ = tick.millis;
}
// if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) &&
// ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_))
// {
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve);

// last_vehm_send_time_ = tick.millis;
// }

if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_))
{
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna);

last_lowest_priority_controller_send_time_ = tick.millis;
}
// if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_))
// {
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl);
// enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna);

// last_lowest_priority_controller_send_time_ = tick.millis;
// }

DrivetrainCommand_s command;

Expand Down
4 changes: 2 additions & 2 deletions lib/systems/include/TorqueControllerMux.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ class TorqueControllerMux
, torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)])
, torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)])
, torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
, _dbController(controllerOutputs_[static_cast<int>(TorqueController_e::TC_DRIVEBRAIN)], 30, 10)
, _dbController(controllerOutputs_[static_cast<int>(TorqueController_e::TC_DRIVEBRAIN)], 100, 10)
, tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)])
, telemHandle_(telemInterface) {}

Expand All @@ -88,7 +88,7 @@ class TorqueControllerMux
, torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale)
, torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale)
, torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
, _dbController(controllerOutputs_[static_cast<int>(TorqueController_e::TC_DRIVEBRAIN)], 30, 10)
, _dbController(controllerOutputs_[static_cast<int>(TorqueController_e::TC_DRIVEBRAIN)], 200, 120)
, tcCASEWrapper_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_CASE_SYSTEM)])
, telemHandle_(telemInterface) {}

Expand Down
27 changes: 22 additions & 5 deletions lib/systems/src/DrivebrainController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,30 @@

void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller)
{
_writeout.ready = false;

_writeout.ready = true;


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);
bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter);

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

// only in the case that this is the active controller do we want to clear our timing failure
Expand All @@ -16,9 +34,9 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp
// timing failure should be false here
_timing_failure = false;
}

if (!timing_failure && (!_timing_failure))
{
_writeout.ready = true;
_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;

Expand All @@ -28,7 +46,6 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp
else
{
_timing_failure = true;
_writeout.ready = false;
_writeout.command = {}; // set command to all zeros if bad latency is apparent
_writeout.command = {0.0f, 0.0f, 0.0f ,0.0f }; // set command to all zeros if bad latency is apparent
}
}
4 changes: 2 additions & 2 deletions lib/systems/src/TorqueControllerMux.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,9 +90,9 @@ void TorqueControllerMux::tick(
// Check if the current controller is ready. If it has faulted, revert to safe mode
// When the car goes below 5m/s, it will attempt to re-engage the faulted controller
// It will stay in safe mode if the controller is still faulted
if (controllerOutputs_[static_cast<int>(muxMode_)].ready == false)
if (!controllerOutputs_[static_cast<int>(muxMode_)].ready)
{
muxMode_ = TorqueController_e::TC_SAFE_MODE;
muxMode_ = TorqueController_e::TC_NO_CONTROLLER;
}

// Update TCMux status
Expand Down
6 changes: 3 additions & 3 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -551,12 +551,12 @@ void tick_all_interfaces(const SysTick_s &current_system_tick)
a1.get().conversions[MCU15_BRAKE2_CHANNEL],
pedals_system.getMechBrakeActiveThreshold(),
{});
ams_interface.tick(current_system_tick);
}

if (t.trigger50) // 50Hz
{
steering1.sample();
ams_interface.tick(current_system_tick);
}

if (t.trigger100) // 100Hz
Expand Down Expand Up @@ -651,7 +651,7 @@ void tick_all_systems(const SysTick_s &current_system_tick)
vn_interface.get_vn_struct().vn_status);

// case_system.update_config_from_param_interface(param_interface);

auto test = db_interface.get_latest_db_data();
torque_controller_mux.tick(
current_system_tick,
drivetrain.get_dynamic_data(),
Expand All @@ -663,7 +663,7 @@ void tick_all_systems(const SysTick_s &current_system_tick)
dashboard.torqueModeButtonPressed(),
vn_interface.get_vn_struct(),
controller_output,
db_interface.get_latest_db_data());
test);
}

void handle_ethernet_interface_comms()
Expand Down
38 changes: 19 additions & 19 deletions test/test_systems/drivebrain_controller_test.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,35 +33,35 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent) {
DrivebrainController controller(torque_controller_output_s, 5, 5);
runTick(&controller, 1001, 1006, true);

EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, torque_setpoint_too_latent) {
TorqueControllerOutput_s torque_controller_output_s = {};
DrivebrainController controller(torque_controller_output_s, 5, 5);
runTick(&controller, 1006, 1001, true);

EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, msg_jitter_too_high) {
TorqueControllerOutput_s torque_controller_output_s = {};
DrivebrainController controller(torque_controller_output_s, 10, 5);
runTick(&controller, 1001, 1009, true);

EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_torque_setpoint_too_latent) {
TorqueControllerOutput_s torque_controller_output_s = {};
DrivebrainController controller(torque_controller_output_s, 5, 10);
runTick(&controller, 1001, 1015, true);

EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}


Expand All @@ -70,42 +70,42 @@ TEST(DrivebrainControllerTesting, speed_setpoint_too_latent_and_msg_jitter_too_h
DrivebrainController controller(torque_controller_output_s, 10, 5);
runTick(&controller, 1011, 1001, true);

EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, torque_setpoint_too_latent_and_msg_jitter_too_high) {
TorqueControllerOutput_s torque_controller_output_s = {};
DrivebrainController controller(torque_controller_output_s, 10, 5);
runTick(&controller, 1001, 1011, true);

EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, failing_stay_failing) {
TorqueControllerOutput_s torque_controller_output_s = {};
DrivebrainController controller(torque_controller_output_s, 10, 5);
runTick(&controller, 1001, 1011, true);
EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);

runTick(&controller, 1001, 1001, true);
EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);
}

TEST(DrivebrainControllerTesting, failing_reset_success) {
TorqueControllerOutput_s torque_controller_output_s = {};
DrivebrainController controller(torque_controller_output_s, 10, 5);
runTick(&controller, 1001, 1011, true);
EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_NE(torque_controller_output_s.command.speeds_rpm[0], 1);
// EXPECT_FALSE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0);

runTick(&controller, 1001, 1001, false);

runTick(&controller, 1001, 1001, true);
EXPECT_TRUE(torque_controller_output_s.ready);
// EXPECT_TRUE(torque_controller_output_s.ready);
EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1);
}

Expand Down

0 comments on commit 57c2a96

Please sign in to comment.