diff --git a/.gitmodules b/.gitmodules index 4b5e7c7a..4fc902cd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,4 @@ [submodule "src/Middlewares/SUFST/can-defs"] path = src/Middlewares/SUFST/can-defs url = https://github.com/sufst/can-defs + branch = feat/r2d-pump-lockout diff --git a/src/Middlewares/SUFST/can-defs b/src/Middlewares/SUFST/can-defs index f50171db..8ddc2b80 160000 --- a/src/Middlewares/SUFST/can-defs +++ b/src/Middlewares/SUFST/can-defs @@ -1 +1 @@ -Subproject commit f50171dba2f1c196955db9003b73f4c42231bdb6 +Subproject commit 8ddc2b8059cb0ac7c4a40dd782efe6671c551a45 diff --git a/src/SUFST/Inc/Services/pm100.h b/src/SUFST/Inc/Services/pm100.h index 4a5548ba..992c7d15 100644 --- a/src/SUFST/Inc/Services/pm100.h +++ b/src/SUFST/Inc/Services/pm100.h @@ -13,6 +13,7 @@ #define PM100_H #include +#include #include #include #include @@ -32,6 +33,8 @@ #define PM100_RX_QUEUE_SIZE 10 // 10 items +#define PUMP_RUNNING_THRESHOLD 8 // Voltage/V + /** * @brief PM100 context */ @@ -49,6 +52,7 @@ typedef struct struct can_c_pm100_temperature_set_1_t temp1; struct can_c_pm100_temperature_set_2_t temp2; struct can_c_pm100_temperature_set_3_t temp3; + struct can_s_pdm_out_voltage_t vout; uint16_t error; const config_pm100_t* config_ptr; } pm100_context_t; @@ -69,4 +73,6 @@ int16_t pm100_max_inverter_temp(pm100_context_t* pm100_ptr); status_t pm100_disable(pm100_context_t* pm100_ptr); status_t pm100_request_torque(pm100_context_t* pm100_ptr, uint16_t torque); +bool pm100_check_pumps_running(pm100_context_t* pm100_ptr); + #endif diff --git a/src/SUFST/Inc/config.h b/src/SUFST/Inc/config.h index 1ca270c8..6a636f01 100644 --- a/src/SUFST/Inc/config.h +++ b/src/SUFST/Inc/config.h @@ -46,6 +46,7 @@ typedef struct { config_thread_t thread; // control thread config uint32_t schedule_ticks; // number of ticks between runs of the control loop thread bool r2d_requires_brake; // whether or not the brake needs to be pressed for R2D activation + bool r2d_requires_pump; // whether or not the pump needs to be running for R2D activation uint32_t ts_ready_timeout_ticks; // ticks after which waiting for TS ready times out uint32_t ts_ready_poll_ticks; // how often to poll input when waiting for TS ready uint32_t precharge_timeout_ticks; // ticks after which waiting for precharge times out diff --git a/src/SUFST/Src/Services/ctrl.c b/src/SUFST/Src/Services/ctrl.c index 445fd0a3..3c41271e 100644 --- a/src/SUFST/Src/Services/ctrl.c +++ b/src/SUFST/Src/Services/ctrl.c @@ -242,12 +242,14 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) // TS is ready, can initiate pre-charge sequence // TS on LED turns solid + // powers on pumps in preparation for r2d case (CTRL_STATE_PRECHARGE_WAIT): { const uint32_t charge_time = tx_time_get() - ctrl_ptr->precharge_start; if (pm100_is_precharged(ctrl_ptr->pm100_ptr)) { + ctrl_ptr->pump_pwr = 1; next_state = CTRL_STATE_R2D_WAIT; dash_clear_buttons(dash_ptr); LOG_INFO("Precharge complete\n"); @@ -264,6 +266,7 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) // pre-charge is complete, wait for R2D signal // also wait for brake to be fully pressed (if enabled) + // also wait for pumps to be running (if enabled) case (CTRL_STATE_R2D_WAIT): { if (!trc_ready()) @@ -295,12 +298,15 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) ? (ctrl_ptr->bps_reading > BPS_ON_THRESH) : 1; + r2d &= (config_ptr->r2d_requires_pump) + ? pm100_check_pumps_running(ctrl_ptr->pm100_ptr) + : 1; + if (r2d) { dash_set_r2d_led_state(dash_ptr, GPIO_PIN_SET); pm100_disable(ctrl_ptr->pm100_ptr); rtds_activate(ctrl_ptr->rtds_config_ptr); - ctrl_ptr->pump_pwr = 1; next_state = CTRL_STATE_TS_ON; diff --git a/src/SUFST/Src/Services/pm100.c b/src/SUFST/Src/Services/pm100.c index 2b915ba9..131b1a84 100644 --- a/src/SUFST/Src/Services/pm100.c +++ b/src/SUFST/Src/Services/pm100.c @@ -127,18 +127,37 @@ void pm100_thread_entry(ULONG input) pm100_context_t* pm100_ptr = (pm100_context_t*) input; const config_pm100_t* config_ptr = pm100_ptr->config_ptr; - // set up RTCAN subscriptions - uint32_t subscriptions[] = {CAN_C_PM100_INTERNAL_STATES_FRAME_ID, - CAN_C_PM100_FAULT_CODES_FRAME_ID, - CAN_C_PM100_TEMPERATURE_SET_1_FRAME_ID, - CAN_C_PM100_TEMPERATURE_SET_2_FRAME_ID, - CAN_C_PM100_TEMPERATURE_SET_3_FRAME_ID}; - - for (uint32_t i = 0; i < sizeof(subscriptions) / sizeof(subscriptions[0]); + // set up RTCAN CAN C subscriptions + uint32_t can_c_subscriptions[] = {CAN_C_PM100_INTERNAL_STATES_FRAME_ID, + CAN_C_PM100_FAULT_CODES_FRAME_ID, + CAN_C_PM100_TEMPERATURE_SET_1_FRAME_ID, + CAN_C_PM100_TEMPERATURE_SET_2_FRAME_ID, + CAN_C_PM100_TEMPERATURE_SET_3_FRAME_ID}; + + for (uint32_t i = 0; + i < sizeof(can_c_subscriptions) / sizeof(can_c_subscriptions[0]); i++) { rtcan_status_t status = rtcan_subscribe(pm100_ptr->rtcan_c_ptr, - subscriptions[i], + can_c_subscriptions[i], + &pm100_ptr->can_rx_queue); + + if (status != RTCAN_OK) + { + // TODO: update broadcast states with error + tx_thread_terminate(&pm100_ptr->thread); + } + } + + // set up RTCAN CAN S subscriptions + uint32_t can_s_subscriptions[] = {CAN_S_PDM_OUT_VOLTAGE_FRAME_ID}; + + for (uint32_t i = 0; + i < sizeof(can_s_subscriptions) / sizeof(can_s_subscriptions[0]); + i++) + { + rtcan_status_t status = rtcan_subscribe(pm100_ptr->rtcan_s_ptr, + can_s_subscriptions[i], &pm100_ptr->can_rx_queue); if (status != RTCAN_OK) @@ -166,7 +185,6 @@ void pm100_thread_entry(ULONG input) { pm100_ptr->broadcasts_valid = true; process_broadcast(pm100_ptr, msg_ptr); - rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr); } else { @@ -191,6 +209,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) msg_ptr->data, msg_ptr->length); + rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr); + break; } @@ -213,6 +233,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) (void) pm100_disable(pm100_ptr); } + rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr); + break; } @@ -223,6 +245,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) msg_ptr->data, msg_ptr->length); + rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr); + break; } @@ -232,6 +256,8 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) msg_ptr->data, msg_ptr->length); + rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr); + break; } @@ -241,6 +267,19 @@ void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) msg_ptr->data, msg_ptr->length); + rtcan_msg_consumed(pm100_ptr->rtcan_c_ptr, msg_ptr); + + break; + } + + case CAN_S_PDM_OUT_VOLTAGE_FRAME_ID: + { + can_s_pdm_out_voltage_unpack(&pm100_ptr->vout, + msg_ptr->data, + msg_ptr->length); + + rtcan_msg_consumed(pm100_ptr->rtcan_s_ptr, msg_ptr); + break; } @@ -434,3 +473,9 @@ status_t pm100_request_torque(pm100_context_t* pm100_ptr, uint16_t torque) return status; } + +bool pm100_check_pumps_running(pm100_context_t* pm100_ptr) +{ + return (&pm100_ptr->vout.pdm_output_4_voltage > PUMP_RUNNING_THRESHOLD) + && (&pm100_ptr->vout.pdm_output_5_voltage > PUMP_RUNNING_THRESHOLD); +} diff --git a/src/SUFST/Src/config.c b/src/SUFST/Src/config.c index 43218b62..fb79a8b7 100644 --- a/src/SUFST/Src/config.c +++ b/src/SUFST/Src/config.c @@ -91,6 +91,7 @@ static const config_t config_instance = { }, .schedule_ticks = SECONDS_TO_TICKS(0.01), // 100Hz control loop .r2d_requires_brake = true, + .r2d_requires_pump = true, .bps_on_threshold = 5, .apps_bps_low_threshold = 5, .apps_bps_high_threshold = 20,