From cfd1106fb89b5482507cb3d743dc624cf45bf52f Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 15:52:36 +0100 Subject: [PATCH 01/11] feat: disable inverter when speed is low and no torque requested --- src/SUFST/Src/Services/pm100.c | 150 +++++++++++++++++++-------------- 1 file changed, 85 insertions(+), 65 deletions(-) diff --git a/src/SUFST/Src/Services/pm100.c b/src/SUFST/Src/Services/pm100.c index 0a31e95..3c468ae 100644 --- a/src/SUFST/Src/Services/pm100.c +++ b/src/SUFST/Src/Services/pm100.c @@ -3,29 +3,29 @@ #include #include -#define PM100_NO_FAULTS 0x00 +#define PM100_NO_FAULTS 0x00 -#define PM100_VSM_STATE_START 0x00 -#define PM100_VSM_STATE_PRECHARGE_INIT 0x01 -#define PM100_VSM_STATE_PRECHARGE_ACTIVE 0x02 +#define PM100_VSM_STATE_START 0x00 +#define PM100_VSM_STATE_PRECHARGE_INIT 0x01 +#define PM100_VSM_STATE_PRECHARGE_ACTIVE 0x02 #define PM100_VSM_STATE_PRECHARGE_COMPLETE 0x03 -#define PM100_VSM_STATE_WAIT 0x04 -#define PM100_VSM_STATE_READY 0x05 -#define PM100_VSM_STATE_RUNNING 0x06 -#define PM100_VSM_STATE_FAULT 0x07 - -#define PM100_LOCKOUT_DISABLED 0x0 -#define PM100_LOCKOUT_ENABLED 0x1 -#define PM100_CAN_MODE 0x0 -#define PM100_VSM_MODE 0x1 -#define PM100_DIRECTION_REVERSE 0x0 -#define PM100_DIRECTION_FORWARD 0x1 -#define PM100_INVERTER_OFF 0x0 -#define PM100_INVERTER_ON 0x1 -#define PM100_TORQUE_MODE 0x0 -#define PM100_SPEED_MODE 0x1 -#define PM100_SPEED_MODE_DISABLE 0x0 -#define PM100_SPEED_MODE_ENABLE 0x1 +#define PM100_VSM_STATE_WAIT 0x04 +#define PM100_VSM_STATE_READY 0x05 +#define PM100_VSM_STATE_RUNNING 0x06 +#define PM100_VSM_STATE_FAULT 0x07 + +#define PM100_LOCKOUT_DISABLED 0x0 +#define PM100_LOCKOUT_ENABLED 0x1 +#define PM100_CAN_MODE 0x0 +#define PM100_VSM_MODE 0x1 +#define PM100_DIRECTION_REVERSE 0x0 +#define PM100_DIRECTION_FORWARD 0x1 +#define PM100_INVERTER_OFF 0x0 +#define PM100_INVERTER_ON 0x1 +#define PM100_TORQUE_MODE 0x0 +#define PM100_SPEED_MODE 0x1 +#define PM100_SPEED_MODE_DISABLE 0x0 +#define PM100_SPEED_MODE_ENABLE 0x1 #define PM100_DIRECTION_FORWARD 0x1 #define PM100_DIRECTION_REVERSE 0x0 @@ -34,8 +34,8 @@ * internal function prototypes */ static void pm100_thread_entry(ULONG input); -static void process_broadcast(pm100_context_t *pm100_ptr, - const rtcan_msg_t *msg_ptr); +static void process_broadcast(pm100_context_t* pm100_ptr, + const rtcan_msg_t* msg_ptr); /** * @brief Initialises the PM100 service @@ -61,7 +61,7 @@ status_t pm100_init(pm100_context_t* pm100_ptr, status_t status = STATUS_OK; // create service thread - void *stack_ptr = NULL; + void* stack_ptr = NULL; UINT tx_status = tx_byte_allocate(stack_pool_ptr, &stack_ptr, config_ptr->thread.stack_size, @@ -70,9 +70,9 @@ status_t pm100_init(pm100_context_t* pm100_ptr, if (tx_status == TX_SUCCESS) { tx_status = tx_thread_create(&pm100_ptr->thread, - (CHAR *)config_ptr->thread.name, + (CHAR*) config_ptr->thread.name, pm100_thread_entry, - (ULONG)pm100_ptr, + (ULONG) pm100_ptr, stack_ptr, config_ptr->thread.stack_size, config_ptr->thread.priority, @@ -124,17 +124,16 @@ status_t pm100_init(pm100_context_t* pm100_ptr, */ 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; + 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, - CAN_C_PM100_MOTOR_POSITION_INFO_FRAME_ID}; + 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, + CAN_C_PM100_MOTOR_POSITION_INFO_FRAME_ID}; for (uint32_t i = 0; i < sizeof(subscriptions) / sizeof(subscriptions[0]); i++) @@ -146,7 +145,8 @@ void pm100_thread_entry(ULONG input) if (status != RTCAN_OK) { // TODO: update broadcast states with error - LOG_ERROR("Could not subscribe on %d message. Terminating thread\n"); + LOG_ERROR( + "Could not subscribe on %d message. Terminating thread\n"); tx_thread_terminate(&pm100_ptr->thread); } } @@ -154,7 +154,7 @@ void pm100_thread_entry(ULONG input) // process incoming messages, or timeout while (1) { - rtcan_msg_t *msg_ptr = NULL; + rtcan_msg_t* msg_ptr = NULL; UINT status = tx_queue_receive(&pm100_ptr->can_rx_queue, &msg_ptr, config_ptr->broadcast_timeout_ticks); @@ -185,7 +185,7 @@ void pm100_thread_entry(ULONG input) * @param[in] pm100_ptr PM100 context * @param[in] msg_ptr Incoming message */ -void process_broadcast(pm100_context_t *pm100_ptr, const rtcan_msg_t *msg_ptr) +void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) { switch (msg_ptr->identifier) { @@ -204,15 +204,17 @@ void process_broadcast(pm100_context_t *pm100_ptr, const rtcan_msg_t *msg_ptr) msg_ptr->data, msg_ptr->length); - if (pm100_ptr->faults.pm100_run_fault_hi != PM100_NO_FAULTS || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) + if (pm100_ptr->faults.pm100_run_fault_hi != PM100_NO_FAULTS + || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) { pm100_ptr->error |= PM100_ERROR_RUN_FAULT; - (void)pm100_disable(pm100_ptr); + (void) pm100_disable(pm100_ptr); } - else if (pm100_ptr->faults.pm100_post_fault_hi != PM100_NO_FAULTS || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) + else if (pm100_ptr->faults.pm100_post_fault_hi != PM100_NO_FAULTS + || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) { pm100_ptr->error |= PM100_ERROR_POST_FAULT; - (void)pm100_disable(pm100_ptr); + (void) pm100_disable(pm100_ptr); } break; @@ -278,7 +280,7 @@ void process_broadcast(pm100_context_t *pm100_ptr, const rtcan_msg_t *msg_ptr) * * @param[in] pm100_ptr PM100 context */ -status_t pm100_lvs_on(pm100_context_t *pm100_ptr) +status_t pm100_lvs_on(pm100_context_t* pm100_ptr) { // UINT tx_status = tx_thread_resume(&pm100_ptr->thread); @@ -290,10 +292,11 @@ status_t pm100_lvs_on(pm100_context_t *pm100_ptr) * * @param[in] pm100_ptr PM100 context */ -bool pm100_is_precharged(pm100_context_t *pm100_ptr) +bool pm100_is_precharged(pm100_context_t* pm100_ptr) { - UINT tx_status = tx_mutex_get(&pm100_ptr->state_mutex, - pm100_ptr->config_ptr->precharge_timeout_ticks); + UINT tx_status + = tx_mutex_get(&pm100_ptr->state_mutex, + pm100_ptr->config_ptr->precharge_timeout_ticks); bool broadcasts_valid = false; uint8_t vsm_state = PM100_VSM_STATE_FAULT; // assume something safe @@ -302,10 +305,14 @@ bool pm100_is_precharged(pm100_context_t *pm100_ptr) { vsm_state = pm100_ptr->states.pm100_vsm_state; broadcasts_valid = pm100_ptr->broadcasts_valid; - (void)tx_mutex_put(&pm100_ptr->state_mutex); + (void) tx_mutex_put(&pm100_ptr->state_mutex); } - return broadcasts_valid && (vsm_state == PM100_VSM_STATE_PRECHARGE_COMPLETE || vsm_state == PM100_VSM_STATE_WAIT || vsm_state == PM100_VSM_STATE_READY || vsm_state == PM100_VSM_STATE_RUNNING); + return broadcasts_valid + && (vsm_state == PM100_VSM_STATE_PRECHARGE_COMPLETE + || vsm_state == PM100_VSM_STATE_WAIT + || vsm_state == PM100_VSM_STATE_READY + || vsm_state == PM100_VSM_STATE_RUNNING); } int16_t pm100_max_inverter_temp(pm100_context_t* pm100_ptr) @@ -357,7 +364,7 @@ int16_t pm100_motor_temp(pm100_context_t* pm100_ptr) return motor_temp; } -int16_t pm100_motor_speed(pm100_context_t *pm100_ptr) +int16_t pm100_motor_speed(pm100_context_t* pm100_ptr) { int16_t speed = 0; @@ -372,7 +379,7 @@ int16_t pm100_motor_speed(pm100_context_t *pm100_ptr) return speed; } -status_t pm100_lvs_off(pm100_context_t *pm100_ptr) +status_t pm100_lvs_off(pm100_context_t* pm100_ptr) { return STATUS_OK; } @@ -385,7 +392,7 @@ status_t pm100_lvs_off(pm100_context_t *pm100_ptr) * * @param[in] pm100_ptr */ -status_t pm100_disable(pm100_context_t *pm100_ptr) +status_t pm100_disable(pm100_context_t* pm100_ptr) { LOG_INFO("Sending PM100 Disable command\n"); rtcan_msg_t msg = {.identifier = CAN_C_PM100_COMMAND_MESSAGE_FRAME_ID, @@ -412,7 +419,7 @@ status_t pm100_disable(pm100_context_t *pm100_ptr) * @param[in] pm100_ptr PM100 context * @param[in] torque Desired torque */ -status_t pm100_request_torque(pm100_context_t *pm100_ptr, uint16_t torque) +status_t pm100_request_torque(pm100_context_t* pm100_ptr, uint16_t torque) { status_t status = STATUS_OK; @@ -420,29 +427,42 @@ status_t pm100_request_torque(pm100_context_t *pm100_ptr, uint16_t torque) pm100_ptr); // do this before getting mutex to avoid deadlock const bool no_errors = (pm100_ptr->error == PM100_ERROR_NONE); - UINT tx_status = tx_mutex_get(&pm100_ptr->state_mutex, - pm100_ptr->config_ptr->torque_request_timeout_ticks); + UINT tx_status + = tx_mutex_get(&pm100_ptr->state_mutex, + pm100_ptr->config_ptr->torque_request_timeout_ticks); if (no_errors && is_precharged && tx_status == TX_SUCCESS) { if (pm100_ptr->broadcasts_valid) { - if (pm100_ptr->states.pm100_inverter_enable_lockout == PM100_LOCKOUT_DISABLED) + if (pm100_ptr->states.pm100_inverter_enable_lockout + == PM100_LOCKOUT_DISABLED) { - rtcan_msg_t msg = {.identifier = CAN_C_PM100_COMMAND_MESSAGE_FRAME_ID, - .length = CAN_C_PM100_COMMAND_MESSAGE_LENGTH, - .extended = CAN_C_PM100_COMMAND_MESSAGE_IS_EXTENDED, - .data = {0, 0, 0, 0, 0, 0, 0, 0}}; + bool inverter_enable = PM100_INVERTER_ON; + + rtcan_msg_t msg + = {.identifier = CAN_C_PM100_COMMAND_MESSAGE_FRAME_ID, + .length = CAN_C_PM100_COMMAND_MESSAGE_LENGTH, + .extended = CAN_C_PM100_COMMAND_MESSAGE_IS_EXTENDED, + .data = {0, 0, 0, 0, 0, 0, 0, 0}}; + + uint16_t speed = pm100_motor_speed(pm100_ptr); + if (torque == 0 && speed < 10) + { + inverter_enable = PM100_INVERTER_OFF; + } - struct can_c_pm100_command_message_t cmd = {.pm100_torque_command = torque, - .pm100_direction_command = PM100_DIRECTION_REVERSE, - .pm100_speed_mode_enable = PM100_SPEED_MODE_DISABLE, - .pm100_inverter_enable = PM100_INVERTER_ON}; + struct can_c_pm100_command_message_t cmd + = {.pm100_torque_command = torque, + .pm100_direction_command = PM100_DIRECTION_REVERSE, + .pm100_speed_mode_enable = PM100_SPEED_MODE_DISABLE, + .pm100_inverter_enable = inverter_enable}; can_c_pm100_command_message_pack(msg.data, &cmd, msg.length); LOG_INFO("Sending torque request\n"); - rtcan_status_t rtcan_status = rtcan_transmit(pm100_ptr->rtcan_c_ptr, &msg); + rtcan_status_t rtcan_status + = rtcan_transmit(pm100_ptr->rtcan_c_ptr, &msg); status = (rtcan_status == RTCAN_OK) ? STATUS_OK : STATUS_ERROR; } else @@ -453,7 +473,7 @@ status_t pm100_request_torque(pm100_context_t *pm100_ptr, uint16_t torque) } } - (void)tx_mutex_put(&pm100_ptr->state_mutex); + (void) tx_mutex_put(&pm100_ptr->state_mutex); } else { From eae5ce42836812f0f3bff0c82457230523593a9f Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 15:56:32 +0100 Subject: [PATCH 02/11] Feat/high speed torque limit (#261) * Feat: limit torque at high speed * fix: tweak values --- src/SUFST/Inc/Functions/torque_map.h | 7 +- src/SUFST/Inc/config.h | 3 + src/SUFST/Src/Functions/torque_map.c | 82 +- src/SUFST/Src/Services/ctrl.c | 1081 +++++++++++++------------- src/SUFST/Src/config.c | 7 +- 5 files changed, 632 insertions(+), 548 deletions(-) diff --git a/src/SUFST/Inc/Functions/torque_map.h b/src/SUFST/Inc/Functions/torque_map.h index 9c3b268..71df930 100644 --- a/src/SUFST/Inc/Functions/torque_map.h +++ b/src/SUFST/Inc/Functions/torque_map.h @@ -24,6 +24,10 @@ typedef struct _torque_map_t uint16_t deadzone_end; // end of deadzone float deadzone_scale; // scale factor for inputs const config_torque_map_t* config_ptr; // configuration + uint16_t output_max; + uint16_t speed_min; // minimum Torque request at max speed (Nm *10) + uint16_t speed_start; // speed to start limiting torque (rpm) + uint16_t speed_end; // speed for max torque limiting (rpm) } torque_map_t; /* @@ -31,6 +35,7 @@ typedef struct _torque_map_t */ status_t torque_map_init(torque_map_t* map_ptr, const config_torque_map_t* config_ptr); -uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input); +uint16_t +torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed); #endif \ No newline at end of file diff --git a/src/SUFST/Inc/config.h b/src/SUFST/Inc/config.h index 46111ba..6436246 100644 --- a/src/SUFST/Inc/config.h +++ b/src/SUFST/Inc/config.h @@ -116,6 +116,9 @@ typedef struct { uint16_t input_max; // maximum input value (range must be zero to max) uint16_t output_max; // maximum output value (Nm * 10) float deadzone_fraction; // fraction of input range for deadzone + uint16_t speed_min; // minimum Torque request at max speed (Nm *10) + uint16_t speed_start; // speed to start limiting torque (rpm) + uint16_t speed_end; // speed for max torque limiting (rpm) } config_torque_map_t; /** diff --git a/src/SUFST/Src/Functions/torque_map.c b/src/SUFST/Src/Functions/torque_map.c index a25e829..4a30566 100644 --- a/src/SUFST/Src/Functions/torque_map.c +++ b/src/SUFST/Src/Functions/torque_map.c @@ -3,9 +3,11 @@ /* * internal function prototypes */ -static inline uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input); -static uint16_t null_torque_map(torque_map_t *map_ptr, uint16_t input); -static uint16_t linear_torque_map(torque_map_t *map_ptr, uint16_t input); +static inline uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input); +static uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input); +static uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input); +static inline uint16_t +apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed); /** * @brief Initialises the torque map @@ -16,15 +18,18 @@ static uint16_t linear_torque_map(torque_map_t *map_ptr, uint16_t input); * @param[in] map_ptr Torque map * @param[in] config_ptr Configuration */ -status_t torque_map_init(torque_map_t *map_ptr, - const config_torque_map_t *config_ptr) +status_t torque_map_init(torque_map_t* map_ptr, + const config_torque_map_t* config_ptr) { map_ptr->config_ptr = config_ptr; // pre-compute deadzone parameters - map_ptr->deadzone_end = config_ptr->deadzone_fraction * config_ptr->input_max; + map_ptr->deadzone_end + = config_ptr->deadzone_fraction * config_ptr->input_max; - map_ptr->deadzone_scale = ((float)config_ptr->input_max) / ((float)(config_ptr->input_max - map_ptr->deadzone_end)); + map_ptr->deadzone_scale + = ((float) config_ptr->input_max) + / ((float) (config_ptr->input_max - map_ptr->deadzone_end)); // load mapping function status_t status = STATUS_OK; @@ -43,6 +48,10 @@ status_t torque_map_init(torque_map_t *map_ptr, break; }; + map_ptr->speed_min = config_ptr->speed_min; + map_ptr->speed_start = config_ptr->speed_start; + map_ptr->speed_end = config_ptr->speed_end; + return status; } @@ -52,12 +61,12 @@ status_t torque_map_init(torque_map_t *map_ptr, * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t torque_map_apply(torque_map_t *map_ptr, uint16_t input) +uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed) { const uint16_t input_deadzone = apply_deadzone(map_ptr, input); const uint16_t torque = map_ptr->map_func(map_ptr, input_deadzone); - - return torque; + const uint16_t limited_torque = apply_speed_limit(map_ptr, torque, speed); + return limited_torque; } /** @@ -67,7 +76,7 @@ uint16_t torque_map_apply(torque_map_t *map_ptr, uint16_t input) * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input) +uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input) { uint16_t result = 0; @@ -78,7 +87,7 @@ uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input) else { const uint16_t shifted_input = input - map_ptr->deadzone_end; - result = (uint16_t)(shifted_input * map_ptr->deadzone_scale); + result = (uint16_t) (shifted_input * map_ptr->deadzone_scale); } return result; @@ -87,7 +96,7 @@ uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input) /** * @brief A torque map that returns zero */ -uint16_t null_torque_map(torque_map_t *map_ptr, uint16_t input) +uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input) { UNUSED(map_ptr); UNUSED(input); @@ -97,13 +106,54 @@ uint16_t null_torque_map(torque_map_t *map_ptr, uint16_t input) /** * @brief A linear torque map */ -uint16_t linear_torque_map(torque_map_t *map_ptr, uint16_t input) +uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input) { - const float scale_factor = map_ptr->config_ptr->output_max / (float)map_ptr->config_ptr->input_max; + const float scale_factor = map_ptr->config_ptr->output_max + / (float) map_ptr->config_ptr->input_max; - const uint16_t torque = (uint16_t)(input * scale_factor); + const uint16_t torque = (uint16_t) (input * scale_factor); // TODO: clip to range return torque; +} + +uint16_t +apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed) +{ + uint16_t result = 0; + if (speed < map_ptr->speed_start) + { + result = input; + } + else if (speed > map_ptr->speed_end) + { + if (input < map_ptr->speed_min) + { + result = input; + } + else + { + result = map_ptr->speed_min; + } + } + else + { + uint16_t max_torque + = map_ptr->config_ptr->output_max + - (map_ptr->config_ptr->output_max - map_ptr->speed_min) + * (speed - map_ptr->speed_start) + / (map_ptr->speed_end - map_ptr->speed_start); + + if (input < max_torque) + { + result = input; + } + else + { + result = max_torque; + } + } + + return result; } \ No newline at end of file diff --git a/src/SUFST/Src/Services/ctrl.c b/src/SUFST/Src/Services/ctrl.c index b31b3fe..0c6349f 100644 --- a/src/SUFST/Src/Services/ctrl.c +++ b/src/SUFST/Src/Services/ctrl.c @@ -16,11 +16,15 @@ * internal function prototypes */ void ctrl_thread_entry(ULONG input); -void ctrl_state_machine_tick(ctrl_context_t *ctrl_ptr); -void ctrl_update_canbc_states(ctrl_context_t *ctrl_ptr); -void ctrl_handle_ts_fault(ctrl_context_t *ctrl_ptr); -status_t ctrl_get_apps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result); -status_t ctrl_get_bps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result); +void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr); +void ctrl_update_canbc_states(ctrl_context_t* ctrl_ptr); +void ctrl_handle_ts_fault(ctrl_context_t* ctrl_ptr); +status_t ctrl_get_apps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result); +status_t ctrl_get_bps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result); bool ctrl_fan_passed_on_threshold(ctrl_context_t* ctrl_ptr); bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr); @@ -38,80 +42,79 @@ bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr); * @param[in] rtds_config_ptr RTDS configuration * @param[in] torque_map_config_ptr Torque map configuration */ -status_t ctrl_init(ctrl_context_t *ctrl_ptr, - dash_context_t *dash_ptr, - pm100_context_t *pm100_ptr, - tick_context_t *tick_ptr, - remote_ctrl_context_t *remote_ctrl_ptr, - canbc_context_t *canbc_ptr, - TX_BYTE_POOL *stack_pool_ptr, - const config_ctrl_t *config_ptr, - const config_rtds_t *rtds_config_ptr, - const config_torque_map_t *torque_map_config_ptr) +status_t ctrl_init(ctrl_context_t* ctrl_ptr, + dash_context_t* dash_ptr, + pm100_context_t* pm100_ptr, + tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + canbc_context_t* canbc_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_ctrl_t* config_ptr, + const config_rtds_t* rtds_config_ptr, + const config_torque_map_t* torque_map_config_ptr) { - ctrl_ptr->state = CTRL_STATE_TS_BUTTON_WAIT; - ctrl_ptr->dash_ptr = dash_ptr; - ctrl_ptr->pm100_ptr = pm100_ptr; - ctrl_ptr->tick_ptr = tick_ptr; - ctrl_ptr->canbc_ptr = canbc_ptr; - ctrl_ptr->config_ptr = config_ptr; - ctrl_ptr->rtds_config_ptr = rtds_config_ptr; - ctrl_ptr->error = CTRL_ERROR_NONE; - ctrl_ptr->apps_reading = 0; - ctrl_ptr->bps_reading = 0; - ctrl_ptr->sagl_reading = 0; - ctrl_ptr->torque_request = 0; - ctrl_ptr->shdn_reading = 0; - ctrl_ptr->precharge_start = 0; - ctrl_ptr->inverter_pwr = false; - ctrl_ptr->pump_pwr = false; - ctrl_ptr->fan_pwr = false; - ctrl_ptr->remote_ctrl_ptr = remote_ctrl_ptr; - - - // create the thread - void *stack_ptr = NULL; - UINT tx_status = tx_byte_allocate(stack_pool_ptr, - &stack_ptr, - config_ptr->thread.stack_size, - TX_NO_WAIT); - - if (tx_status == TX_SUCCESS) - { - tx_status = tx_thread_create(&ctrl_ptr->thread, - (CHAR *)config_ptr->thread.name, - ctrl_thread_entry, - (ULONG)ctrl_ptr, - stack_ptr, - config_ptr->thread.stack_size, - config_ptr->thread.priority, - config_ptr->thread.priority, - TX_NO_TIME_SLICE, - TX_AUTO_START); - } - - status_t status = (tx_status == TX_SUCCESS) ? STATUS_OK : STATUS_ERROR; - - // initialise the torque map - if (status == STATUS_OK) - { - status = torque_map_init(&ctrl_ptr->torque_map, torque_map_config_ptr); - } - - // make sure TS is disabled - trc_set_ts_on(GPIO_PIN_RESET); - - // check all ok before starting - if (status != STATUS_OK) - { - tx_thread_terminate(&ctrl_ptr->thread); - ctrl_ptr->error |= CTRL_ERROR_INIT; - } - - // send initial state update - ctrl_update_canbc_states(ctrl_ptr); - - return status; + ctrl_ptr->state = CTRL_STATE_TS_BUTTON_WAIT; + ctrl_ptr->dash_ptr = dash_ptr; + ctrl_ptr->pm100_ptr = pm100_ptr; + ctrl_ptr->tick_ptr = tick_ptr; + ctrl_ptr->canbc_ptr = canbc_ptr; + ctrl_ptr->config_ptr = config_ptr; + ctrl_ptr->rtds_config_ptr = rtds_config_ptr; + ctrl_ptr->error = CTRL_ERROR_NONE; + ctrl_ptr->apps_reading = 0; + ctrl_ptr->bps_reading = 0; + ctrl_ptr->sagl_reading = 0; + ctrl_ptr->torque_request = 0; + ctrl_ptr->shdn_reading = 0; + ctrl_ptr->precharge_start = 0; + ctrl_ptr->inverter_pwr = false; + ctrl_ptr->pump_pwr = false; + ctrl_ptr->fan_pwr = false; + ctrl_ptr->remote_ctrl_ptr = remote_ctrl_ptr; + + // create the thread + void* stack_ptr = NULL; + UINT tx_status = tx_byte_allocate(stack_pool_ptr, + &stack_ptr, + config_ptr->thread.stack_size, + TX_NO_WAIT); + + if (tx_status == TX_SUCCESS) + { + tx_status = tx_thread_create(&ctrl_ptr->thread, + (CHAR*) config_ptr->thread.name, + ctrl_thread_entry, + (ULONG) ctrl_ptr, + stack_ptr, + config_ptr->thread.stack_size, + config_ptr->thread.priority, + config_ptr->thread.priority, + TX_NO_TIME_SLICE, + TX_AUTO_START); + } + + status_t status = (tx_status == TX_SUCCESS) ? STATUS_OK : STATUS_ERROR; + + // initialise the torque map + if (status == STATUS_OK) + { + status = torque_map_init(&ctrl_ptr->torque_map, torque_map_config_ptr); + } + + // make sure TS is disabled + trc_set_ts_on(GPIO_PIN_RESET); + + // check all ok before starting + if (status != STATUS_OK) + { + tx_thread_terminate(&ctrl_ptr->thread); + ctrl_ptr->error |= CTRL_ERROR_INIT; + } + + // send initial state update + ctrl_update_canbc_states(ctrl_ptr); + + return status; } /** @@ -121,12 +124,12 @@ status_t ctrl_init(ctrl_context_t *ctrl_ptr, */ void ctrl_thread_entry(ULONG input) { - ctrl_context_t *ctrl_ptr = (ctrl_context_t *)input; + ctrl_context_t* ctrl_ptr = (ctrl_context_t*) input; - while (1) - { - uint32_t start_time = tx_time_get(); - dash_update_buttons(ctrl_ptr->dash_ptr); + while (1) + { + uint32_t start_time = tx_time_get(); + dash_update_buttons(ctrl_ptr->dash_ptr); ctrl_ptr->shdn_reading = trc_ready(); @@ -196,422 +199,438 @@ bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr) * * @param[in] ctrl_ptr Control context */ -void ctrl_state_machine_tick(ctrl_context_t *ctrl_ptr) +void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) { // reduce typing... dash_context_t* dash_ptr = ctrl_ptr->dash_ptr; - remote_ctrl_context_t *remote_ctrl_ptr = ctrl_ptr->remote_ctrl_ptr; + remote_ctrl_context_t* remote_ctrl_ptr = ctrl_ptr->remote_ctrl_ptr; const config_ctrl_t* config_ptr = ctrl_ptr->config_ptr; const uint16_t BPS_ON_THRESH = config_ptr->bps_on_threshold; - ctrl_state_t next_state = ctrl_ptr->state; - - // In simulation mode, the TS and R2D buttons are controlled by the remote control, but the dash is still in effect - #ifdef VCU_SIMULATION_MODE - dash_ptr->tson_flag = dash_ptr->tson_flag || remote_get_ts_on_reading(remote_ctrl_ptr); - dash_ptr->r2d_flag = dash_ptr->r2d_flag || remote_get_r2d_reading(remote_ctrl_ptr); - #endif - - switch (ctrl_ptr->state) - { - - // wait for TS button to be held and released - // then begin activating the TS - case (CTRL_STATE_TS_BUTTON_WAIT): - { - if (dash_ptr->tson_flag) - { - dash_clear_buttons(dash_ptr); - - if (trc_ready()) - { - LOG_INFO("TSON pressed & SHDN closed\n"); - - trc_set_ts_on(GPIO_PIN_SET); - - next_state = CTRL_STATE_WAIT_NEG_AIR; - ctrl_ptr->neg_air_start = tx_time_get(); - } - } - else{ - ctrl_ptr->inverter_pwr = false; // Turn off inverter if TS button is not pressed + ctrl_state_t next_state = ctrl_ptr->state; + +// In simulation mode, the TS and R2D buttons are controlled by the remote +// control, but the dash is still in effect +#ifdef VCU_SIMULATION_MODE + dash_ptr->tson_flag + = dash_ptr->tson_flag || remote_get_ts_on_reading(remote_ctrl_ptr); + dash_ptr->r2d_flag + = dash_ptr->r2d_flag || remote_get_r2d_reading(remote_ctrl_ptr); +#endif + + switch (ctrl_ptr->state) + { + + // wait for TS button to be held and released + // then begin activating the TS + case (CTRL_STATE_TS_BUTTON_WAIT): + { + if (dash_ptr->tson_flag) + { + dash_clear_buttons(dash_ptr); + + if (trc_ready()) + { + LOG_INFO("TSON pressed & SHDN closed\n"); + + trc_set_ts_on(GPIO_PIN_SET); + + next_state = CTRL_STATE_WAIT_NEG_AIR; + ctrl_ptr->neg_air_start = tx_time_get(); + } + } + else + { + ctrl_ptr->inverter_pwr + = false; // Turn off inverter if TS button is not pressed + } + + break; + } + + case (CTRL_STATE_WAIT_NEG_AIR): + { + if (tx_time_get() + >= ctrl_ptr->neg_air_start + TX_TIMER_TICKS_PER_SECOND / 4) + { + LOG_INFO("Neg AIR closed, turning on inverter\n"); + + ctrl_ptr->inverter_pwr = true; + + next_state = CTRL_STATE_PRECHARGE_WAIT; + ctrl_ptr->precharge_start = tx_time_get(); } - break; - } - - case (CTRL_STATE_WAIT_NEG_AIR): - { - if (tx_time_get() >= ctrl_ptr->neg_air_start + TX_TIMER_TICKS_PER_SECOND / 4) - { - LOG_INFO("Neg AIR closed, turning on inverter\n"); - - ctrl_ptr->inverter_pwr = true; - - next_state = CTRL_STATE_PRECHARGE_WAIT; - ctrl_ptr->precharge_start = tx_time_get(); - } - - break; - } - - // TS is ready, can initiate pre-charge sequence - // TS on LED turns solid - 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)) - { - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_TS_ON; - #else - next_state = CTRL_STATE_R2D_WAIT; - #endif - dash_clear_buttons(dash_ptr); - LOG_INFO("Precharge complete\n"); - } - else if (charge_time >= config_ptr->precharge_timeout_ticks) - { - ctrl_ptr->error |= CTRL_ERROR_PRECHARGE_TIMEOUT; - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - LOG_ERROR("Precharge timeout reached\n"); - } - - break; - } - - // pre-charge is complete, wait for R2D signal - // also wait for brake to be fully pressed (if enabled) - case (CTRL_STATE_R2D_WAIT): - { - if (!trc_ready()) - { - LOG_ERROR("SHDN opened\n"); - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - } - else if (dash_ptr->tson_flag) // TSON pressed, disable TS - { - dash_ptr->tson_flag = false; - - ctrl_ptr->inverter_pwr = false; // Turn off inverter - trc_set_ts_on(GPIO_PIN_RESET); // Turn off AIRs - - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_TS_OFF; - #else - next_state = CTRL_STATE_TS_BUTTON_WAIT; - #endif - } - else if (dash_ptr->r2d_flag) // R2D pressed - { - #ifndef VCU_SIMULATION_MODE - dash_ptr->r2d_flag = false; - #endif - - status_t result = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - bool r2d = false; - - if (result == STATUS_OK) - { - r2d = (config_ptr->r2d_requires_brake) ? (ctrl_ptr->bps_reading > BPS_ON_THRESH) : 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; - - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_ON; - #else - next_state = CTRL_STATE_TS_ON; - #endif - LOG_INFO("R2D active\n"); - } - } - else - { - LOG_ERROR("BPS reading failed\n"); - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - } - } - break; - } - - // the TS is on - case (CTRL_STATE_TS_ON): - { - // read from the APPS - status_t pm100_status; - - status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->apps_reading); - status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - if (dash_ptr->r2d_flag) - { - dash_clear_buttons(dash_ptr); - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; - #else - next_state = CTRL_STATE_R2D_OFF; - #endif - } - else if (apps_status == STATUS_OK && bps_status == STATUS_OK) - { - // Check for brake + accel pedal pressed - if (ctrl_ptr->apps_reading >= - ctrl_ptr->config_ptr->apps_bps_high_threshold && - ctrl_ptr->bps_reading > BPS_ON_THRESH) - { - LOG_ERROR("BP and AP pressed\n"); - - if (tx_time_get() >= ctrl_ptr->apps_bps_start + - (TX_TIMER_TICKS_PER_SECOND / 3)) - { - LOG_ERROR("BP-AP fault\n"); - // next_state = CTRL_STATE_APPS_BPS_FAULT; - } - } - else - { - ctrl_ptr->apps_bps_start = tx_time_get(); - } - #ifdef VCU_SIMULATION_MODE - #ifndef VCU_SIMULATION_ON_POWER - ctrl_ptr->torque_request = remote_get_torque_reading(remote_ctrl_ptr); - #else - uint16_t power = remote_get_power_reading(remote_ctrl_ptr); - - int16_t motor_speed = pm100_motor_speed(ctrl_ptr->pm100_ptr); - uint16_t rad_s = 1; - - // this if to be removed - if (motor_speed < 10) - { - motor_speed = 10; - } - // rpm to rad/s - rad_s = (uint16_t)(motor_speed * 0.10472); - if (rad_s == 0) - rad_s = 1; - ctrl_ptr->torque_request = (uint16_t)(power / rad_s); - if (ctrl_ptr->torque_request > 1500) - ctrl_ptr->torque_request = 1500; - #endif - #else - ctrl_ptr->torque_request = torque_map_apply(&ctrl_ptr->torque_map, - ctrl_ptr->apps_reading); - #endif - - LOG_INFO("ADC: %d, Torque: %d\n", - ctrl_ptr->apps_reading, ctrl_ptr->torque_request); - - pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, - ctrl_ptr->torque_request); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - } - else - { - LOG_ERROR("APPS / BPS fault\n"); - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - break; - } - - case CTRL_STATE_R2D_OFF: - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - ctrl_ptr->motor_torque_zero_start = tx_time_get(); - ctrl_ptr->pump_pwr = 0; - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - else - { - next_state = CTRL_STATE_R2D_OFF_WAIT; - } - break; - } - - case CTRL_STATE_R2D_OFF_WAIT: - { + break; + } + + // TS is ready, can initiate pre-charge sequence + // TS on LED turns solid + 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)) + { +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_TS_ON; +#else + next_state = CTRL_STATE_R2D_WAIT; +#endif + dash_clear_buttons(dash_ptr); + LOG_INFO("Precharge complete\n"); + } + else if (charge_time >= config_ptr->precharge_timeout_ticks) + { + ctrl_ptr->error |= CTRL_ERROR_PRECHARGE_TIMEOUT; + next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; + LOG_ERROR("Precharge timeout reached\n"); + } + + break; + } + + // pre-charge is complete, wait for R2D signal + // also wait for brake to be fully pressed (if enabled) + case (CTRL_STATE_R2D_WAIT): + { + if (!trc_ready()) + { + LOG_ERROR("SHDN opened\n"); + next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; + } + else if (dash_ptr->tson_flag) // TSON pressed, disable TS + { + dash_ptr->tson_flag = false; + + ctrl_ptr->inverter_pwr = false; // Turn off inverter + trc_set_ts_on(GPIO_PIN_RESET); // Turn off AIRs + +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_TS_OFF; +#else + next_state = CTRL_STATE_TS_BUTTON_WAIT; +#endif + } + else if (dash_ptr->r2d_flag) // R2D pressed + { +#ifndef VCU_SIMULATION_MODE + dash_ptr->r2d_flag = false; +#endif + + status_t result = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->bps_reading); + + bool r2d = false; + + if (result == STATUS_OK) + { + r2d = (config_ptr->r2d_requires_brake) + ? (ctrl_ptr->bps_reading > BPS_ON_THRESH) + : 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; + +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_R2D_ON; +#else + next_state = CTRL_STATE_TS_ON; +#endif + LOG_INFO("R2D active\n"); + } + } + else + { + LOG_ERROR("BPS reading failed\n"); + next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; + } + } + break; + } + + // the TS is on + case (CTRL_STATE_TS_ON): + { + // read from the APPS + status_t pm100_status; + + status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->apps_reading); + status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->bps_reading); + + if (dash_ptr->r2d_flag) + { + dash_clear_buttons(dash_ptr); +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; +#else + next_state = CTRL_STATE_R2D_OFF; +#endif + } + else if (apps_status == STATUS_OK && bps_status == STATUS_OK) + { + // Check for brake + accel pedal pressed + if (ctrl_ptr->apps_reading + >= ctrl_ptr->config_ptr->apps_bps_high_threshold + && ctrl_ptr->bps_reading > BPS_ON_THRESH) + { + LOG_ERROR("BP and AP pressed\n"); + + if (tx_time_get() >= ctrl_ptr->apps_bps_start + + (TX_TIMER_TICKS_PER_SECOND / 3)) + { + LOG_ERROR("BP-AP fault\n"); + // next_state = CTRL_STATE_APPS_BPS_FAULT; + } + } + else + { + ctrl_ptr->apps_bps_start = tx_time_get(); + } + + int16_t motor_speed = pm100_motor_speed(ctrl_ptr->pm100_ptr); + +#ifdef VCU_SIMULATION_MODE +#ifndef VCU_SIMULATION_ON_POWER + ctrl_ptr->torque_request + = remote_get_torque_reading(remote_ctrl_ptr); +#else + uint16_t power = remote_get_power_reading(remote_ctrl_ptr); + + uint16_t rad_s = 1; + + // this if to be removed + if (motor_speed < 10) + { + motor_speed = 10; + } + // rpm to rad/s + rad_s = (uint16_t) (motor_speed * 0.10472); + if (rad_s == 0) + rad_s = 1; + ctrl_ptr->torque_request = (uint16_t) (power / rad_s); + if (ctrl_ptr->torque_request > 1500) + ctrl_ptr->torque_request = 1500; +#endif +#else + ctrl_ptr->torque_request = torque_map_apply(&ctrl_ptr->torque_map, + ctrl_ptr->apps_reading, + motor_speed); +#endif + + LOG_INFO("ADC: %d, Torque: %d\n", + ctrl_ptr->apps_reading, + ctrl_ptr->torque_request); + + pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, + ctrl_ptr->torque_request); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + } + else + { + LOG_ERROR("APPS / BPS fault\n"); + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + break; + } + + case CTRL_STATE_R2D_OFF: + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + ctrl_ptr->motor_torque_zero_start = tx_time_get(); + ctrl_ptr->pump_pwr = 0; + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + else + { + next_state = CTRL_STATE_R2D_OFF_WAIT; + } + break; + } + + case CTRL_STATE_R2D_OFF_WAIT: + { dash_set_r2d_led_state(dash_ptr, GPIO_PIN_RESET); - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - else if (tx_time_get() >= ctrl_ptr->motor_torque_zero_start + - TX_TIMER_TICKS_PER_SECOND / 2) - { - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; - #else - next_state = CTRL_STATE_R2D_WAIT; - #endif - - } - break; - } - - // activation or runtime failure - case (CTRL_STATE_TS_ACTIVATION_FAILURE): - case (CTRL_STATE_TS_RUN_FAULT): - { - LOG_ERROR("TS fault during activation or runtime\n"); - ctrl_handle_ts_fault(ctrl_ptr); - next_state = CTRL_STATE_TS_BUTTON_WAIT; - break; - } - - case (CTRL_STATE_SPIN): // Spin forever - { - break; - } - - // SCS fault - // this is recoverable, if the signal becomes plausible again - case (CTRL_STATE_APPS_SCS_FAULT): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - if (ctrl_get_apps_reading(ctrl_ptr->tick_ptr, remote_ctrl_ptr, &ctrl_ptr->apps_reading) == STATUS_OK) - { - next_state = CTRL_STATE_TS_ON; - } - - break; - } - - case (CTRL_STATE_APPS_BPS_FAULT): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->apps_reading); - status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - if (apps_status == STATUS_OK && bps_status == STATUS_OK) - { - if ((ctrl_ptr->apps_reading < ctrl_ptr->config_ptr->apps_bps_low_threshold) && - (ctrl_ptr->bps_reading < BPS_ON_THRESH)) - { - next_state = CTRL_STATE_TS_ON; - } - } - else - { - next_state = CTRL_STATE_APPS_SCS_FAULT; - } - - break; - } - - // Needed when in simulation mode to avoid the TS being turned on again - case (CTRL_STATE_SIM_WAIT_TS_OFF): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - if (!dash_ptr->tson_flag) - { - next_state = CTRL_STATE_TS_BUTTON_WAIT; - } - break; - } - - // Needed when in simulation mode to avoid the TS being turned off again - case (CTRL_STATE_SIM_WAIT_TS_ON): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - if (!dash_ptr->tson_flag) - { - next_state = CTRL_STATE_R2D_WAIT; - } - break; - } - - // Needed when in simulation mode to avoid the R2D being turned off again - case (CTRL_STATE_SIM_WAIT_R2D_ON): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - if (!dash_ptr->r2d_flag) - { - next_state = CTRL_STATE_TS_ON; - } - break; - } - - // Needed when in simulation mode to avoid the R2D being turned on again - case (CTRL_STATE_SIM_WAIT_R2D_OFF): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - if (!dash_ptr->r2d_flag) - { - dash_set_r2d_led_state(dash_ptr, GPIO_PIN_RESET); - next_state = CTRL_STATE_R2D_WAIT; - } - break; - } - - default: - break; - } - - ctrl_ptr->state = next_state; + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + else if (tx_time_get() >= ctrl_ptr->motor_torque_zero_start + + TX_TIMER_TICKS_PER_SECOND / 2) + { +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; +#else + next_state = CTRL_STATE_R2D_WAIT; +#endif + } + break; + } + + // activation or runtime failure + case (CTRL_STATE_TS_ACTIVATION_FAILURE): + case (CTRL_STATE_TS_RUN_FAULT): + { + LOG_ERROR("TS fault during activation or runtime\n"); + ctrl_handle_ts_fault(ctrl_ptr); + next_state = CTRL_STATE_TS_BUTTON_WAIT; + break; + } + + case (CTRL_STATE_SPIN): // Spin forever + { + break; + } + + // SCS fault + // this is recoverable, if the signal becomes plausible again + case (CTRL_STATE_APPS_SCS_FAULT): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + if (ctrl_get_apps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->apps_reading) + == STATUS_OK) + { + next_state = CTRL_STATE_TS_ON; + } + + break; + } + + case (CTRL_STATE_APPS_BPS_FAULT): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->apps_reading); + status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->bps_reading); + + if (apps_status == STATUS_OK && bps_status == STATUS_OK) + { + if ((ctrl_ptr->apps_reading + < ctrl_ptr->config_ptr->apps_bps_low_threshold) + && (ctrl_ptr->bps_reading < BPS_ON_THRESH)) + { + next_state = CTRL_STATE_TS_ON; + } + } + else + { + next_state = CTRL_STATE_APPS_SCS_FAULT; + } + + break; + } + + // Needed when in simulation mode to avoid the TS being turned on again + case (CTRL_STATE_SIM_WAIT_TS_OFF): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + if (!dash_ptr->tson_flag) + { + next_state = CTRL_STATE_TS_BUTTON_WAIT; + } + break; + } + + // Needed when in simulation mode to avoid the TS being turned off again + case (CTRL_STATE_SIM_WAIT_TS_ON): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + if (!dash_ptr->tson_flag) + { + next_state = CTRL_STATE_R2D_WAIT; + } + break; + } + + // Needed when in simulation mode to avoid the R2D being turned off again + case (CTRL_STATE_SIM_WAIT_R2D_ON): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + if (!dash_ptr->r2d_flag) + { + next_state = CTRL_STATE_TS_ON; + } + break; + } + + // Needed when in simulation mode to avoid the R2D being turned on again + case (CTRL_STATE_SIM_WAIT_R2D_OFF): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + if (!dash_ptr->r2d_flag) + { + dash_set_r2d_led_state(dash_ptr, GPIO_PIN_RESET); + next_state = CTRL_STATE_R2D_WAIT; + } + break; + } + + default: + break; + } + + ctrl_ptr->state = next_state; } /** @@ -620,19 +639,19 @@ void ctrl_state_machine_tick(ctrl_context_t *ctrl_ptr) * * @param[in] ctrl_ptr Control context */ -void ctrl_handle_ts_fault(ctrl_context_t *ctrl_ptr) +void ctrl_handle_ts_fault(ctrl_context_t* ctrl_ptr) { - dash_context_t *dash_ptr = ctrl_ptr->dash_ptr; - const config_ctrl_t *config_ptr = ctrl_ptr->config_ptr; + dash_context_t* dash_ptr = ctrl_ptr->dash_ptr; + const config_ctrl_t* config_ptr = ctrl_ptr->config_ptr; - // pm100_lvs_off(ctrl_ptr->pm100_ptr); - ctrl_ptr->inverter_pwr = false; - ctrl_ptr->pump_pwr = false; - ctrl_ptr->fan_pwr = false; + // pm100_lvs_off(ctrl_ptr->pm100_ptr); + ctrl_ptr->inverter_pwr = false; + ctrl_ptr->pump_pwr = false; + ctrl_ptr->fan_pwr = false; - trc_set_ts_on(GPIO_PIN_RESET); - dash_blink_ts_on_led(dash_ptr, config_ptr->error_led_toggle_ticks); - ctrl_update_canbc_states(ctrl_ptr); + trc_set_ts_on(GPIO_PIN_RESET); + dash_blink_ts_on_led(dash_ptr, config_ptr->error_led_toggle_ticks); + ctrl_update_canbc_states(ctrl_ptr); } /** @@ -640,39 +659,43 @@ void ctrl_handle_ts_fault(ctrl_context_t *ctrl_ptr) * * @param[in] ctrl_ptr */ -void ctrl_update_canbc_states(ctrl_context_t *ctrl_ptr) +void ctrl_update_canbc_states(ctrl_context_t* ctrl_ptr) { - canbc_states_t *states = canbc_lock_state(ctrl_ptr->canbc_ptr, TX_NO_WAIT); - - if (states != NULL) - { - // TODO: add ready to drive state? - states->sensors.vcu_sagl = ctrl_ptr->sagl_reading; - states->sensors.vcu_torque_request = ctrl_ptr->torque_request; - states->temps.vcu_max_temp = (int8_t) ctrl_ptr->max_temp; - states->state.vcu_ctrl_state = (uint8_t)ctrl_ptr->state; - states->state.vcu_drs_active = ctrl_ptr->shdn_reading; - states->errors.vcu_ctrl_error = ctrl_ptr->error; - states->pdm.inverter = ctrl_ptr->inverter_pwr; - states->pdm.pump = ctrl_ptr->pump_pwr; - states->pdm.fan = ctrl_ptr->fan_pwr; - canbc_unlock_state(ctrl_ptr->canbc_ptr); - } + canbc_states_t* states = canbc_lock_state(ctrl_ptr->canbc_ptr, TX_NO_WAIT); + + if (states != NULL) + { + // TODO: add ready to drive state? + states->sensors.vcu_sagl = ctrl_ptr->sagl_reading; + states->sensors.vcu_torque_request = ctrl_ptr->torque_request; + states->temps.vcu_max_temp = (int8_t) ctrl_ptr->max_temp; + states->state.vcu_ctrl_state = (uint8_t) ctrl_ptr->state; + states->state.vcu_drs_active = ctrl_ptr->shdn_reading; + states->errors.vcu_ctrl_error = ctrl_ptr->error; + states->pdm.inverter = ctrl_ptr->inverter_pwr; + states->pdm.pump = ctrl_ptr->pump_pwr; + states->pdm.fan = ctrl_ptr->fan_pwr; + canbc_unlock_state(ctrl_ptr->canbc_ptr); + } } -status_t ctrl_get_apps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result) +status_t ctrl_get_apps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result) { - #ifdef VCU_SIMULATION_MODE - return remote_get_apps_reading(remote_ctrl_ptr, result); - #else - return tick_get_apps_reading(tick_ptr, result); - #endif +#ifdef VCU_SIMULATION_MODE + return remote_get_apps_reading(remote_ctrl_ptr, result); +#else + return tick_get_apps_reading(tick_ptr, result); +#endif } -status_t ctrl_get_bps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result) +status_t ctrl_get_bps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result) { - #ifdef VCU_SIMULATION_MODE - return remote_get_bps_reading(remote_ctrl_ptr, result); - #else - return tick_get_bps_reading(tick_ptr, result); - #endif +#ifdef VCU_SIMULATION_MODE + return remote_get_bps_reading(remote_ctrl_ptr, result); +#else + return tick_get_bps_reading(tick_ptr, result); +#endif } \ No newline at end of file diff --git a/src/SUFST/Src/config.c b/src/SUFST/Src/config.c index f97d3b1..23b4d02 100644 --- a/src/SUFST/Src/config.c +++ b/src/SUFST/Src/config.c @@ -110,8 +110,11 @@ static const config_t config_instance = { .torque_map = { .function = TORQUE_MAP_LINEAR, .input_max = 100, - .output_max = 500, - .deadzone_fraction = 0.28f + .output_max = 1000, + .deadzone_fraction = 0.28f, + .speed_min = 500, + .speed_start = 2000, + .speed_end = 3000 }, .pm100 = { .thread = { From 5e5066e153c987f1049d4bd4805f952a350d8767 Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 17:02:41 +0100 Subject: [PATCH 03/11] style: run trunk --- src/SUFST/Src/Interfaces/bps.c | 12 ++++++---- src/SUFST/Src/Services/tick.c | 43 +++++++++++++++++----------------- 2 files changed, 29 insertions(+), 26 deletions(-) diff --git a/src/SUFST/Src/Interfaces/bps.c b/src/SUFST/Src/Interfaces/bps.c index 684793a..017e4c2 100644 --- a/src/SUFST/Src/Interfaces/bps.c +++ b/src/SUFST/Src/Interfaces/bps.c @@ -6,16 +6,18 @@ * @param[in] bps_ptr BPS context * @param[in] config_ptr Configuration */ -status_t bps_init(bps_context_t *bps_ptr, const config_bps_t *config_ptr) +status_t bps_init(bps_context_t* bps_ptr, const config_bps_t* config_ptr) { bps_ptr->config_ptr = config_ptr; // compute pressure thresholds - const uint16_t range = config_ptr->scs.max_mapped - config_ptr->scs.min_mapped; + const uint16_t range + = config_ptr->scs.max_mapped - config_ptr->scs.min_mapped; const uint16_t offset = config_ptr->scs.min_mapped; - bps_ptr->fully_pressed_threshold = (config_ptr->fully_pressed_fraction * range) + offset; + bps_ptr->fully_pressed_threshold + = (config_ptr->fully_pressed_fraction * range) + offset; // create the SCS instance status_t status = scs_create(&bps_ptr->signal, &config_ptr->scs); @@ -29,7 +31,7 @@ status_t bps_init(bps_context_t *bps_ptr, const config_bps_t *config_ptr) * @param[in] bps_ptr * @param[out] reading_ptr */ -status_t bps_read(bps_context_t *bps_ptr, uint16_t *reading_ptr) +status_t bps_read(bps_context_t* bps_ptr, uint16_t* reading_ptr) { status_t status = scs_read(&bps_ptr->signal, reading_ptr); scs_status_t status_verbose = bps_ptr->signal.status_verbose; @@ -64,7 +66,7 @@ status_t bps_read(bps_context_t *bps_ptr, uint16_t *reading_ptr) * @retval true BPS is fully pressed * @retval false BPS not fully pressed, or SCS fault */ -bool bps_fully_pressed(bps_context_t *bps_ptr) +bool bps_fully_pressed(bps_context_t* bps_ptr) { uint16_t reading = 0; status_t status = bps_read(bps_ptr, &reading); diff --git a/src/SUFST/Src/Services/tick.c b/src/SUFST/Src/Services/tick.c index 87598b4..71fda99 100644 --- a/src/SUFST/Src/Services/tick.c +++ b/src/SUFST/Src/Services/tick.c @@ -2,14 +2,14 @@ #define BPS_LIGHT_THRESH 40 -static status_t lock_tick_sensors(tick_context_t *tick_ptr, uint32_t timeout); -static void unlock_tick_sensors(tick_context_t *tick_ptr); -void tick_update_canbc_states(tick_context_t *tick_ptr); +static status_t lock_tick_sensors(tick_context_t* tick_ptr, uint32_t timeout); +static void unlock_tick_sensors(tick_context_t* tick_ptr); +void tick_update_canbc_states(tick_context_t* tick_ptr); static void tick_thread_entry(ULONG input) { - tick_context_t *tick_ptr = (tick_context_t *)input; - const config_tick_t *config_ptr = tick_ptr->config_ptr; + tick_context_t* tick_ptr = (tick_context_t*) input; + const config_tick_t* config_ptr = tick_ptr->config_ptr; while (1) { @@ -17,7 +17,8 @@ static void tick_thread_entry(ULONG input) tick_ptr->bps_status = bps_read(&tick_ptr->bps, &tick_ptr->bps_reading); tick_ptr->brakelight_pwr = (tick_ptr->bps_reading > BPS_LIGHT_THRESH); - tick_ptr->apps_status = apps_read(&tick_ptr->apps, &tick_ptr->apps_reading); + tick_ptr->apps_status + = apps_read(&tick_ptr->apps, &tick_ptr->apps_reading); /*LOG_INFO(tick_ptr->log_ptr, "Brake pressure: %d status: %d\n", tick_ptr->bps_reading, tick_ptr->bps_status);*/ @@ -32,12 +33,12 @@ static void tick_thread_entry(ULONG input) } } -status_t tick_init(tick_context_t *tick_ptr, - canbc_context_t *canbc_ptr, - TX_BYTE_POOL *stack_pool_ptr, - const config_tick_t *config_ptr, - const config_apps_t *apps_config_ptr, - const config_bps_t *bps_config_ptr) +status_t tick_init(tick_context_t* tick_ptr, + canbc_context_t* canbc_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_tick_t* config_ptr, + const config_apps_t* apps_config_ptr, + const config_bps_t* bps_config_ptr) { tick_ptr->config_ptr = config_ptr; tick_ptr->canbc_ptr = canbc_ptr; @@ -48,7 +49,7 @@ status_t tick_init(tick_context_t *tick_ptr, status_t status = STATUS_OK; - void *stack_ptr = NULL; + void* stack_ptr = NULL; UINT tx_status = tx_byte_allocate(stack_pool_ptr, &stack_ptr, config_ptr->thread.stack_size, @@ -72,9 +73,9 @@ status_t tick_init(tick_context_t *tick_ptr, if (tx_status == TX_SUCCESS) { tx_status = tx_thread_create(&tick_ptr->thread, - (CHAR *)config_ptr->thread.name, + (CHAR*) config_ptr->thread.name, tick_thread_entry, - (ULONG)tick_ptr, + (ULONG) tick_ptr, stack_ptr, config_ptr->thread.stack_size, config_ptr->thread.priority, @@ -96,9 +97,9 @@ status_t tick_init(tick_context_t *tick_ptr, return status; } -void tick_update_canbc_states(tick_context_t *tick_ptr) +void tick_update_canbc_states(tick_context_t* tick_ptr) { - canbc_states_t *states = canbc_lock_state(tick_ptr->canbc_ptr, TX_NO_WAIT); + canbc_states_t* states = canbc_lock_state(tick_ptr->canbc_ptr, TX_NO_WAIT); if (states != NULL) { @@ -109,7 +110,7 @@ void tick_update_canbc_states(tick_context_t *tick_ptr) } } -static status_t lock_tick_sensors(tick_context_t *tick_ptr, uint32_t timeout) +static status_t lock_tick_sensors(tick_context_t* tick_ptr, uint32_t timeout) { UINT tx_status = tx_mutex_get(&tick_ptr->sensor_mutex, timeout); @@ -123,12 +124,12 @@ static status_t lock_tick_sensors(tick_context_t *tick_ptr, uint32_t timeout) } } -static void unlock_tick_sensors(tick_context_t *tick_ptr) +static void unlock_tick_sensors(tick_context_t* tick_ptr) { tx_mutex_put(&tick_ptr->sensor_mutex); } -status_t tick_get_bps_reading(tick_context_t *tick_ptr, uint16_t *result) +status_t tick_get_bps_reading(tick_context_t* tick_ptr, uint16_t* result) { status_t status = STATUS_ERROR; @@ -146,7 +147,7 @@ status_t tick_get_bps_reading(tick_context_t *tick_ptr, uint16_t *result) return status; } -status_t tick_get_apps_reading(tick_context_t *tick_ptr, uint16_t *result) +status_t tick_get_apps_reading(tick_context_t* tick_ptr, uint16_t* result) { status_t status = STATUS_ERROR; From efcfb7fe48211cf4086f05c5f63439e3f062e867 Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 18:25:05 +0100 Subject: [PATCH 04/11] feat: add SOC, BMS temp and pump can-defs --- src/Middlewares/SUFST/can-defs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Middlewares/SUFST/can-defs b/src/Middlewares/SUFST/can-defs index 9528a40..db6f470 160000 --- a/src/Middlewares/SUFST/can-defs +++ b/src/Middlewares/SUFST/can-defs @@ -1 +1 @@ -Subproject commit 9528a405d4c66ba1ee01a016f98143db85247724 +Subproject commit db6f470da0fb577b4c1306e32ed61f6266e57428 From f497eeace3c86d929efd90c2118d0a5948f41363 Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 21:41:50 +0100 Subject: [PATCH 05/11] feat: add canrx service --- Makefile | 1 + src/SUFST/Inc/Services/canrx.h | 56 ++ src/SUFST/Inc/Services/ctrl.h | 120 ++-- src/SUFST/Inc/config.h | 8 + src/SUFST/Inc/vcu.h | 30 +- src/SUFST/Src/Interfaces/bps.c | 12 +- src/SUFST/Src/Services/canrx.c | 224 +++++++ src/SUFST/Src/Services/ctrl.c | 1081 ++++++++++++++++---------------- src/SUFST/Src/Services/pm100.c | 142 +++-- src/SUFST/Src/Services/tick.c | 43 +- src/SUFST/Src/config.c | 8 + src/SUFST/Src/vcu.c | 52 +- 12 files changed, 1065 insertions(+), 712 deletions(-) create mode 100644 src/SUFST/Inc/Services/canrx.h create mode 100644 src/SUFST/Src/Services/canrx.c diff --git a/Makefile b/Makefile index 17f2e38..ff47fe2 100644 --- a/Makefile +++ b/Makefile @@ -101,6 +101,7 @@ src/SUFST/Src/Interfaces/rtds.c \ src/SUFST/Src/Interfaces/scs.c \ src/SUFST/Src/Interfaces/trc.c \ src/SUFST/Src/Services/canbc.c \ +src/SUFST/Src/Services/canrx.c \ src/SUFST/Src/Services/ctrl.c \ src/SUFST/Src/Services/remote_ctrl.c \ src/SUFST/Src/Services/dash.c \ diff --git a/src/SUFST/Inc/Services/canrx.h b/src/SUFST/Inc/Services/canrx.h new file mode 100644 index 0000000..c3e812a --- /dev/null +++ b/src/SUFST/Inc/Services/canrx.h @@ -0,0 +1,56 @@ +/****************************************************************************** + * @file canrx.h + * @author Robert Kirkbride (@r-kirkbride, rgak1g24@soton.ac.uk) + * @brief Can Listener Service + * @details This service is responsible for listening for CAN messages required + * by other services + *****************************************************************************/ + +#ifndef CANRX_H +#define CANRX_H + +#include +#include +#include +#include + +#include "config.h" +#include "log.h" +#include "status.h" + +#define CANRX_QUEUE_SIZE 10 // 10 items + +#define CANRX_ERROR_NONE 0x00 // no errors +#define CANRX_ERROR_INIT 0x01 // initialisation error +#define CANRX_ERROR_BROADCAST_TIMEOUT 0x02 // no broadcasts received +#define CANRX_ERROR_POST_FAULT 0x04 // power-on self-test fault +#define CANRX_ERROR_RUN_FAULT 0x08 // runtime fault + +/** + * @brief CANRX context + */ +typedef struct +{ + TX_THREAD thread; + rtcan_handle_t* rtcan_c_ptr; + rtcan_handle_t* rtcan_s_ptr; + TX_QUEUE can_c_rx_queue; + TX_QUEUE can_s_rx_queue; + ULONG can_rx_queue_mem[CANRX_QUEUE_SIZE]; + TX_MUTEX state_mutex; + bool broadcasts_valid; + struct can_s_msgid_0_x202_t msgid_x202; + uint16_t error; + const config_canrx_t* config_ptr; +} canrx_context_t; + +/* + * public functions + */ +status_t canrx_init(canrx_context_t* canrx_ptr, + rtcan_handle_t* rtcan_c_ptr, + rtcan_handle_t* rtcan_s_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_canrx_t* config_ptr); + +#endif diff --git a/src/SUFST/Inc/Services/ctrl.h b/src/SUFST/Inc/Services/ctrl.h index b989c3f..0e285ac 100644 --- a/src/SUFST/Inc/Services/ctrl.h +++ b/src/SUFST/Inc/Services/ctrl.h @@ -15,23 +15,24 @@ #include "apps.h" #include "bps.h" #include "canbc.h" +#include "canrx.h" #include "config.h" #include "dash.h" #include "log.h" #include "pm100.h" +#include "remote_ctrl.h" #include "status.h" #include "tick.h" #include "torque_map.h" -#include "remote_ctrl.h" /* * error codes */ -#define CTRL_ERROR_NONE 0x00 -#define CTRL_ERROR_INIT 0x01 // service failed to initialise -#define CTRL_ERROR_TS_READY_TIMEOUT 0x02 // TS ready from TRC timed out -#define CTRL_ERROR_PRECHARGE_TIMEOUT 0x04 // precharge timed out -#define CTRL_ERROR_TRC_RUN_FAULT 0x08 // TRC faulted at runtime +#define CTRL_ERROR_NONE 0x00 +#define CTRL_ERROR_INIT 0x01 // service failed to initialise +#define CTRL_ERROR_TS_READY_TIMEOUT 0x02 // TS ready from TRC timed out +#define CTRL_ERROR_PRECHARGE_TIMEOUT 0x04 // precharge timed out +#define CTRL_ERROR_TRC_RUN_FAULT 0x08 // TRC faulted at runtime #define CTRL_ERROR_INVERTER_RUN_FAULT 0x10 // inverter faulted at runtime /** @@ -39,22 +40,22 @@ */ typedef enum { - CTRL_STATE_TS_BUTTON_WAIT, - CTRL_STATE_WAIT_NEG_AIR, - CTRL_STATE_PRECHARGE_WAIT, - CTRL_STATE_R2D_WAIT, - CTRL_STATE_TS_ON, - CTRL_STATE_R2D_OFF, - CTRL_STATE_R2D_OFF_WAIT, - CTRL_STATE_TS_ACTIVATION_FAILURE, - CTRL_STATE_TS_RUN_FAULT, - CTRL_STATE_SPIN, - CTRL_STATE_APPS_SCS_FAULT, - CTRL_STATE_APPS_BPS_FAULT, - CTRL_STATE_SIM_WAIT_TS_OFF, - CTRL_STATE_SIM_WAIT_TS_ON, - CTRL_STATE_SIM_WAIT_R2D_OFF, - CTRL_STATE_SIM_WAIT_R2D_ON, + CTRL_STATE_TS_BUTTON_WAIT, + CTRL_STATE_WAIT_NEG_AIR, + CTRL_STATE_PRECHARGE_WAIT, + CTRL_STATE_R2D_WAIT, + CTRL_STATE_TS_ON, + CTRL_STATE_R2D_OFF, + CTRL_STATE_R2D_OFF_WAIT, + CTRL_STATE_TS_ACTIVATION_FAILURE, + CTRL_STATE_TS_RUN_FAULT, + CTRL_STATE_SPIN, + CTRL_STATE_APPS_SCS_FAULT, + CTRL_STATE_APPS_BPS_FAULT, + CTRL_STATE_SIM_WAIT_TS_OFF, + CTRL_STATE_SIM_WAIT_TS_ON, + CTRL_STATE_SIM_WAIT_R2D_OFF, + CTRL_STATE_SIM_WAIT_R2D_ON, } ctrl_state_t; /** @@ -62,35 +63,37 @@ typedef enum */ typedef struct { - ctrl_state_t state; // state machine state - TX_THREAD thread; // service thread - uint16_t apps_reading; // APPS reading (% * 10) - uint16_t bps_reading; // BPS reading (% * 10) - int16_t sagl_reading; // steering angle reading (deg * 10) - int16_t motor_speed_reading; // motor speed reading (rpm) - uint16_t torque_request; // last torque request - uint8_t shdn_reading; - int16_t motor_temp; - int16_t inv_temp; - int8_t max_temp; + ctrl_state_t state; // state machine state + TX_THREAD thread; // service thread + uint16_t apps_reading; // APPS reading (% * 10) + uint16_t bps_reading; // BPS reading (% * 10) + int16_t sagl_reading; // steering angle reading (deg * 10) + int16_t motor_speed_reading; // motor speed reading (rpm) + uint16_t torque_request; // last torque request + uint8_t shdn_reading; + int16_t motor_temp; + int16_t inv_temp; + int8_t max_temp; - bool inverter_pwr; - bool pump_pwr; - bool fan_pwr; + bool inverter_pwr; + bool pump_pwr; + bool fan_pwr; - uint32_t neg_air_start; - uint32_t precharge_start; // precharge start time in ticks - uint32_t motor_torque_zero_start; - uint32_t apps_bps_start; - dash_context_t *dash_ptr; // dash service - pm100_context_t *pm100_ptr; // PM100 service - canbc_context_t *canbc_ptr; // CANBC service - tick_context_t *tick_ptr; // tick thread (reads certain sensors) - remote_ctrl_context_t *remote_ctrl_ptr; // tick thread (reads certain sensors) - torque_map_t torque_map; // torque map (APPS -> torque request) + uint32_t neg_air_start; + uint32_t precharge_start; // precharge start time in ticks + uint32_t motor_torque_zero_start; + uint32_t apps_bps_start; + dash_context_t* dash_ptr; // dash service + pm100_context_t* pm100_ptr; // PM100 service + canbc_context_t* canbc_ptr; // CANBC service + canrx_context_t* canrx_ptr; // CANRX service + tick_context_t* tick_ptr; // tick thread (reads certain sensors) + remote_ctrl_context_t* + remote_ctrl_ptr; // tick thread (reads certain sensors) + torque_map_t torque_map; // torque map (APPS -> torque request) - const config_ctrl_t *config_ptr; // config - const config_rtds_t *rtds_config_ptr; // RTDS config + const config_ctrl_t* config_ptr; // config + const config_rtds_t* rtds_config_ptr; // RTDS config uint8_t error; // error code @@ -99,15 +102,16 @@ typedef struct /* * public functions */ -status_t ctrl_init(ctrl_context_t *ctrl_ptr, - dash_context_t *dash_ptr, - pm100_context_t *pm100_ptr, - tick_context_t *tick_ptr, - remote_ctrl_context_t *remote_ctrl_ptr, - canbc_context_t *canbc_ptr, - TX_BYTE_POOL *stack_pool_ptr, - const config_ctrl_t *config_ptr, - const config_rtds_t *rtds_config_ptr, - const config_torque_map_t *torque_map_config_ptr); +status_t ctrl_init(ctrl_context_t* ctrl_ptr, + dash_context_t* dash_ptr, + pm100_context_t* pm100_ptr, + tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + canbc_context_t* canbc_ptr, + canrx_context_t* canrx_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_ctrl_t* config_ptr, + const config_rtds_t* rtds_config_ptr, + const config_torque_map_t* torque_map_config_ptr); #endif diff --git a/src/SUFST/Inc/config.h b/src/SUFST/Inc/config.h index 46111ba..b6c4f12 100644 --- a/src/SUFST/Inc/config.h +++ b/src/SUFST/Inc/config.h @@ -137,6 +137,13 @@ typedef struct { uint32_t broadcast_period_ticks; // ticks between broadcasts } config_canbc_t; +/** + * @brief CAN recieving service + */ +typedef struct { + config_thread_t thread; // CANRX thread config + uint32_t broadcast_timeout_ticks; // maximum number of ticks to wait for a broadcast +} config_canrx_t; typedef struct { config_thread_t thread; // thread config @@ -216,6 +223,7 @@ typedef struct { config_tick_t tick; config_remote_ctrl_t remote_ctrl; config_canbc_t canbc; + config_canrx_t canrx; config_heartbeat_t heartbeat; config_log_t log; config_rtos_t rtos; diff --git a/src/SUFST/Inc/vcu.h b/src/SUFST/Inc/vcu.h index 75ada43..ec6dd90 100644 --- a/src/SUFST/Inc/vcu.h +++ b/src/SUFST/Inc/vcu.h @@ -14,15 +14,16 @@ #include #include "canbc.h" +#include "canrx.h" #include "config.h" #include "ctrl.h" #include "dash.h" #include "heartbeat.h" #include "log.h" #include "pm100.h" -#include "tick.h" -#include "status.h" #include "remote_ctrl.h" +#include "status.h" +#include "tick.h" /** * @brief VCU context @@ -31,18 +32,19 @@ */ typedef struct { - rtcan_handle_t rtcan_s; // RTCAN service for sensors CAN bus - rtcan_handle_t rtcan_c; // RTCAN service for critical systems CAN bus - canbc_context_t canbc; // CAN broadcasting service instance - dash_context_t dash; // dash service - ctrl_context_t ctrl; // control service - pm100_context_t pm100; // PM100 service - tick_context_t tick; - remote_ctrl_context_t remote_ctrl; - heartbeat_context_t heartbeat; // heartbeat service - log_context_t log; // logging service - uint32_t err; // current error code - const config_t* config_ptr; // pointer to global VCU configuration + rtcan_handle_t rtcan_s; // RTCAN service for sensors CAN bus + rtcan_handle_t rtcan_c; // RTCAN service for critical systems CAN bus + canbc_context_t canbc; // CAN broadcasting service instance + canrx_context_t canrx; // CAN recieving service instance + dash_context_t dash; // dash service + ctrl_context_t ctrl; // control service + pm100_context_t pm100; // PM100 service + tick_context_t tick; + remote_ctrl_context_t remote_ctrl; + heartbeat_context_t heartbeat; // heartbeat service + log_context_t log; // logging service + uint32_t err; // current error code + const config_t* config_ptr; // pointer to global VCU configuration } vcu_context_t; diff --git a/src/SUFST/Src/Interfaces/bps.c b/src/SUFST/Src/Interfaces/bps.c index 684793a..017e4c2 100644 --- a/src/SUFST/Src/Interfaces/bps.c +++ b/src/SUFST/Src/Interfaces/bps.c @@ -6,16 +6,18 @@ * @param[in] bps_ptr BPS context * @param[in] config_ptr Configuration */ -status_t bps_init(bps_context_t *bps_ptr, const config_bps_t *config_ptr) +status_t bps_init(bps_context_t* bps_ptr, const config_bps_t* config_ptr) { bps_ptr->config_ptr = config_ptr; // compute pressure thresholds - const uint16_t range = config_ptr->scs.max_mapped - config_ptr->scs.min_mapped; + const uint16_t range + = config_ptr->scs.max_mapped - config_ptr->scs.min_mapped; const uint16_t offset = config_ptr->scs.min_mapped; - bps_ptr->fully_pressed_threshold = (config_ptr->fully_pressed_fraction * range) + offset; + bps_ptr->fully_pressed_threshold + = (config_ptr->fully_pressed_fraction * range) + offset; // create the SCS instance status_t status = scs_create(&bps_ptr->signal, &config_ptr->scs); @@ -29,7 +31,7 @@ status_t bps_init(bps_context_t *bps_ptr, const config_bps_t *config_ptr) * @param[in] bps_ptr * @param[out] reading_ptr */ -status_t bps_read(bps_context_t *bps_ptr, uint16_t *reading_ptr) +status_t bps_read(bps_context_t* bps_ptr, uint16_t* reading_ptr) { status_t status = scs_read(&bps_ptr->signal, reading_ptr); scs_status_t status_verbose = bps_ptr->signal.status_verbose; @@ -64,7 +66,7 @@ status_t bps_read(bps_context_t *bps_ptr, uint16_t *reading_ptr) * @retval true BPS is fully pressed * @retval false BPS not fully pressed, or SCS fault */ -bool bps_fully_pressed(bps_context_t *bps_ptr) +bool bps_fully_pressed(bps_context_t* bps_ptr) { uint16_t reading = 0; status_t status = bps_read(bps_ptr, &reading); diff --git a/src/SUFST/Src/Services/canrx.c b/src/SUFST/Src/Services/canrx.c new file mode 100644 index 0000000..33576cc --- /dev/null +++ b/src/SUFST/Src/Services/canrx.c @@ -0,0 +1,224 @@ +#include "canrx.h" + +#include +#include + +/* + * internal function prototypes + */ +static void canrx_thread_entry(ULONG input); +static void process_broadcast(canrx_context_t* canrx_ptr, + const rtcan_msg_t* msg_ptr); + +/** + * @brief Initialises the CANRX service + * + * @param[in] canrx_ptr CANRX context + * @param[in] stack_pool_ptr Memory pool for service thread stack + * @param[in] rtcan_c_ptr RTCAN C instance for receiving broadcasts + * @param[in] rtcan_s_ptr RTCAN S instance for sending precharge cmd + * @param[in] config_ptr Configuration + */ +status_t canrx_init(canrx_context_t* canrx_ptr, + rtcan_handle_t* rtcan_c_ptr, + rtcan_handle_t* rtcan_s_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_canrx_t* config_ptr) +{ + canrx_ptr->config_ptr = config_ptr; + canrx_ptr->rtcan_c_ptr = rtcan_c_ptr; + canrx_ptr->rtcan_s_ptr = rtcan_s_ptr; + canrx_ptr->error = CANRX_ERROR_NONE; + canrx_ptr->broadcasts_valid = false; + + status_t status = STATUS_OK; + + // create service thread + void* stack_ptr = NULL; + UINT tx_status = tx_byte_allocate(stack_pool_ptr, + &stack_ptr, + config_ptr->thread.stack_size, + TX_NO_WAIT); + + if (tx_status == TX_SUCCESS) + { + tx_status = tx_thread_create(&canrx_ptr->thread, + (CHAR*) config_ptr->thread.name, + canrx_thread_entry, + (ULONG) canrx_ptr, + stack_ptr, + config_ptr->thread.stack_size, + config_ptr->thread.priority, + config_ptr->thread.priority, + TX_NO_TIME_SLICE, + TX_AUTO_START); + } + + // create CAN receive queue + if (tx_status == TX_SUCCESS) + { + tx_status = tx_queue_create(&canrx_ptr->can_c_rx_queue, + NULL, + TX_1_ULONG, + canrx_ptr->can_rx_queue_mem, + sizeof(canrx_ptr->can_rx_queue_mem)); + } + + if (tx_status == TX_SUCCESS) + { + tx_status = tx_queue_create(&canrx_ptr->can_s_rx_queue, + NULL, + TX_1_ULONG, + canrx_ptr->can_rx_queue_mem, + sizeof(canrx_ptr->can_rx_queue_mem)); + } + + // create state mutex + if (tx_status == TX_SUCCESS) + { + tx_status = tx_mutex_create(&canrx_ptr->state_mutex, NULL, TX_INHERIT); + } + + if (tx_status != TX_SUCCESS) + { + status = STATUS_ERROR; + } + + // check all ok + if (status != STATUS_OK) + { + tx_thread_terminate(&canrx_ptr->thread); + } + + // turn off power + HAL_GPIO_WritePin(STATUS_GPIO_Port, // This pin used to be called STATUS + STATUS_Pin, + GPIO_PIN_RESET); + + return status; +} + +/** + * @brief CANRX service thread entry function + * + * @details This thread is responsible for receiving and processing broadcast + * messages from the CANRX + */ +void canrx_thread_entry(ULONG input) +{ + canrx_context_t* canrx_ptr = (canrx_context_t*) input; + const config_canrx_t* config_ptr = canrx_ptr->config_ptr; + + // set up RTCAN subscriptions + /* This is commented, as there are currently no can_c_subscriptions + Uncomment to add a can_c_subscription + + uint32_t can_c_subscriptions[0]; + + for (uint32_t i = 0; + i < sizeof(can_c_subscriptions) / sizeof(can_c_subscriptions[0]); + i++) + { + rtcan_status_t status = rtcan_subscribe(canrx_ptr->rtcan_c_ptr, + can_c_subscriptions[i], + &canrx_ptr->can_c_rx_queue); + + if (status != RTCAN_OK) + { + // TODO: update broadcast states with error + tx_thread_terminate(&canrx_ptr->thread); + } + } + */ + + uint32_t can_s_subscriptions[] = {CAN_S_MSGID_0_X202_FRAME_ID}; + + for (uint32_t i = 0; + i < sizeof(can_s_subscriptions) / sizeof(can_s_subscriptions[0]); + i++) + { + rtcan_status_t status = rtcan_subscribe(canrx_ptr->rtcan_c_ptr, + can_s_subscriptions[i], + &canrx_ptr->can_s_rx_queue); + + if (status != RTCAN_OK) + { + // TODO: update broadcast states with error + tx_thread_terminate(&canrx_ptr->thread); + } + } + + // process incoming messages, or timeout + while (1) + { + UINT status = TX_QUEUE_ERROR; + rtcan_msg_t* msg_ptr = NULL; + UINT can_s_status + = tx_queue_receive(&canrx_ptr->can_s_rx_queue, + &msg_ptr, + config_ptr->broadcast_timeout_ticks); + + if (can_s_status == TX_SUCCESS && msg_ptr != NULL) + { + canrx_ptr->broadcasts_valid = true; + process_broadcast(canrx_ptr, msg_ptr); + rtcan_msg_consumed(canrx_ptr->rtcan_s_ptr, msg_ptr); + status = TX_SUCCESS; + } + + msg_ptr = NULL; + UINT can_c_status + = tx_queue_receive(&canrx_ptr->can_c_rx_queue, + &msg_ptr, + config_ptr->broadcast_timeout_ticks); + + if (can_c_status == TX_SUCCESS && msg_ptr != NULL) + { + canrx_ptr->broadcasts_valid = true; + process_broadcast(canrx_ptr, msg_ptr); + rtcan_msg_consumed(canrx_ptr->rtcan_c_ptr, msg_ptr); + status = TX_SUCCESS; + } + + if (can_s_status == TX_QUEUE_EMPTY && can_c_status == TX_QUEUE_EMPTY) + { + canrx_ptr->broadcasts_valid = false; + LOG_ERROR("CANRX recieving timed out\n"); + } + else if (status == TX_SUCCESS) + { + canrx_ptr->broadcasts_valid = true; + process_broadcast(canrx_ptr, msg_ptr); + rtcan_msg_consumed(canrx_ptr->rtcan_c_ptr, msg_ptr); + } + else + { + LOG_ERROR("CANRX other error\n"); + } + } +} + +/** + * @brief Processes incoming broadcast messages + * + * @param[in] canrx_ptr CANRX context + * @param[in] msg_ptr Incoming message + */ +void process_broadcast(canrx_context_t* canrx_ptr, const rtcan_msg_t* msg_ptr) +{ + switch (msg_ptr->identifier) + { + + case CAN_S_MSGID_0_X202_FRAME_ID: + { + can_s_msgid_0_x202_unpack(&canrx_ptr->msgid_x202, + msg_ptr->data, + msg_ptr->length); + + break; + } + + default: + break; + } +} diff --git a/src/SUFST/Src/Services/ctrl.c b/src/SUFST/Src/Services/ctrl.c index b31b3fe..2359d85 100644 --- a/src/SUFST/Src/Services/ctrl.c +++ b/src/SUFST/Src/Services/ctrl.c @@ -16,11 +16,15 @@ * internal function prototypes */ void ctrl_thread_entry(ULONG input); -void ctrl_state_machine_tick(ctrl_context_t *ctrl_ptr); -void ctrl_update_canbc_states(ctrl_context_t *ctrl_ptr); -void ctrl_handle_ts_fault(ctrl_context_t *ctrl_ptr); -status_t ctrl_get_apps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result); -status_t ctrl_get_bps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result); +void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr); +void ctrl_update_canbc_states(ctrl_context_t* ctrl_ptr); +void ctrl_handle_ts_fault(ctrl_context_t* ctrl_ptr); +status_t ctrl_get_apps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result); +status_t ctrl_get_bps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result); bool ctrl_fan_passed_on_threshold(ctrl_context_t* ctrl_ptr); bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr); @@ -30,6 +34,7 @@ bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr); * @param[in] ctrl_ptr Control context * @param[in] dash_ptr Dash context * @param[in] canbc_ptr CANBC context + * @param[in] canrx_ptr CANRX context * @param[in] pm100_ptr PM100 context * @param[in] stack_pool_ptr Byte pool to allocate thread stack from * @param[in] config_ptr Configuration @@ -38,80 +43,81 @@ bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr); * @param[in] rtds_config_ptr RTDS configuration * @param[in] torque_map_config_ptr Torque map configuration */ -status_t ctrl_init(ctrl_context_t *ctrl_ptr, - dash_context_t *dash_ptr, - pm100_context_t *pm100_ptr, - tick_context_t *tick_ptr, - remote_ctrl_context_t *remote_ctrl_ptr, - canbc_context_t *canbc_ptr, - TX_BYTE_POOL *stack_pool_ptr, - const config_ctrl_t *config_ptr, - const config_rtds_t *rtds_config_ptr, - const config_torque_map_t *torque_map_config_ptr) +status_t ctrl_init(ctrl_context_t* ctrl_ptr, + dash_context_t* dash_ptr, + pm100_context_t* pm100_ptr, + tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + canbc_context_t* canbc_ptr, + canrx_context_t* canrx_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_ctrl_t* config_ptr, + const config_rtds_t* rtds_config_ptr, + const config_torque_map_t* torque_map_config_ptr) { - ctrl_ptr->state = CTRL_STATE_TS_BUTTON_WAIT; - ctrl_ptr->dash_ptr = dash_ptr; - ctrl_ptr->pm100_ptr = pm100_ptr; - ctrl_ptr->tick_ptr = tick_ptr; - ctrl_ptr->canbc_ptr = canbc_ptr; - ctrl_ptr->config_ptr = config_ptr; - ctrl_ptr->rtds_config_ptr = rtds_config_ptr; - ctrl_ptr->error = CTRL_ERROR_NONE; - ctrl_ptr->apps_reading = 0; - ctrl_ptr->bps_reading = 0; - ctrl_ptr->sagl_reading = 0; - ctrl_ptr->torque_request = 0; - ctrl_ptr->shdn_reading = 0; - ctrl_ptr->precharge_start = 0; - ctrl_ptr->inverter_pwr = false; - ctrl_ptr->pump_pwr = false; - ctrl_ptr->fan_pwr = false; - ctrl_ptr->remote_ctrl_ptr = remote_ctrl_ptr; - - - // create the thread - void *stack_ptr = NULL; - UINT tx_status = tx_byte_allocate(stack_pool_ptr, - &stack_ptr, - config_ptr->thread.stack_size, - TX_NO_WAIT); - - if (tx_status == TX_SUCCESS) - { - tx_status = tx_thread_create(&ctrl_ptr->thread, - (CHAR *)config_ptr->thread.name, - ctrl_thread_entry, - (ULONG)ctrl_ptr, - stack_ptr, - config_ptr->thread.stack_size, - config_ptr->thread.priority, - config_ptr->thread.priority, - TX_NO_TIME_SLICE, - TX_AUTO_START); - } - - status_t status = (tx_status == TX_SUCCESS) ? STATUS_OK : STATUS_ERROR; - - // initialise the torque map - if (status == STATUS_OK) - { - status = torque_map_init(&ctrl_ptr->torque_map, torque_map_config_ptr); - } - - // make sure TS is disabled - trc_set_ts_on(GPIO_PIN_RESET); - - // check all ok before starting - if (status != STATUS_OK) - { - tx_thread_terminate(&ctrl_ptr->thread); - ctrl_ptr->error |= CTRL_ERROR_INIT; - } - - // send initial state update - ctrl_update_canbc_states(ctrl_ptr); - - return status; + ctrl_ptr->state = CTRL_STATE_TS_BUTTON_WAIT; + ctrl_ptr->dash_ptr = dash_ptr; + ctrl_ptr->pm100_ptr = pm100_ptr; + ctrl_ptr->tick_ptr = tick_ptr; + ctrl_ptr->canbc_ptr = canbc_ptr; + ctrl_ptr->canrx_ptr = canrx_ptr; + ctrl_ptr->config_ptr = config_ptr; + ctrl_ptr->rtds_config_ptr = rtds_config_ptr; + ctrl_ptr->error = CTRL_ERROR_NONE; + ctrl_ptr->apps_reading = 0; + ctrl_ptr->bps_reading = 0; + ctrl_ptr->sagl_reading = 0; + ctrl_ptr->torque_request = 0; + ctrl_ptr->shdn_reading = 0; + ctrl_ptr->precharge_start = 0; + ctrl_ptr->inverter_pwr = false; + ctrl_ptr->pump_pwr = false; + ctrl_ptr->fan_pwr = false; + ctrl_ptr->remote_ctrl_ptr = remote_ctrl_ptr; + + // create the thread + void* stack_ptr = NULL; + UINT tx_status = tx_byte_allocate(stack_pool_ptr, + &stack_ptr, + config_ptr->thread.stack_size, + TX_NO_WAIT); + + if (tx_status == TX_SUCCESS) + { + tx_status = tx_thread_create(&ctrl_ptr->thread, + (CHAR*) config_ptr->thread.name, + ctrl_thread_entry, + (ULONG) ctrl_ptr, + stack_ptr, + config_ptr->thread.stack_size, + config_ptr->thread.priority, + config_ptr->thread.priority, + TX_NO_TIME_SLICE, + TX_AUTO_START); + } + + status_t status = (tx_status == TX_SUCCESS) ? STATUS_OK : STATUS_ERROR; + + // initialise the torque map + if (status == STATUS_OK) + { + status = torque_map_init(&ctrl_ptr->torque_map, torque_map_config_ptr); + } + + // make sure TS is disabled + trc_set_ts_on(GPIO_PIN_RESET); + + // check all ok before starting + if (status != STATUS_OK) + { + tx_thread_terminate(&ctrl_ptr->thread); + ctrl_ptr->error |= CTRL_ERROR_INIT; + } + + // send initial state update + ctrl_update_canbc_states(ctrl_ptr); + + return status; } /** @@ -121,12 +127,12 @@ status_t ctrl_init(ctrl_context_t *ctrl_ptr, */ void ctrl_thread_entry(ULONG input) { - ctrl_context_t *ctrl_ptr = (ctrl_context_t *)input; + ctrl_context_t* ctrl_ptr = (ctrl_context_t*) input; - while (1) - { - uint32_t start_time = tx_time_get(); - dash_update_buttons(ctrl_ptr->dash_ptr); + while (1) + { + uint32_t start_time = tx_time_get(); + dash_update_buttons(ctrl_ptr->dash_ptr); ctrl_ptr->shdn_reading = trc_ready(); @@ -196,422 +202,435 @@ bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr) * * @param[in] ctrl_ptr Control context */ -void ctrl_state_machine_tick(ctrl_context_t *ctrl_ptr) +void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) { // reduce typing... dash_context_t* dash_ptr = ctrl_ptr->dash_ptr; - remote_ctrl_context_t *remote_ctrl_ptr = ctrl_ptr->remote_ctrl_ptr; + remote_ctrl_context_t* remote_ctrl_ptr = ctrl_ptr->remote_ctrl_ptr; const config_ctrl_t* config_ptr = ctrl_ptr->config_ptr; const uint16_t BPS_ON_THRESH = config_ptr->bps_on_threshold; - ctrl_state_t next_state = ctrl_ptr->state; - - // In simulation mode, the TS and R2D buttons are controlled by the remote control, but the dash is still in effect - #ifdef VCU_SIMULATION_MODE - dash_ptr->tson_flag = dash_ptr->tson_flag || remote_get_ts_on_reading(remote_ctrl_ptr); - dash_ptr->r2d_flag = dash_ptr->r2d_flag || remote_get_r2d_reading(remote_ctrl_ptr); - #endif - - switch (ctrl_ptr->state) - { - - // wait for TS button to be held and released - // then begin activating the TS - case (CTRL_STATE_TS_BUTTON_WAIT): - { - if (dash_ptr->tson_flag) - { - dash_clear_buttons(dash_ptr); - - if (trc_ready()) - { - LOG_INFO("TSON pressed & SHDN closed\n"); - - trc_set_ts_on(GPIO_PIN_SET); - - next_state = CTRL_STATE_WAIT_NEG_AIR; - ctrl_ptr->neg_air_start = tx_time_get(); - } - } - else{ - ctrl_ptr->inverter_pwr = false; // Turn off inverter if TS button is not pressed + ctrl_state_t next_state = ctrl_ptr->state; + +// In simulation mode, the TS and R2D buttons are controlled by the remote +// control, but the dash is still in effect +#ifdef VCU_SIMULATION_MODE + dash_ptr->tson_flag + = dash_ptr->tson_flag || remote_get_ts_on_reading(remote_ctrl_ptr); + dash_ptr->r2d_flag + = dash_ptr->r2d_flag || remote_get_r2d_reading(remote_ctrl_ptr); +#endif + + switch (ctrl_ptr->state) + { + + // wait for TS button to be held and released + // then begin activating the TS + case (CTRL_STATE_TS_BUTTON_WAIT): + { + if (dash_ptr->tson_flag) + { + dash_clear_buttons(dash_ptr); + + if (trc_ready()) + { + LOG_INFO("TSON pressed & SHDN closed\n"); + + trc_set_ts_on(GPIO_PIN_SET); + + next_state = CTRL_STATE_WAIT_NEG_AIR; + ctrl_ptr->neg_air_start = tx_time_get(); + } } + else + { + ctrl_ptr->inverter_pwr + = false; // Turn off inverter if TS button is not pressed + } + + break; + } + + case (CTRL_STATE_WAIT_NEG_AIR): + { + if (tx_time_get() + >= ctrl_ptr->neg_air_start + TX_TIMER_TICKS_PER_SECOND / 4) + { + LOG_INFO("Neg AIR closed, turning on inverter\n"); + + ctrl_ptr->inverter_pwr = true; + + next_state = CTRL_STATE_PRECHARGE_WAIT; + ctrl_ptr->precharge_start = tx_time_get(); + } + + break; + } + + // TS is ready, can initiate pre-charge sequence + // TS on LED turns solid + 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)) + { +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_TS_ON; +#else + next_state = CTRL_STATE_R2D_WAIT; +#endif + dash_clear_buttons(dash_ptr); + LOG_INFO("Precharge complete\n"); + } + else if (charge_time >= config_ptr->precharge_timeout_ticks) + { + ctrl_ptr->error |= CTRL_ERROR_PRECHARGE_TIMEOUT; + next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; + LOG_ERROR("Precharge timeout reached\n"); + } + + break; + } + + // pre-charge is complete, wait for R2D signal + // also wait for brake to be fully pressed (if enabled) + case (CTRL_STATE_R2D_WAIT): + { + if (!trc_ready()) + { + LOG_ERROR("SHDN opened\n"); + next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; + } + else if (dash_ptr->tson_flag) // TSON pressed, disable TS + { + dash_ptr->tson_flag = false; + + ctrl_ptr->inverter_pwr = false; // Turn off inverter + trc_set_ts_on(GPIO_PIN_RESET); // Turn off AIRs + +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_TS_OFF; +#else + next_state = CTRL_STATE_TS_BUTTON_WAIT; +#endif + } + else if (dash_ptr->r2d_flag) // R2D pressed + { +#ifndef VCU_SIMULATION_MODE + dash_ptr->r2d_flag = false; +#endif + + status_t result = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->bps_reading); + + bool r2d = false; + + if (result == STATUS_OK) + { + r2d = (config_ptr->r2d_requires_brake) + ? (ctrl_ptr->bps_reading > BPS_ON_THRESH) + : 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; + +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_R2D_ON; +#else + next_state = CTRL_STATE_TS_ON; +#endif + LOG_INFO("R2D active\n"); + } + } + else + { + LOG_ERROR("BPS reading failed\n"); + next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; + } + } + break; + } + + // the TS is on + case (CTRL_STATE_TS_ON): + { + // read from the APPS + status_t pm100_status; + + status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->apps_reading); + status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->bps_reading); + + if (dash_ptr->r2d_flag) + { + dash_clear_buttons(dash_ptr); +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; +#else + next_state = CTRL_STATE_R2D_OFF; +#endif + } + else if (apps_status == STATUS_OK && bps_status == STATUS_OK) + { + // Check for brake + accel pedal pressed + if (ctrl_ptr->apps_reading + >= ctrl_ptr->config_ptr->apps_bps_high_threshold + && ctrl_ptr->bps_reading > BPS_ON_THRESH) + { + LOG_ERROR("BP and AP pressed\n"); + + if (tx_time_get() >= ctrl_ptr->apps_bps_start + + (TX_TIMER_TICKS_PER_SECOND / 3)) + { + LOG_ERROR("BP-AP fault\n"); + // next_state = CTRL_STATE_APPS_BPS_FAULT; + } + } + else + { + ctrl_ptr->apps_bps_start = tx_time_get(); + } +#ifdef VCU_SIMULATION_MODE +#ifndef VCU_SIMULATION_ON_POWER + ctrl_ptr->torque_request + = remote_get_torque_reading(remote_ctrl_ptr); +#else + uint16_t power = remote_get_power_reading(remote_ctrl_ptr); + + int16_t motor_speed = pm100_motor_speed(ctrl_ptr->pm100_ptr); + uint16_t rad_s = 1; + + // this if to be removed + if (motor_speed < 10) + { + motor_speed = 10; + } + // rpm to rad/s + rad_s = (uint16_t) (motor_speed * 0.10472); + if (rad_s == 0) + rad_s = 1; + ctrl_ptr->torque_request = (uint16_t) (power / rad_s); + if (ctrl_ptr->torque_request > 1500) + ctrl_ptr->torque_request = 1500; +#endif +#else + ctrl_ptr->torque_request = torque_map_apply(&ctrl_ptr->torque_map, + ctrl_ptr->apps_reading); +#endif + + LOG_INFO("ADC: %d, Torque: %d\n", + ctrl_ptr->apps_reading, + ctrl_ptr->torque_request); + + pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, + ctrl_ptr->torque_request); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + } + else + { + LOG_ERROR("APPS / BPS fault\n"); + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + break; + } + + case CTRL_STATE_R2D_OFF: + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + ctrl_ptr->motor_torque_zero_start = tx_time_get(); + ctrl_ptr->pump_pwr = 0; - break; - } - - case (CTRL_STATE_WAIT_NEG_AIR): - { - if (tx_time_get() >= ctrl_ptr->neg_air_start + TX_TIMER_TICKS_PER_SECOND / 4) - { - LOG_INFO("Neg AIR closed, turning on inverter\n"); - - ctrl_ptr->inverter_pwr = true; - - next_state = CTRL_STATE_PRECHARGE_WAIT; - ctrl_ptr->precharge_start = tx_time_get(); - } - - break; - } - - // TS is ready, can initiate pre-charge sequence - // TS on LED turns solid - 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)) - { - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_TS_ON; - #else - next_state = CTRL_STATE_R2D_WAIT; - #endif - dash_clear_buttons(dash_ptr); - LOG_INFO("Precharge complete\n"); - } - else if (charge_time >= config_ptr->precharge_timeout_ticks) - { - ctrl_ptr->error |= CTRL_ERROR_PRECHARGE_TIMEOUT; - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - LOG_ERROR("Precharge timeout reached\n"); - } - - break; - } - - // pre-charge is complete, wait for R2D signal - // also wait for brake to be fully pressed (if enabled) - case (CTRL_STATE_R2D_WAIT): - { - if (!trc_ready()) - { - LOG_ERROR("SHDN opened\n"); - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - } - else if (dash_ptr->tson_flag) // TSON pressed, disable TS - { - dash_ptr->tson_flag = false; - - ctrl_ptr->inverter_pwr = false; // Turn off inverter - trc_set_ts_on(GPIO_PIN_RESET); // Turn off AIRs - - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_TS_OFF; - #else - next_state = CTRL_STATE_TS_BUTTON_WAIT; - #endif - } - else if (dash_ptr->r2d_flag) // R2D pressed - { - #ifndef VCU_SIMULATION_MODE - dash_ptr->r2d_flag = false; - #endif - - status_t result = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - bool r2d = false; - - if (result == STATUS_OK) - { - r2d = (config_ptr->r2d_requires_brake) ? (ctrl_ptr->bps_reading > BPS_ON_THRESH) : 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; - - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_ON; - #else - next_state = CTRL_STATE_TS_ON; - #endif - LOG_INFO("R2D active\n"); - } - } - else - { - LOG_ERROR("BPS reading failed\n"); - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - } - } - break; - } - - // the TS is on - case (CTRL_STATE_TS_ON): - { - // read from the APPS - status_t pm100_status; - - status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->apps_reading); - status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - if (dash_ptr->r2d_flag) - { - dash_clear_buttons(dash_ptr); - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; - #else - next_state = CTRL_STATE_R2D_OFF; - #endif - } - else if (apps_status == STATUS_OK && bps_status == STATUS_OK) - { - // Check for brake + accel pedal pressed - if (ctrl_ptr->apps_reading >= - ctrl_ptr->config_ptr->apps_bps_high_threshold && - ctrl_ptr->bps_reading > BPS_ON_THRESH) - { - LOG_ERROR("BP and AP pressed\n"); - - if (tx_time_get() >= ctrl_ptr->apps_bps_start + - (TX_TIMER_TICKS_PER_SECOND / 3)) - { - LOG_ERROR("BP-AP fault\n"); - // next_state = CTRL_STATE_APPS_BPS_FAULT; - } - } - else - { - ctrl_ptr->apps_bps_start = tx_time_get(); - } - #ifdef VCU_SIMULATION_MODE - #ifndef VCU_SIMULATION_ON_POWER - ctrl_ptr->torque_request = remote_get_torque_reading(remote_ctrl_ptr); - #else - uint16_t power = remote_get_power_reading(remote_ctrl_ptr); - - int16_t motor_speed = pm100_motor_speed(ctrl_ptr->pm100_ptr); - uint16_t rad_s = 1; - - // this if to be removed - if (motor_speed < 10) - { - motor_speed = 10; - } - // rpm to rad/s - rad_s = (uint16_t)(motor_speed * 0.10472); - if (rad_s == 0) - rad_s = 1; - ctrl_ptr->torque_request = (uint16_t)(power / rad_s); - if (ctrl_ptr->torque_request > 1500) - ctrl_ptr->torque_request = 1500; - #endif - #else - ctrl_ptr->torque_request = torque_map_apply(&ctrl_ptr->torque_map, - ctrl_ptr->apps_reading); - #endif - - LOG_INFO("ADC: %d, Torque: %d\n", - ctrl_ptr->apps_reading, ctrl_ptr->torque_request); - - pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, - ctrl_ptr->torque_request); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - } - else - { - LOG_ERROR("APPS / BPS fault\n"); - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - break; - } - - case CTRL_STATE_R2D_OFF: - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - ctrl_ptr->motor_torque_zero_start = tx_time_get(); - ctrl_ptr->pump_pwr = 0; - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - else - { - next_state = CTRL_STATE_R2D_OFF_WAIT; - } - break; - } - - case CTRL_STATE_R2D_OFF_WAIT: - { + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + else + { + next_state = CTRL_STATE_R2D_OFF_WAIT; + } + break; + } + + case CTRL_STATE_R2D_OFF_WAIT: + { dash_set_r2d_led_state(dash_ptr, GPIO_PIN_RESET); - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - else if (tx_time_get() >= ctrl_ptr->motor_torque_zero_start + - TX_TIMER_TICKS_PER_SECOND / 2) - { - #ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; - #else - next_state = CTRL_STATE_R2D_WAIT; - #endif - - } - break; - } - - // activation or runtime failure - case (CTRL_STATE_TS_ACTIVATION_FAILURE): - case (CTRL_STATE_TS_RUN_FAULT): - { - LOG_ERROR("TS fault during activation or runtime\n"); - ctrl_handle_ts_fault(ctrl_ptr); - next_state = CTRL_STATE_TS_BUTTON_WAIT; - break; - } - - case (CTRL_STATE_SPIN): // Spin forever - { - break; - } - - // SCS fault - // this is recoverable, if the signal becomes plausible again - case (CTRL_STATE_APPS_SCS_FAULT): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - if (ctrl_get_apps_reading(ctrl_ptr->tick_ptr, remote_ctrl_ptr, &ctrl_ptr->apps_reading) == STATUS_OK) - { - next_state = CTRL_STATE_TS_ON; - } - - break; - } - - case (CTRL_STATE_APPS_BPS_FAULT): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->apps_reading); - status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - if (apps_status == STATUS_OK && bps_status == STATUS_OK) - { - if ((ctrl_ptr->apps_reading < ctrl_ptr->config_ptr->apps_bps_low_threshold) && - (ctrl_ptr->bps_reading < BPS_ON_THRESH)) - { - next_state = CTRL_STATE_TS_ON; - } - } - else - { - next_state = CTRL_STATE_APPS_SCS_FAULT; - } - - break; - } - - // Needed when in simulation mode to avoid the TS being turned on again - case (CTRL_STATE_SIM_WAIT_TS_OFF): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - if (!dash_ptr->tson_flag) - { - next_state = CTRL_STATE_TS_BUTTON_WAIT; - } - break; - } - - // Needed when in simulation mode to avoid the TS being turned off again - case (CTRL_STATE_SIM_WAIT_TS_ON): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - if (!dash_ptr->tson_flag) - { - next_state = CTRL_STATE_R2D_WAIT; - } - break; - } - - // Needed when in simulation mode to avoid the R2D being turned off again - case (CTRL_STATE_SIM_WAIT_R2D_ON): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - if (!dash_ptr->r2d_flag) - { - next_state = CTRL_STATE_TS_ON; - } - break; - } - - // Needed when in simulation mode to avoid the R2D being turned on again - case (CTRL_STATE_SIM_WAIT_R2D_OFF): - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - if (!dash_ptr->r2d_flag) - { - dash_set_r2d_led_state(dash_ptr, GPIO_PIN_RESET); - next_state = CTRL_STATE_R2D_WAIT; - } - break; - } - - default: - break; - } - - ctrl_ptr->state = next_state; + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + else if (tx_time_get() >= ctrl_ptr->motor_torque_zero_start + + TX_TIMER_TICKS_PER_SECOND / 2) + { +#ifdef VCU_SIMULATION_MODE + next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; +#else + next_state = CTRL_STATE_R2D_WAIT; +#endif + } + break; + } + + // activation or runtime failure + case (CTRL_STATE_TS_ACTIVATION_FAILURE): + case (CTRL_STATE_TS_RUN_FAULT): + { + LOG_ERROR("TS fault during activation or runtime\n"); + ctrl_handle_ts_fault(ctrl_ptr); + next_state = CTRL_STATE_TS_BUTTON_WAIT; + break; + } + + case (CTRL_STATE_SPIN): // Spin forever + { + break; + } + + // SCS fault + // this is recoverable, if the signal becomes plausible again + case (CTRL_STATE_APPS_SCS_FAULT): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + if (ctrl_get_apps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->apps_reading) + == STATUS_OK) + { + next_state = CTRL_STATE_TS_ON; + } + + break; + } + + case (CTRL_STATE_APPS_BPS_FAULT): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->apps_reading); + status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, + remote_ctrl_ptr, + &ctrl_ptr->bps_reading); + + if (apps_status == STATUS_OK && bps_status == STATUS_OK) + { + if ((ctrl_ptr->apps_reading + < ctrl_ptr->config_ptr->apps_bps_low_threshold) + && (ctrl_ptr->bps_reading < BPS_ON_THRESH)) + { + next_state = CTRL_STATE_TS_ON; + } + } + else + { + next_state = CTRL_STATE_APPS_SCS_FAULT; + } + + break; + } + + // Needed when in simulation mode to avoid the TS being turned on again + case (CTRL_STATE_SIM_WAIT_TS_OFF): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + + if (!dash_ptr->tson_flag) + { + next_state = CTRL_STATE_TS_BUTTON_WAIT; + } + break; + } + + // Needed when in simulation mode to avoid the TS being turned off again + case (CTRL_STATE_SIM_WAIT_TS_ON): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + if (!dash_ptr->tson_flag) + { + next_state = CTRL_STATE_R2D_WAIT; + } + break; + } + + // Needed when in simulation mode to avoid the R2D being turned off again + case (CTRL_STATE_SIM_WAIT_R2D_ON): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + if (!dash_ptr->r2d_flag) + { + next_state = CTRL_STATE_TS_ON; + } + break; + } + + // Needed when in simulation mode to avoid the R2D being turned on again + case (CTRL_STATE_SIM_WAIT_R2D_OFF): + { + ctrl_ptr->torque_request = 0; + status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); + if (pm100_status != STATUS_OK) + { + next_state = CTRL_STATE_TS_RUN_FAULT; + } + if (!dash_ptr->r2d_flag) + { + dash_set_r2d_led_state(dash_ptr, GPIO_PIN_RESET); + next_state = CTRL_STATE_R2D_WAIT; + } + break; + } + + default: + break; + } + + ctrl_ptr->state = next_state; } /** @@ -620,19 +639,19 @@ void ctrl_state_machine_tick(ctrl_context_t *ctrl_ptr) * * @param[in] ctrl_ptr Control context */ -void ctrl_handle_ts_fault(ctrl_context_t *ctrl_ptr) +void ctrl_handle_ts_fault(ctrl_context_t* ctrl_ptr) { - dash_context_t *dash_ptr = ctrl_ptr->dash_ptr; - const config_ctrl_t *config_ptr = ctrl_ptr->config_ptr; + dash_context_t* dash_ptr = ctrl_ptr->dash_ptr; + const config_ctrl_t* config_ptr = ctrl_ptr->config_ptr; - // pm100_lvs_off(ctrl_ptr->pm100_ptr); - ctrl_ptr->inverter_pwr = false; - ctrl_ptr->pump_pwr = false; - ctrl_ptr->fan_pwr = false; + // pm100_lvs_off(ctrl_ptr->pm100_ptr); + ctrl_ptr->inverter_pwr = false; + ctrl_ptr->pump_pwr = false; + ctrl_ptr->fan_pwr = false; - trc_set_ts_on(GPIO_PIN_RESET); - dash_blink_ts_on_led(dash_ptr, config_ptr->error_led_toggle_ticks); - ctrl_update_canbc_states(ctrl_ptr); + trc_set_ts_on(GPIO_PIN_RESET); + dash_blink_ts_on_led(dash_ptr, config_ptr->error_led_toggle_ticks); + ctrl_update_canbc_states(ctrl_ptr); } /** @@ -640,39 +659,43 @@ void ctrl_handle_ts_fault(ctrl_context_t *ctrl_ptr) * * @param[in] ctrl_ptr */ -void ctrl_update_canbc_states(ctrl_context_t *ctrl_ptr) +void ctrl_update_canbc_states(ctrl_context_t* ctrl_ptr) { - canbc_states_t *states = canbc_lock_state(ctrl_ptr->canbc_ptr, TX_NO_WAIT); - - if (states != NULL) - { - // TODO: add ready to drive state? - states->sensors.vcu_sagl = ctrl_ptr->sagl_reading; - states->sensors.vcu_torque_request = ctrl_ptr->torque_request; - states->temps.vcu_max_temp = (int8_t) ctrl_ptr->max_temp; - states->state.vcu_ctrl_state = (uint8_t)ctrl_ptr->state; - states->state.vcu_drs_active = ctrl_ptr->shdn_reading; - states->errors.vcu_ctrl_error = ctrl_ptr->error; - states->pdm.inverter = ctrl_ptr->inverter_pwr; - states->pdm.pump = ctrl_ptr->pump_pwr; - states->pdm.fan = ctrl_ptr->fan_pwr; - canbc_unlock_state(ctrl_ptr->canbc_ptr); - } + canbc_states_t* states = canbc_lock_state(ctrl_ptr->canbc_ptr, TX_NO_WAIT); + + if (states != NULL) + { + // TODO: add ready to drive state? + states->sensors.vcu_sagl = ctrl_ptr->sagl_reading; + states->sensors.vcu_torque_request = ctrl_ptr->torque_request; + states->temps.vcu_max_temp = (int8_t) ctrl_ptr->max_temp; + states->state.vcu_ctrl_state = (uint8_t) ctrl_ptr->state; + states->state.vcu_drs_active = ctrl_ptr->shdn_reading; + states->errors.vcu_ctrl_error = ctrl_ptr->error; + states->pdm.inverter = ctrl_ptr->inverter_pwr; + states->pdm.pump = ctrl_ptr->pump_pwr; + states->pdm.fan = ctrl_ptr->fan_pwr; + canbc_unlock_state(ctrl_ptr->canbc_ptr); + } } -status_t ctrl_get_apps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result) +status_t ctrl_get_apps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result) { - #ifdef VCU_SIMULATION_MODE - return remote_get_apps_reading(remote_ctrl_ptr, result); - #else - return tick_get_apps_reading(tick_ptr, result); - #endif +#ifdef VCU_SIMULATION_MODE + return remote_get_apps_reading(remote_ctrl_ptr, result); +#else + return tick_get_apps_reading(tick_ptr, result); +#endif } -status_t ctrl_get_bps_reading(tick_context_t *tick_ptr, remote_ctrl_context_t *remote_ctrl_ptr, uint16_t *result) +status_t ctrl_get_bps_reading(tick_context_t* tick_ptr, + remote_ctrl_context_t* remote_ctrl_ptr, + uint16_t* result) { - #ifdef VCU_SIMULATION_MODE - return remote_get_bps_reading(remote_ctrl_ptr, result); - #else - return tick_get_bps_reading(tick_ptr, result); - #endif +#ifdef VCU_SIMULATION_MODE + return remote_get_bps_reading(remote_ctrl_ptr, result); +#else + return tick_get_bps_reading(tick_ptr, result); +#endif } \ No newline at end of file diff --git a/src/SUFST/Src/Services/pm100.c b/src/SUFST/Src/Services/pm100.c index 0a31e95..ad80984 100644 --- a/src/SUFST/Src/Services/pm100.c +++ b/src/SUFST/Src/Services/pm100.c @@ -3,29 +3,29 @@ #include #include -#define PM100_NO_FAULTS 0x00 +#define PM100_NO_FAULTS 0x00 -#define PM100_VSM_STATE_START 0x00 -#define PM100_VSM_STATE_PRECHARGE_INIT 0x01 -#define PM100_VSM_STATE_PRECHARGE_ACTIVE 0x02 +#define PM100_VSM_STATE_START 0x00 +#define PM100_VSM_STATE_PRECHARGE_INIT 0x01 +#define PM100_VSM_STATE_PRECHARGE_ACTIVE 0x02 #define PM100_VSM_STATE_PRECHARGE_COMPLETE 0x03 -#define PM100_VSM_STATE_WAIT 0x04 -#define PM100_VSM_STATE_READY 0x05 -#define PM100_VSM_STATE_RUNNING 0x06 -#define PM100_VSM_STATE_FAULT 0x07 - -#define PM100_LOCKOUT_DISABLED 0x0 -#define PM100_LOCKOUT_ENABLED 0x1 -#define PM100_CAN_MODE 0x0 -#define PM100_VSM_MODE 0x1 -#define PM100_DIRECTION_REVERSE 0x0 -#define PM100_DIRECTION_FORWARD 0x1 -#define PM100_INVERTER_OFF 0x0 -#define PM100_INVERTER_ON 0x1 -#define PM100_TORQUE_MODE 0x0 -#define PM100_SPEED_MODE 0x1 -#define PM100_SPEED_MODE_DISABLE 0x0 -#define PM100_SPEED_MODE_ENABLE 0x1 +#define PM100_VSM_STATE_WAIT 0x04 +#define PM100_VSM_STATE_READY 0x05 +#define PM100_VSM_STATE_RUNNING 0x06 +#define PM100_VSM_STATE_FAULT 0x07 + +#define PM100_LOCKOUT_DISABLED 0x0 +#define PM100_LOCKOUT_ENABLED 0x1 +#define PM100_CAN_MODE 0x0 +#define PM100_VSM_MODE 0x1 +#define PM100_DIRECTION_REVERSE 0x0 +#define PM100_DIRECTION_FORWARD 0x1 +#define PM100_INVERTER_OFF 0x0 +#define PM100_INVERTER_ON 0x1 +#define PM100_TORQUE_MODE 0x0 +#define PM100_SPEED_MODE 0x1 +#define PM100_SPEED_MODE_DISABLE 0x0 +#define PM100_SPEED_MODE_ENABLE 0x1 #define PM100_DIRECTION_FORWARD 0x1 #define PM100_DIRECTION_REVERSE 0x0 @@ -34,8 +34,8 @@ * internal function prototypes */ static void pm100_thread_entry(ULONG input); -static void process_broadcast(pm100_context_t *pm100_ptr, - const rtcan_msg_t *msg_ptr); +static void process_broadcast(pm100_context_t* pm100_ptr, + const rtcan_msg_t* msg_ptr); /** * @brief Initialises the PM100 service @@ -61,7 +61,7 @@ status_t pm100_init(pm100_context_t* pm100_ptr, status_t status = STATUS_OK; // create service thread - void *stack_ptr = NULL; + void* stack_ptr = NULL; UINT tx_status = tx_byte_allocate(stack_pool_ptr, &stack_ptr, config_ptr->thread.stack_size, @@ -70,9 +70,9 @@ status_t pm100_init(pm100_context_t* pm100_ptr, if (tx_status == TX_SUCCESS) { tx_status = tx_thread_create(&pm100_ptr->thread, - (CHAR *)config_ptr->thread.name, + (CHAR*) config_ptr->thread.name, pm100_thread_entry, - (ULONG)pm100_ptr, + (ULONG) pm100_ptr, stack_ptr, config_ptr->thread.stack_size, config_ptr->thread.priority, @@ -124,17 +124,16 @@ status_t pm100_init(pm100_context_t* pm100_ptr, */ 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; + 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, - CAN_C_PM100_MOTOR_POSITION_INFO_FRAME_ID}; + 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, + CAN_C_PM100_MOTOR_POSITION_INFO_FRAME_ID}; for (uint32_t i = 0; i < sizeof(subscriptions) / sizeof(subscriptions[0]); i++) @@ -146,7 +145,8 @@ void pm100_thread_entry(ULONG input) if (status != RTCAN_OK) { // TODO: update broadcast states with error - LOG_ERROR("Could not subscribe on %d message. Terminating thread\n"); + LOG_ERROR( + "Could not subscribe on %d message. Terminating thread\n"); tx_thread_terminate(&pm100_ptr->thread); } } @@ -154,7 +154,7 @@ void pm100_thread_entry(ULONG input) // process incoming messages, or timeout while (1) { - rtcan_msg_t *msg_ptr = NULL; + rtcan_msg_t* msg_ptr = NULL; UINT status = tx_queue_receive(&pm100_ptr->can_rx_queue, &msg_ptr, config_ptr->broadcast_timeout_ticks); @@ -185,7 +185,7 @@ void pm100_thread_entry(ULONG input) * @param[in] pm100_ptr PM100 context * @param[in] msg_ptr Incoming message */ -void process_broadcast(pm100_context_t *pm100_ptr, const rtcan_msg_t *msg_ptr) +void process_broadcast(pm100_context_t* pm100_ptr, const rtcan_msg_t* msg_ptr) { switch (msg_ptr->identifier) { @@ -204,15 +204,17 @@ void process_broadcast(pm100_context_t *pm100_ptr, const rtcan_msg_t *msg_ptr) msg_ptr->data, msg_ptr->length); - if (pm100_ptr->faults.pm100_run_fault_hi != PM100_NO_FAULTS || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) + if (pm100_ptr->faults.pm100_run_fault_hi != PM100_NO_FAULTS + || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) { pm100_ptr->error |= PM100_ERROR_RUN_FAULT; - (void)pm100_disable(pm100_ptr); + (void) pm100_disable(pm100_ptr); } - else if (pm100_ptr->faults.pm100_post_fault_hi != PM100_NO_FAULTS || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) + else if (pm100_ptr->faults.pm100_post_fault_hi != PM100_NO_FAULTS + || pm100_ptr->faults.pm100_post_fault_lo != PM100_NO_FAULTS) { pm100_ptr->error |= PM100_ERROR_POST_FAULT; - (void)pm100_disable(pm100_ptr); + (void) pm100_disable(pm100_ptr); } break; @@ -278,7 +280,7 @@ void process_broadcast(pm100_context_t *pm100_ptr, const rtcan_msg_t *msg_ptr) * * @param[in] pm100_ptr PM100 context */ -status_t pm100_lvs_on(pm100_context_t *pm100_ptr) +status_t pm100_lvs_on(pm100_context_t* pm100_ptr) { // UINT tx_status = tx_thread_resume(&pm100_ptr->thread); @@ -290,10 +292,11 @@ status_t pm100_lvs_on(pm100_context_t *pm100_ptr) * * @param[in] pm100_ptr PM100 context */ -bool pm100_is_precharged(pm100_context_t *pm100_ptr) +bool pm100_is_precharged(pm100_context_t* pm100_ptr) { - UINT tx_status = tx_mutex_get(&pm100_ptr->state_mutex, - pm100_ptr->config_ptr->precharge_timeout_ticks); + UINT tx_status + = tx_mutex_get(&pm100_ptr->state_mutex, + pm100_ptr->config_ptr->precharge_timeout_ticks); bool broadcasts_valid = false; uint8_t vsm_state = PM100_VSM_STATE_FAULT; // assume something safe @@ -302,10 +305,14 @@ bool pm100_is_precharged(pm100_context_t *pm100_ptr) { vsm_state = pm100_ptr->states.pm100_vsm_state; broadcasts_valid = pm100_ptr->broadcasts_valid; - (void)tx_mutex_put(&pm100_ptr->state_mutex); + (void) tx_mutex_put(&pm100_ptr->state_mutex); } - return broadcasts_valid && (vsm_state == PM100_VSM_STATE_PRECHARGE_COMPLETE || vsm_state == PM100_VSM_STATE_WAIT || vsm_state == PM100_VSM_STATE_READY || vsm_state == PM100_VSM_STATE_RUNNING); + return broadcasts_valid + && (vsm_state == PM100_VSM_STATE_PRECHARGE_COMPLETE + || vsm_state == PM100_VSM_STATE_WAIT + || vsm_state == PM100_VSM_STATE_READY + || vsm_state == PM100_VSM_STATE_RUNNING); } int16_t pm100_max_inverter_temp(pm100_context_t* pm100_ptr) @@ -357,7 +364,7 @@ int16_t pm100_motor_temp(pm100_context_t* pm100_ptr) return motor_temp; } -int16_t pm100_motor_speed(pm100_context_t *pm100_ptr) +int16_t pm100_motor_speed(pm100_context_t* pm100_ptr) { int16_t speed = 0; @@ -372,7 +379,7 @@ int16_t pm100_motor_speed(pm100_context_t *pm100_ptr) return speed; } -status_t pm100_lvs_off(pm100_context_t *pm100_ptr) +status_t pm100_lvs_off(pm100_context_t* pm100_ptr) { return STATUS_OK; } @@ -385,7 +392,7 @@ status_t pm100_lvs_off(pm100_context_t *pm100_ptr) * * @param[in] pm100_ptr */ -status_t pm100_disable(pm100_context_t *pm100_ptr) +status_t pm100_disable(pm100_context_t* pm100_ptr) { LOG_INFO("Sending PM100 Disable command\n"); rtcan_msg_t msg = {.identifier = CAN_C_PM100_COMMAND_MESSAGE_FRAME_ID, @@ -412,7 +419,7 @@ status_t pm100_disable(pm100_context_t *pm100_ptr) * @param[in] pm100_ptr PM100 context * @param[in] torque Desired torque */ -status_t pm100_request_torque(pm100_context_t *pm100_ptr, uint16_t torque) +status_t pm100_request_torque(pm100_context_t* pm100_ptr, uint16_t torque) { status_t status = STATUS_OK; @@ -420,29 +427,34 @@ status_t pm100_request_torque(pm100_context_t *pm100_ptr, uint16_t torque) pm100_ptr); // do this before getting mutex to avoid deadlock const bool no_errors = (pm100_ptr->error == PM100_ERROR_NONE); - UINT tx_status = tx_mutex_get(&pm100_ptr->state_mutex, - pm100_ptr->config_ptr->torque_request_timeout_ticks); + UINT tx_status + = tx_mutex_get(&pm100_ptr->state_mutex, + pm100_ptr->config_ptr->torque_request_timeout_ticks); if (no_errors && is_precharged && tx_status == TX_SUCCESS) { if (pm100_ptr->broadcasts_valid) { - if (pm100_ptr->states.pm100_inverter_enable_lockout == PM100_LOCKOUT_DISABLED) + if (pm100_ptr->states.pm100_inverter_enable_lockout + == PM100_LOCKOUT_DISABLED) { - rtcan_msg_t msg = {.identifier = CAN_C_PM100_COMMAND_MESSAGE_FRAME_ID, - .length = CAN_C_PM100_COMMAND_MESSAGE_LENGTH, - .extended = CAN_C_PM100_COMMAND_MESSAGE_IS_EXTENDED, - .data = {0, 0, 0, 0, 0, 0, 0, 0}}; + rtcan_msg_t msg + = {.identifier = CAN_C_PM100_COMMAND_MESSAGE_FRAME_ID, + .length = CAN_C_PM100_COMMAND_MESSAGE_LENGTH, + .extended = CAN_C_PM100_COMMAND_MESSAGE_IS_EXTENDED, + .data = {0, 0, 0, 0, 0, 0, 0, 0}}; - struct can_c_pm100_command_message_t cmd = {.pm100_torque_command = torque, - .pm100_direction_command = PM100_DIRECTION_REVERSE, - .pm100_speed_mode_enable = PM100_SPEED_MODE_DISABLE, - .pm100_inverter_enable = PM100_INVERTER_ON}; + struct can_c_pm100_command_message_t cmd + = {.pm100_torque_command = torque, + .pm100_direction_command = PM100_DIRECTION_REVERSE, + .pm100_speed_mode_enable = PM100_SPEED_MODE_DISABLE, + .pm100_inverter_enable = PM100_INVERTER_ON}; can_c_pm100_command_message_pack(msg.data, &cmd, msg.length); LOG_INFO("Sending torque request\n"); - rtcan_status_t rtcan_status = rtcan_transmit(pm100_ptr->rtcan_c_ptr, &msg); + rtcan_status_t rtcan_status + = rtcan_transmit(pm100_ptr->rtcan_c_ptr, &msg); status = (rtcan_status == RTCAN_OK) ? STATUS_OK : STATUS_ERROR; } else @@ -453,7 +465,7 @@ status_t pm100_request_torque(pm100_context_t *pm100_ptr, uint16_t torque) } } - (void)tx_mutex_put(&pm100_ptr->state_mutex); + (void) tx_mutex_put(&pm100_ptr->state_mutex); } else { diff --git a/src/SUFST/Src/Services/tick.c b/src/SUFST/Src/Services/tick.c index 87598b4..71fda99 100644 --- a/src/SUFST/Src/Services/tick.c +++ b/src/SUFST/Src/Services/tick.c @@ -2,14 +2,14 @@ #define BPS_LIGHT_THRESH 40 -static status_t lock_tick_sensors(tick_context_t *tick_ptr, uint32_t timeout); -static void unlock_tick_sensors(tick_context_t *tick_ptr); -void tick_update_canbc_states(tick_context_t *tick_ptr); +static status_t lock_tick_sensors(tick_context_t* tick_ptr, uint32_t timeout); +static void unlock_tick_sensors(tick_context_t* tick_ptr); +void tick_update_canbc_states(tick_context_t* tick_ptr); static void tick_thread_entry(ULONG input) { - tick_context_t *tick_ptr = (tick_context_t *)input; - const config_tick_t *config_ptr = tick_ptr->config_ptr; + tick_context_t* tick_ptr = (tick_context_t*) input; + const config_tick_t* config_ptr = tick_ptr->config_ptr; while (1) { @@ -17,7 +17,8 @@ static void tick_thread_entry(ULONG input) tick_ptr->bps_status = bps_read(&tick_ptr->bps, &tick_ptr->bps_reading); tick_ptr->brakelight_pwr = (tick_ptr->bps_reading > BPS_LIGHT_THRESH); - tick_ptr->apps_status = apps_read(&tick_ptr->apps, &tick_ptr->apps_reading); + tick_ptr->apps_status + = apps_read(&tick_ptr->apps, &tick_ptr->apps_reading); /*LOG_INFO(tick_ptr->log_ptr, "Brake pressure: %d status: %d\n", tick_ptr->bps_reading, tick_ptr->bps_status);*/ @@ -32,12 +33,12 @@ static void tick_thread_entry(ULONG input) } } -status_t tick_init(tick_context_t *tick_ptr, - canbc_context_t *canbc_ptr, - TX_BYTE_POOL *stack_pool_ptr, - const config_tick_t *config_ptr, - const config_apps_t *apps_config_ptr, - const config_bps_t *bps_config_ptr) +status_t tick_init(tick_context_t* tick_ptr, + canbc_context_t* canbc_ptr, + TX_BYTE_POOL* stack_pool_ptr, + const config_tick_t* config_ptr, + const config_apps_t* apps_config_ptr, + const config_bps_t* bps_config_ptr) { tick_ptr->config_ptr = config_ptr; tick_ptr->canbc_ptr = canbc_ptr; @@ -48,7 +49,7 @@ status_t tick_init(tick_context_t *tick_ptr, status_t status = STATUS_OK; - void *stack_ptr = NULL; + void* stack_ptr = NULL; UINT tx_status = tx_byte_allocate(stack_pool_ptr, &stack_ptr, config_ptr->thread.stack_size, @@ -72,9 +73,9 @@ status_t tick_init(tick_context_t *tick_ptr, if (tx_status == TX_SUCCESS) { tx_status = tx_thread_create(&tick_ptr->thread, - (CHAR *)config_ptr->thread.name, + (CHAR*) config_ptr->thread.name, tick_thread_entry, - (ULONG)tick_ptr, + (ULONG) tick_ptr, stack_ptr, config_ptr->thread.stack_size, config_ptr->thread.priority, @@ -96,9 +97,9 @@ status_t tick_init(tick_context_t *tick_ptr, return status; } -void tick_update_canbc_states(tick_context_t *tick_ptr) +void tick_update_canbc_states(tick_context_t* tick_ptr) { - canbc_states_t *states = canbc_lock_state(tick_ptr->canbc_ptr, TX_NO_WAIT); + canbc_states_t* states = canbc_lock_state(tick_ptr->canbc_ptr, TX_NO_WAIT); if (states != NULL) { @@ -109,7 +110,7 @@ void tick_update_canbc_states(tick_context_t *tick_ptr) } } -static status_t lock_tick_sensors(tick_context_t *tick_ptr, uint32_t timeout) +static status_t lock_tick_sensors(tick_context_t* tick_ptr, uint32_t timeout) { UINT tx_status = tx_mutex_get(&tick_ptr->sensor_mutex, timeout); @@ -123,12 +124,12 @@ static status_t lock_tick_sensors(tick_context_t *tick_ptr, uint32_t timeout) } } -static void unlock_tick_sensors(tick_context_t *tick_ptr) +static void unlock_tick_sensors(tick_context_t* tick_ptr) { tx_mutex_put(&tick_ptr->sensor_mutex); } -status_t tick_get_bps_reading(tick_context_t *tick_ptr, uint16_t *result) +status_t tick_get_bps_reading(tick_context_t* tick_ptr, uint16_t* result) { status_t status = STATUS_ERROR; @@ -146,7 +147,7 @@ status_t tick_get_bps_reading(tick_context_t *tick_ptr, uint16_t *result) return status; } -status_t tick_get_apps_reading(tick_context_t *tick_ptr, uint16_t *result) +status_t tick_get_apps_reading(tick_context_t* tick_ptr, uint16_t* result) { status_t status = STATUS_ERROR; diff --git a/src/SUFST/Src/config.c b/src/SUFST/Src/config.c index f97d3b1..9b33b19 100644 --- a/src/SUFST/Src/config.c +++ b/src/SUFST/Src/config.c @@ -151,6 +151,14 @@ static const config_t config_instance = { }, .broadcast_period_ticks = SECONDS_TO_TICKS(0.1) }, + .canrx = { + .thread = { + .name = "CANRX", + .priority = 3, + .stack_size = 1024 + }, + .broadcast_timeout_ticks = SECONDS_TO_TICKS(10), + }, .heartbeat = { .thread = { .name = "HEARTBEAT", diff --git a/src/SUFST/Src/vcu.c b/src/SUFST/Src/vcu.c index 69bf8c8..ec7ddb1 100644 --- a/src/SUFST/Src/vcu.c +++ b/src/SUFST/Src/vcu.c @@ -24,11 +24,11 @@ * @param[in] app_mem_pool Pointer to RTOS application memory pool * @param[in] config_ptr Pointer to VCU configuration instance */ -status_t vcu_init(vcu_context_t *vcu_ptr, - CAN_HandleTypeDef *can_c_h, - CAN_HandleTypeDef *can_s_h, - TX_BYTE_POOL *app_mem_pool, - const config_t *config_ptr) +status_t vcu_init(vcu_context_t* vcu_ptr, + CAN_HandleTypeDef* can_c_h, + CAN_HandleTypeDef* can_s_h, + TX_BYTE_POOL* app_mem_pool, + const config_t* config_ptr) { vcu_ptr->config_ptr = config_ptr; @@ -37,12 +37,13 @@ status_t vcu_init(vcu_context_t *vcu_ptr, // logging services (first so we can log errors) if (status == STATUS_OK) { - status = log_init(&vcu_ptr->log, app_mem_pool, &vcu_ptr->config_ptr->log); + status + = log_init(&vcu_ptr->log, app_mem_pool, &vcu_ptr->config_ptr->log); } // RTCAN services - rtcan_handle_t *rtcan_handles[] = {&vcu_ptr->rtcan_s, &vcu_ptr->rtcan_c}; - CAN_HandleTypeDef *can_handles[] = {can_s_h, can_c_h}; + rtcan_handle_t* rtcan_handles[] = {&vcu_ptr->rtcan_s, &vcu_ptr->rtcan_c}; + CAN_HandleTypeDef* can_handles[] = {can_s_h, can_c_h}; ULONG rtcan_priorities[] = {vcu_ptr->config_ptr->rtos.rtcan_s_priority, vcu_ptr->config_ptr->rtos.rtcan_c_priority}; @@ -82,6 +83,16 @@ status_t vcu_init(vcu_context_t *vcu_ptr, &vcu_ptr->config_ptr->canbc); } + // CAN recieve service + if (status == STATUS_OK) + { + status = canrx_init(&vcu_ptr->canrx, + &vcu_ptr->rtcan_c, + &vcu_ptr->rtcan_s, + app_mem_pool, + &vcu_ptr->config_ptr->canrx); + } + // dash if (status == STATUS_OK) { @@ -120,6 +131,7 @@ status_t vcu_init(vcu_context_t *vcu_ptr, &vcu_ptr->tick, &vcu_ptr->remote_ctrl, &vcu_ptr->canbc, + &vcu_ptr->canrx, app_mem_pool, &vcu_ptr->config_ptr->ctrl, &vcu_ptr->config_ptr->rtds, @@ -129,12 +141,11 @@ status_t vcu_init(vcu_context_t *vcu_ptr, // remote control if (status == STATUS_OK) { - status = remote_ctrl_init( - &vcu_ptr->remote_ctrl, - &vcu_ptr->canbc, - app_mem_pool, - &vcu_ptr->rtcan_s, - &vcu_ptr->config_ptr->remote_ctrl); + status = remote_ctrl_init(&vcu_ptr->remote_ctrl, + &vcu_ptr->canbc, + app_mem_pool, + &vcu_ptr->rtcan_s, + &vcu_ptr->config_ptr->remote_ctrl); } // heartbeat @@ -157,11 +168,12 @@ status_t vcu_init(vcu_context_t *vcu_ptr, * @param[in] vcu_ptr VCU instance * @param[in] can_h CAN handle from callback */ -status_t vcu_handle_can_tx_mailbox_callback(vcu_context_t *vcu_ptr, - CAN_HandleTypeDef *can_h) +status_t vcu_handle_can_tx_mailbox_callback(vcu_context_t* vcu_ptr, + CAN_HandleTypeDef* can_h) { // TODO: how to handle CAN C vs CAN S - rtcan_status_t status = rtcan_handle_tx_mailbox_callback(&vcu_ptr->rtcan_c, can_h); + rtcan_status_t status + = rtcan_handle_tx_mailbox_callback(&vcu_ptr->rtcan_c, can_h); if (status != RTCAN_OK) { @@ -190,8 +202,8 @@ status_t vcu_handle_can_tx_mailbox_callback(vcu_context_t *vcu_ptr, * @param[in] can_h CAN handle * @param[in] rx_fifo Rx FIFO number */ -status_t vcu_handle_can_rx_it(vcu_context_t *vcu_ptr, - CAN_HandleTypeDef *can_h, +status_t vcu_handle_can_rx_it(vcu_context_t* vcu_ptr, + CAN_HandleTypeDef* can_h, uint32_t rx_fifo) { rtcan_status_t status; @@ -220,7 +232,7 @@ status_t vcu_handle_can_rx_it(vcu_context_t *vcu_ptr, * @param[in] vcu_ptr VCU instance * @param[in] can_h CAN handle from callback */ -status_t vcu_handle_can_err(vcu_context_t *vcu_ptr, CAN_HandleTypeDef *can_h) +status_t vcu_handle_can_err(vcu_context_t* vcu_ptr, CAN_HandleTypeDef* can_h) { rtcan_status_t status; From f2a052bc8aace628fb7aa23e814c1d97f3d90c83 Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 21:53:36 +0100 Subject: [PATCH 06/11] feat: linear torque limiting when bms temp is high --- src/SUFST/Inc/Functions/torque_map.h | 7 ++- src/SUFST/Inc/Services/ctrl.h | 1 + src/SUFST/Inc/config.h | 3 ++ src/SUFST/Src/Functions/torque_map.c | 77 ++++++++++++++++++++++------ src/SUFST/Src/Services/ctrl.c | 5 +- src/SUFST/Src/config.c | 5 +- 6 files changed, 79 insertions(+), 19 deletions(-) diff --git a/src/SUFST/Inc/Functions/torque_map.h b/src/SUFST/Inc/Functions/torque_map.h index 9c3b268..c29bd49 100644 --- a/src/SUFST/Inc/Functions/torque_map.h +++ b/src/SUFST/Inc/Functions/torque_map.h @@ -24,6 +24,10 @@ typedef struct _torque_map_t uint16_t deadzone_end; // end of deadzone float deadzone_scale; // scale factor for inputs const config_torque_map_t* config_ptr; // configuration + uint16_t + temp_min; // minimum Torque request at the end of the limit (Nm *10) + uint8_t temp_start; // BMS temp to start limiting torque (Celcius) + uint8_t temp_end; // BMS temp for maximum toque limiting (Celcius) } torque_map_t; /* @@ -31,6 +35,7 @@ typedef struct _torque_map_t */ status_t torque_map_init(torque_map_t* map_ptr, const config_torque_map_t* config_ptr); -uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input); +uint16_t +torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp); #endif \ No newline at end of file diff --git a/src/SUFST/Inc/Services/ctrl.h b/src/SUFST/Inc/Services/ctrl.h index 0e285ac..922e4db 100644 --- a/src/SUFST/Inc/Services/ctrl.h +++ b/src/SUFST/Inc/Services/ctrl.h @@ -74,6 +74,7 @@ typedef struct int16_t motor_temp; int16_t inv_temp; int8_t max_temp; + uint8_t bms_temp; bool inverter_pwr; bool pump_pwr; diff --git a/src/SUFST/Inc/config.h b/src/SUFST/Inc/config.h index b6c4f12..beb960b 100644 --- a/src/SUFST/Inc/config.h +++ b/src/SUFST/Inc/config.h @@ -116,6 +116,9 @@ typedef struct { uint16_t input_max; // maximum input value (range must be zero to max) uint16_t output_max; // maximum output value (Nm * 10) float deadzone_fraction; // fraction of input range for deadzone + uint16_t temp_min; // minimum Torque request at the end of the limit (Nm *10) + uint8_t temp_start; // BMS temp to start limiting torque (Celcius) + uint8_t temp_end; // BMS temp for maximum toque limiting (Celcius) } config_torque_map_t; /** diff --git a/src/SUFST/Src/Functions/torque_map.c b/src/SUFST/Src/Functions/torque_map.c index a25e829..48d9f0b 100644 --- a/src/SUFST/Src/Functions/torque_map.c +++ b/src/SUFST/Src/Functions/torque_map.c @@ -3,9 +3,11 @@ /* * internal function prototypes */ -static inline uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input); -static uint16_t null_torque_map(torque_map_t *map_ptr, uint16_t input); -static uint16_t linear_torque_map(torque_map_t *map_ptr, uint16_t input); +static inline uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input); +static uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input); +static uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input); +static inline uint16_t +apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp); /** * @brief Initialises the torque map @@ -16,15 +18,18 @@ static uint16_t linear_torque_map(torque_map_t *map_ptr, uint16_t input); * @param[in] map_ptr Torque map * @param[in] config_ptr Configuration */ -status_t torque_map_init(torque_map_t *map_ptr, - const config_torque_map_t *config_ptr) +status_t torque_map_init(torque_map_t* map_ptr, + const config_torque_map_t* config_ptr) { map_ptr->config_ptr = config_ptr; // pre-compute deadzone parameters - map_ptr->deadzone_end = config_ptr->deadzone_fraction * config_ptr->input_max; + map_ptr->deadzone_end + = config_ptr->deadzone_fraction * config_ptr->input_max; - map_ptr->deadzone_scale = ((float)config_ptr->input_max) / ((float)(config_ptr->input_max - map_ptr->deadzone_end)); + map_ptr->deadzone_scale + = ((float) config_ptr->input_max) + / ((float) (config_ptr->input_max - map_ptr->deadzone_end)); // load mapping function status_t status = STATUS_OK; @@ -43,6 +48,10 @@ status_t torque_map_init(torque_map_t *map_ptr, break; }; + map_ptr->temp_min = config_ptr->temp_min; + map_ptr->temp_start = config_ptr->temp_start; + map_ptr->temp_end = config_ptr->temp_end; + return status; } @@ -52,12 +61,13 @@ status_t torque_map_init(torque_map_t *map_ptr, * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t torque_map_apply(torque_map_t *map_ptr, uint16_t input) +uint16_t +torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) { const uint16_t input_deadzone = apply_deadzone(map_ptr, input); const uint16_t torque = map_ptr->map_func(map_ptr, input_deadzone); - - return torque; + const uint16_t limited_torque = apply_temp_limit(map_ptr, torque, bms_temp); + return limited_torque; } /** @@ -67,7 +77,7 @@ uint16_t torque_map_apply(torque_map_t *map_ptr, uint16_t input) * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input) +uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input) { uint16_t result = 0; @@ -78,7 +88,7 @@ uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input) else { const uint16_t shifted_input = input - map_ptr->deadzone_end; - result = (uint16_t)(shifted_input * map_ptr->deadzone_scale); + result = (uint16_t) (shifted_input * map_ptr->deadzone_scale); } return result; @@ -87,7 +97,7 @@ uint16_t apply_deadzone(torque_map_t *map_ptr, uint16_t input) /** * @brief A torque map that returns zero */ -uint16_t null_torque_map(torque_map_t *map_ptr, uint16_t input) +uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input) { UNUSED(map_ptr); UNUSED(input); @@ -97,13 +107,48 @@ uint16_t null_torque_map(torque_map_t *map_ptr, uint16_t input) /** * @brief A linear torque map */ -uint16_t linear_torque_map(torque_map_t *map_ptr, uint16_t input) +uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input) { - const float scale_factor = map_ptr->config_ptr->output_max / (float)map_ptr->config_ptr->input_max; + const float scale_factor = map_ptr->config_ptr->output_max + / (float) map_ptr->config_ptr->input_max; - const uint16_t torque = (uint16_t)(input * scale_factor); + const uint16_t torque = (uint16_t) (input * scale_factor); // TODO: clip to range return torque; +} + +uint16_t +apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) +{ + uint16_t result = 0; + + if (bms_temp < map_ptr->temp_start) + { + result = input; + } + else if (bms_temp > map_ptr->temp_end) + { + result = map_ptr->temp_min; + } + else + { + uint16_t max_torque + = map_ptr->config_ptr->output_max + - (map_ptr->config_ptr->output_max - map_ptr->temp_min) + * (bms_temp - map_ptr->temp_start) + / (map_ptr->temp_end - map_ptr->temp_start); + + if (input < max_torque) + { + result = input; + } + else + { + result = max_torque; + } + } + + return result; } \ No newline at end of file diff --git a/src/SUFST/Src/Services/ctrl.c b/src/SUFST/Src/Services/ctrl.c index 2359d85..f69a82a 100644 --- a/src/SUFST/Src/Services/ctrl.c +++ b/src/SUFST/Src/Services/ctrl.c @@ -423,8 +423,11 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) ctrl_ptr->torque_request = 1500; #endif #else + ctrl_ptr->bms_temp + = ctrl_ptr->canrx_ptr->msgid_x202.bms_high_temperature; ctrl_ptr->torque_request = torque_map_apply(&ctrl_ptr->torque_map, - ctrl_ptr->apps_reading); + ctrl_ptr->apps_reading, + ctrl_ptr->bms_temp); #endif LOG_INFO("ADC: %d, Torque: %d\n", diff --git a/src/SUFST/Src/config.c b/src/SUFST/Src/config.c index 9b33b19..168aba3 100644 --- a/src/SUFST/Src/config.c +++ b/src/SUFST/Src/config.c @@ -111,7 +111,10 @@ static const config_t config_instance = { .function = TORQUE_MAP_LINEAR, .input_max = 100, .output_max = 500, - .deadzone_fraction = 0.28f + .deadzone_fraction = 0.28f, + .temp_min = 300, + .temp_start = 50, + .temp_end = 60 }, .pm100 = { .thread = { From e82fbe6df3a09e19b72a6ecd2913aebe612c90ec Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 21:57:10 +0100 Subject: [PATCH 07/11] feat: add vcu power save mode to can-defs --- src/Middlewares/SUFST/can-defs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Middlewares/SUFST/can-defs b/src/Middlewares/SUFST/can-defs index db6f470..2c1e9c3 160000 --- a/src/Middlewares/SUFST/can-defs +++ b/src/Middlewares/SUFST/can-defs @@ -1 +1 @@ -Subproject commit db6f470da0fb577b4c1306e32ed61f6266e57428 +Subproject commit 2c1e9c3eaefb5ae5d448d078295c1573ada043d1 From 3ca0e87d8e69acb06dfac0b9530499982ce2956c Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 22:16:57 +0100 Subject: [PATCH 08/11] fix: torque request above max temp --- src/SUFST/Src/Functions/torque_map.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/SUFST/Src/Functions/torque_map.c b/src/SUFST/Src/Functions/torque_map.c index 48d9f0b..d93b8ab 100644 --- a/src/SUFST/Src/Functions/torque_map.c +++ b/src/SUFST/Src/Functions/torque_map.c @@ -130,7 +130,14 @@ apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) } else if (bms_temp > map_ptr->temp_end) { - result = map_ptr->temp_min; + if (input < map_ptr->temp_min) + { + result = input; + } + else + { + result = map_ptr->temp_min; + } } else { From abca4e214c15bef2ece4e448ec662f6cb70e4ba9 Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sat, 12 Jul 2025 22:27:25 +0100 Subject: [PATCH 09/11] feat: add power saving mode --- src/SUFST/Inc/Functions/torque_map.h | 6 ++++-- src/SUFST/Inc/Services/ctrl.h | 1 + src/SUFST/Src/Functions/torque_map.c | 26 +++++++++++++++++++------- src/SUFST/Src/Services/ctrl.c | 10 +++++++--- 4 files changed, 31 insertions(+), 12 deletions(-) diff --git a/src/SUFST/Inc/Functions/torque_map.h b/src/SUFST/Inc/Functions/torque_map.h index c29bd49..8da7c65 100644 --- a/src/SUFST/Inc/Functions/torque_map.h +++ b/src/SUFST/Inc/Functions/torque_map.h @@ -35,7 +35,9 @@ typedef struct _torque_map_t */ status_t torque_map_init(torque_map_t* map_ptr, const config_torque_map_t* config_ptr); -uint16_t -torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp); +uint16_t torque_map_apply(torque_map_t* map_ptr, + uint16_t input, + uint8_t bms_temp, + bool* power_saving); #endif \ No newline at end of file diff --git a/src/SUFST/Inc/Services/ctrl.h b/src/SUFST/Inc/Services/ctrl.h index 922e4db..88c7959 100644 --- a/src/SUFST/Inc/Services/ctrl.h +++ b/src/SUFST/Inc/Services/ctrl.h @@ -79,6 +79,7 @@ typedef struct bool inverter_pwr; bool pump_pwr; bool fan_pwr; + bool power_saving; uint32_t neg_air_start; uint32_t precharge_start; // precharge start time in ticks diff --git a/src/SUFST/Src/Functions/torque_map.c b/src/SUFST/Src/Functions/torque_map.c index d93b8ab..dce4330 100644 --- a/src/SUFST/Src/Functions/torque_map.c +++ b/src/SUFST/Src/Functions/torque_map.c @@ -6,8 +6,10 @@ static inline uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input); static uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input); static uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input); -static inline uint16_t -apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp); +static inline uint16_t apply_temp_limit(torque_map_t* map_ptr, + uint16_t input, + uint8_t bms_temp, + bool* power_saving); /** * @brief Initialises the torque map @@ -61,12 +63,15 @@ status_t torque_map_init(torque_map_t* map_ptr, * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t -torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) +uint16_t torque_map_apply(torque_map_t* map_ptr, + uint16_t input, + uint8_t bms_temp, + bool* power_saving) { const uint16_t input_deadzone = apply_deadzone(map_ptr, input); const uint16_t torque = map_ptr->map_func(map_ptr, input_deadzone); - const uint16_t limited_torque = apply_temp_limit(map_ptr, torque, bms_temp); + const uint16_t limited_torque + = apply_temp_limit(map_ptr, torque, bms_temp, power_saving); return limited_torque; } @@ -119,14 +124,17 @@ uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input) return torque; } -uint16_t -apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) +uint16_t apply_temp_limit(torque_map_t* map_ptr, + uint16_t input, + uint8_t bms_temp, + bool* power_saving) { uint16_t result = 0; if (bms_temp < map_ptr->temp_start) { result = input; + *power_saving = false; } else if (bms_temp > map_ptr->temp_end) { @@ -138,6 +146,8 @@ apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) { result = map_ptr->temp_min; } + + *power_saving = true; } else { @@ -155,6 +165,8 @@ apply_temp_limit(torque_map_t* map_ptr, uint16_t input, uint8_t bms_temp) { result = max_torque; } + + *power_saving = true; } return result; diff --git a/src/SUFST/Src/Services/ctrl.c b/src/SUFST/Src/Services/ctrl.c index f69a82a..cfdf355 100644 --- a/src/SUFST/Src/Services/ctrl.c +++ b/src/SUFST/Src/Services/ctrl.c @@ -425,9 +425,11 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) #else ctrl_ptr->bms_temp = ctrl_ptr->canrx_ptr->msgid_x202.bms_high_temperature; - ctrl_ptr->torque_request = torque_map_apply(&ctrl_ptr->torque_map, - ctrl_ptr->apps_reading, - ctrl_ptr->bms_temp); + ctrl_ptr->torque_request + = torque_map_apply(&ctrl_ptr->torque_map, + ctrl_ptr->apps_reading, + ctrl_ptr->bms_temp, + &ctrl_ptr->power_saving); #endif LOG_INFO("ADC: %d, Torque: %d\n", @@ -651,6 +653,7 @@ void ctrl_handle_ts_fault(ctrl_context_t* ctrl_ptr) ctrl_ptr->inverter_pwr = false; ctrl_ptr->pump_pwr = false; ctrl_ptr->fan_pwr = false; + ctrl_ptr->power_saving = false; trc_set_ts_on(GPIO_PIN_RESET); dash_blink_ts_on_led(dash_ptr, config_ptr->error_led_toggle_ticks); @@ -678,6 +681,7 @@ void ctrl_update_canbc_states(ctrl_context_t* ctrl_ptr) states->pdm.inverter = ctrl_ptr->inverter_pwr; states->pdm.pump = ctrl_ptr->pump_pwr; states->pdm.fan = ctrl_ptr->fan_pwr; + states->state.vcu_power_saving = ctrl_ptr->power_saving; canbc_unlock_state(ctrl_ptr->canbc_ptr); } } From 19d4b76a1d505c724ccaff1407170fd02ff4574e Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sun, 13 Jul 2025 09:57:17 +0100 Subject: [PATCH 10/11] Revert "Merge branch 'feat/temperature-torque-limiting' into disable-inverter+speed-limit" This reverts commit 61f7bb997a6b542975088c43a717fd7e3783e708, reversing changes made to 5e5066e153c987f1049d4bd4805f952a350d8767. --- Makefile | 1 - src/Middlewares/SUFST/can-defs | 2 +- src/SUFST/Inc/Functions/torque_map.h | 8 +- src/SUFST/Inc/Services/canrx.h | 56 ------- src/SUFST/Inc/Services/ctrl.h | 122 +++++++-------- src/SUFST/Inc/config.h | 11 -- src/SUFST/Inc/vcu.h | 30 ++-- src/SUFST/Src/Functions/torque_map.c | 65 +------- src/SUFST/Src/Services/canrx.c | 224 --------------------------- src/SUFST/Src/Services/ctrl.c | 201 ------------------------ src/SUFST/Src/config.c | 11 -- src/SUFST/Src/vcu.c | 52 +++---- 12 files changed, 98 insertions(+), 685 deletions(-) delete mode 100644 src/SUFST/Inc/Services/canrx.h delete mode 100644 src/SUFST/Src/Services/canrx.c diff --git a/Makefile b/Makefile index ff47fe2..17f2e38 100644 --- a/Makefile +++ b/Makefile @@ -101,7 +101,6 @@ src/SUFST/Src/Interfaces/rtds.c \ src/SUFST/Src/Interfaces/scs.c \ src/SUFST/Src/Interfaces/trc.c \ src/SUFST/Src/Services/canbc.c \ -src/SUFST/Src/Services/canrx.c \ src/SUFST/Src/Services/ctrl.c \ src/SUFST/Src/Services/remote_ctrl.c \ src/SUFST/Src/Services/dash.c \ diff --git a/src/Middlewares/SUFST/can-defs b/src/Middlewares/SUFST/can-defs index 2c1e9c3..9528a40 160000 --- a/src/Middlewares/SUFST/can-defs +++ b/src/Middlewares/SUFST/can-defs @@ -1 +1 @@ -Subproject commit 2c1e9c3eaefb5ae5d448d078295c1573ada043d1 +Subproject commit 9528a405d4c66ba1ee01a016f98143db85247724 diff --git a/src/SUFST/Inc/Functions/torque_map.h b/src/SUFST/Inc/Functions/torque_map.h index 64f03db..71df930 100644 --- a/src/SUFST/Inc/Functions/torque_map.h +++ b/src/SUFST/Inc/Functions/torque_map.h @@ -28,10 +28,6 @@ typedef struct _torque_map_t uint16_t speed_min; // minimum Torque request at max speed (Nm *10) uint16_t speed_start; // speed to start limiting torque (rpm) uint16_t speed_end; // speed for max torque limiting (rpm) - uint16_t - temp_min; // minimum Torque request at the end of the limit (Nm *10) - uint8_t temp_start; // BMS temp to start limiting torque (Celcius) - uint8_t temp_end; // BMS temp for maximum toque limiting (Celcius) } torque_map_t; /* @@ -40,8 +36,6 @@ typedef struct _torque_map_t status_t torque_map_init(torque_map_t* map_ptr, const config_torque_map_t* config_ptr); uint16_t -torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed, - uint8_t bms_temp, - bool* power_saving); +torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed); #endif \ No newline at end of file diff --git a/src/SUFST/Inc/Services/canrx.h b/src/SUFST/Inc/Services/canrx.h deleted file mode 100644 index c3e812a..0000000 --- a/src/SUFST/Inc/Services/canrx.h +++ /dev/null @@ -1,56 +0,0 @@ -/****************************************************************************** - * @file canrx.h - * @author Robert Kirkbride (@r-kirkbride, rgak1g24@soton.ac.uk) - * @brief Can Listener Service - * @details This service is responsible for listening for CAN messages required - * by other services - *****************************************************************************/ - -#ifndef CANRX_H -#define CANRX_H - -#include -#include -#include -#include - -#include "config.h" -#include "log.h" -#include "status.h" - -#define CANRX_QUEUE_SIZE 10 // 10 items - -#define CANRX_ERROR_NONE 0x00 // no errors -#define CANRX_ERROR_INIT 0x01 // initialisation error -#define CANRX_ERROR_BROADCAST_TIMEOUT 0x02 // no broadcasts received -#define CANRX_ERROR_POST_FAULT 0x04 // power-on self-test fault -#define CANRX_ERROR_RUN_FAULT 0x08 // runtime fault - -/** - * @brief CANRX context - */ -typedef struct -{ - TX_THREAD thread; - rtcan_handle_t* rtcan_c_ptr; - rtcan_handle_t* rtcan_s_ptr; - TX_QUEUE can_c_rx_queue; - TX_QUEUE can_s_rx_queue; - ULONG can_rx_queue_mem[CANRX_QUEUE_SIZE]; - TX_MUTEX state_mutex; - bool broadcasts_valid; - struct can_s_msgid_0_x202_t msgid_x202; - uint16_t error; - const config_canrx_t* config_ptr; -} canrx_context_t; - -/* - * public functions - */ -status_t canrx_init(canrx_context_t* canrx_ptr, - rtcan_handle_t* rtcan_c_ptr, - rtcan_handle_t* rtcan_s_ptr, - TX_BYTE_POOL* stack_pool_ptr, - const config_canrx_t* config_ptr); - -#endif diff --git a/src/SUFST/Inc/Services/ctrl.h b/src/SUFST/Inc/Services/ctrl.h index 88c7959..b989c3f 100644 --- a/src/SUFST/Inc/Services/ctrl.h +++ b/src/SUFST/Inc/Services/ctrl.h @@ -15,24 +15,23 @@ #include "apps.h" #include "bps.h" #include "canbc.h" -#include "canrx.h" #include "config.h" #include "dash.h" #include "log.h" #include "pm100.h" -#include "remote_ctrl.h" #include "status.h" #include "tick.h" #include "torque_map.h" +#include "remote_ctrl.h" /* * error codes */ -#define CTRL_ERROR_NONE 0x00 -#define CTRL_ERROR_INIT 0x01 // service failed to initialise -#define CTRL_ERROR_TS_READY_TIMEOUT 0x02 // TS ready from TRC timed out -#define CTRL_ERROR_PRECHARGE_TIMEOUT 0x04 // precharge timed out -#define CTRL_ERROR_TRC_RUN_FAULT 0x08 // TRC faulted at runtime +#define CTRL_ERROR_NONE 0x00 +#define CTRL_ERROR_INIT 0x01 // service failed to initialise +#define CTRL_ERROR_TS_READY_TIMEOUT 0x02 // TS ready from TRC timed out +#define CTRL_ERROR_PRECHARGE_TIMEOUT 0x04 // precharge timed out +#define CTRL_ERROR_TRC_RUN_FAULT 0x08 // TRC faulted at runtime #define CTRL_ERROR_INVERTER_RUN_FAULT 0x10 // inverter faulted at runtime /** @@ -40,22 +39,22 @@ */ typedef enum { - CTRL_STATE_TS_BUTTON_WAIT, - CTRL_STATE_WAIT_NEG_AIR, - CTRL_STATE_PRECHARGE_WAIT, - CTRL_STATE_R2D_WAIT, - CTRL_STATE_TS_ON, - CTRL_STATE_R2D_OFF, - CTRL_STATE_R2D_OFF_WAIT, - CTRL_STATE_TS_ACTIVATION_FAILURE, - CTRL_STATE_TS_RUN_FAULT, - CTRL_STATE_SPIN, - CTRL_STATE_APPS_SCS_FAULT, - CTRL_STATE_APPS_BPS_FAULT, - CTRL_STATE_SIM_WAIT_TS_OFF, - CTRL_STATE_SIM_WAIT_TS_ON, - CTRL_STATE_SIM_WAIT_R2D_OFF, - CTRL_STATE_SIM_WAIT_R2D_ON, + CTRL_STATE_TS_BUTTON_WAIT, + CTRL_STATE_WAIT_NEG_AIR, + CTRL_STATE_PRECHARGE_WAIT, + CTRL_STATE_R2D_WAIT, + CTRL_STATE_TS_ON, + CTRL_STATE_R2D_OFF, + CTRL_STATE_R2D_OFF_WAIT, + CTRL_STATE_TS_ACTIVATION_FAILURE, + CTRL_STATE_TS_RUN_FAULT, + CTRL_STATE_SPIN, + CTRL_STATE_APPS_SCS_FAULT, + CTRL_STATE_APPS_BPS_FAULT, + CTRL_STATE_SIM_WAIT_TS_OFF, + CTRL_STATE_SIM_WAIT_TS_ON, + CTRL_STATE_SIM_WAIT_R2D_OFF, + CTRL_STATE_SIM_WAIT_R2D_ON, } ctrl_state_t; /** @@ -63,39 +62,35 @@ typedef enum */ typedef struct { - ctrl_state_t state; // state machine state - TX_THREAD thread; // service thread - uint16_t apps_reading; // APPS reading (% * 10) - uint16_t bps_reading; // BPS reading (% * 10) - int16_t sagl_reading; // steering angle reading (deg * 10) - int16_t motor_speed_reading; // motor speed reading (rpm) - uint16_t torque_request; // last torque request - uint8_t shdn_reading; - int16_t motor_temp; - int16_t inv_temp; - int8_t max_temp; - uint8_t bms_temp; + ctrl_state_t state; // state machine state + TX_THREAD thread; // service thread + uint16_t apps_reading; // APPS reading (% * 10) + uint16_t bps_reading; // BPS reading (% * 10) + int16_t sagl_reading; // steering angle reading (deg * 10) + int16_t motor_speed_reading; // motor speed reading (rpm) + uint16_t torque_request; // last torque request + uint8_t shdn_reading; + int16_t motor_temp; + int16_t inv_temp; + int8_t max_temp; - bool inverter_pwr; - bool pump_pwr; - bool fan_pwr; - bool power_saving; + bool inverter_pwr; + bool pump_pwr; + bool fan_pwr; - uint32_t neg_air_start; - uint32_t precharge_start; // precharge start time in ticks - uint32_t motor_torque_zero_start; - uint32_t apps_bps_start; - dash_context_t* dash_ptr; // dash service - pm100_context_t* pm100_ptr; // PM100 service - canbc_context_t* canbc_ptr; // CANBC service - canrx_context_t* canrx_ptr; // CANRX service - tick_context_t* tick_ptr; // tick thread (reads certain sensors) - remote_ctrl_context_t* - remote_ctrl_ptr; // tick thread (reads certain sensors) - torque_map_t torque_map; // torque map (APPS -> torque request) + uint32_t neg_air_start; + uint32_t precharge_start; // precharge start time in ticks + uint32_t motor_torque_zero_start; + uint32_t apps_bps_start; + dash_context_t *dash_ptr; // dash service + pm100_context_t *pm100_ptr; // PM100 service + canbc_context_t *canbc_ptr; // CANBC service + tick_context_t *tick_ptr; // tick thread (reads certain sensors) + remote_ctrl_context_t *remote_ctrl_ptr; // tick thread (reads certain sensors) + torque_map_t torque_map; // torque map (APPS -> torque request) - const config_ctrl_t* config_ptr; // config - const config_rtds_t* rtds_config_ptr; // RTDS config + const config_ctrl_t *config_ptr; // config + const config_rtds_t *rtds_config_ptr; // RTDS config uint8_t error; // error code @@ -104,16 +99,15 @@ typedef struct /* * public functions */ -status_t ctrl_init(ctrl_context_t* ctrl_ptr, - dash_context_t* dash_ptr, - pm100_context_t* pm100_ptr, - tick_context_t* tick_ptr, - remote_ctrl_context_t* remote_ctrl_ptr, - canbc_context_t* canbc_ptr, - canrx_context_t* canrx_ptr, - TX_BYTE_POOL* stack_pool_ptr, - const config_ctrl_t* config_ptr, - const config_rtds_t* rtds_config_ptr, - const config_torque_map_t* torque_map_config_ptr); +status_t ctrl_init(ctrl_context_t *ctrl_ptr, + dash_context_t *dash_ptr, + pm100_context_t *pm100_ptr, + tick_context_t *tick_ptr, + remote_ctrl_context_t *remote_ctrl_ptr, + canbc_context_t *canbc_ptr, + TX_BYTE_POOL *stack_pool_ptr, + const config_ctrl_t *config_ptr, + const config_rtds_t *rtds_config_ptr, + const config_torque_map_t *torque_map_config_ptr); #endif diff --git a/src/SUFST/Inc/config.h b/src/SUFST/Inc/config.h index 6d83915..6436246 100644 --- a/src/SUFST/Inc/config.h +++ b/src/SUFST/Inc/config.h @@ -119,9 +119,6 @@ typedef struct { uint16_t speed_min; // minimum Torque request at max speed (Nm *10) uint16_t speed_start; // speed to start limiting torque (rpm) uint16_t speed_end; // speed for max torque limiting (rpm) - uint16_t temp_min; // minimum Torque request at the end of the limit (Nm *10) - uint8_t temp_start; // BMS temp to start limiting torque (Celcius) - uint8_t temp_end; // BMS temp for maximum toque limiting (Celcius) } config_torque_map_t; /** @@ -143,13 +140,6 @@ typedef struct { uint32_t broadcast_period_ticks; // ticks between broadcasts } config_canbc_t; -/** - * @brief CAN recieving service - */ -typedef struct { - config_thread_t thread; // CANRX thread config - uint32_t broadcast_timeout_ticks; // maximum number of ticks to wait for a broadcast -} config_canrx_t; typedef struct { config_thread_t thread; // thread config @@ -229,7 +219,6 @@ typedef struct { config_tick_t tick; config_remote_ctrl_t remote_ctrl; config_canbc_t canbc; - config_canrx_t canrx; config_heartbeat_t heartbeat; config_log_t log; config_rtos_t rtos; diff --git a/src/SUFST/Inc/vcu.h b/src/SUFST/Inc/vcu.h index ec6dd90..75ada43 100644 --- a/src/SUFST/Inc/vcu.h +++ b/src/SUFST/Inc/vcu.h @@ -14,16 +14,15 @@ #include #include "canbc.h" -#include "canrx.h" #include "config.h" #include "ctrl.h" #include "dash.h" #include "heartbeat.h" #include "log.h" #include "pm100.h" -#include "remote_ctrl.h" -#include "status.h" #include "tick.h" +#include "status.h" +#include "remote_ctrl.h" /** * @brief VCU context @@ -32,19 +31,18 @@ */ typedef struct { - rtcan_handle_t rtcan_s; // RTCAN service for sensors CAN bus - rtcan_handle_t rtcan_c; // RTCAN service for critical systems CAN bus - canbc_context_t canbc; // CAN broadcasting service instance - canrx_context_t canrx; // CAN recieving service instance - dash_context_t dash; // dash service - ctrl_context_t ctrl; // control service - pm100_context_t pm100; // PM100 service - tick_context_t tick; - remote_ctrl_context_t remote_ctrl; - heartbeat_context_t heartbeat; // heartbeat service - log_context_t log; // logging service - uint32_t err; // current error code - const config_t* config_ptr; // pointer to global VCU configuration + rtcan_handle_t rtcan_s; // RTCAN service for sensors CAN bus + rtcan_handle_t rtcan_c; // RTCAN service for critical systems CAN bus + canbc_context_t canbc; // CAN broadcasting service instance + dash_context_t dash; // dash service + ctrl_context_t ctrl; // control service + pm100_context_t pm100; // PM100 service + tick_context_t tick; + remote_ctrl_context_t remote_ctrl; + heartbeat_context_t heartbeat; // heartbeat service + log_context_t log; // logging service + uint32_t err; // current error code + const config_t* config_ptr; // pointer to global VCU configuration } vcu_context_t; diff --git a/src/SUFST/Src/Functions/torque_map.c b/src/SUFST/Src/Functions/torque_map.c index 415dfaf..4a30566 100644 --- a/src/SUFST/Src/Functions/torque_map.c +++ b/src/SUFST/Src/Functions/torque_map.c @@ -7,9 +7,7 @@ static inline uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input); static uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input); static uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input); static inline uint16_t -apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed, - uint8_t bms_temp, - bool* power_saving); +apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed); /** * @brief Initialises the torque map @@ -53,9 +51,6 @@ status_t torque_map_init(torque_map_t* map_ptr, map_ptr->speed_min = config_ptr->speed_min; map_ptr->speed_start = config_ptr->speed_start; map_ptr->speed_end = config_ptr->speed_end; - map_ptr->temp_min = config_ptr->temp_min; - map_ptr->temp_start = config_ptr->temp_start; - map_ptr->temp_end = config_ptr->temp_end; return status; } @@ -66,16 +61,12 @@ status_t torque_map_init(torque_map_t* map_ptr, * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed, - uint8_t bms_temp, - bool* power_saving) +uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed) { const uint16_t input_deadzone = apply_deadzone(map_ptr, input); const uint16_t torque = map_ptr->map_func(map_ptr, input_deadzone); - const uint16_t speed_limited_torque = apply_speed_limit(map_ptr, torque, speed); - const uint16_t temp_limited_torque - = apply_temp_limit(map_ptr, speed_limited_torque, bms_temp, power_saving); - return temp_limited_torque; + const uint16_t limited_torque = apply_speed_limit(map_ptr, torque, speed); + return limited_torque; } /** @@ -164,53 +155,5 @@ apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed) } } - return result; -} - - uint16_t apply_temp_limit(torque_map_t* map_ptr, - uint16_t input, - uint8_t bms_temp, - bool* power_saving) -{ - uint16_t result = 0; - - if (bms_temp < map_ptr->temp_start) - { - result = input; - *power_saving = false; - } - else if (bms_temp > map_ptr->temp_end) - { - if (input < map_ptr->temp_min) - { - result = input; - } - else - { - result = map_ptr->temp_min; - } - - *power_saving = true; - } - else - { - uint16_t max_torque - = map_ptr->config_ptr->output_max - - (map_ptr->config_ptr->output_max - map_ptr->temp_min) - * (bms_temp - map_ptr->temp_start) - / (map_ptr->temp_end - map_ptr->temp_start); - - if (input < max_torque) - { - result = input; - } - else - { - result = max_torque; - } - - *power_saving = true; - } - return result; } \ No newline at end of file diff --git a/src/SUFST/Src/Services/canrx.c b/src/SUFST/Src/Services/canrx.c deleted file mode 100644 index 33576cc..0000000 --- a/src/SUFST/Src/Services/canrx.c +++ /dev/null @@ -1,224 +0,0 @@ -#include "canrx.h" - -#include -#include - -/* - * internal function prototypes - */ -static void canrx_thread_entry(ULONG input); -static void process_broadcast(canrx_context_t* canrx_ptr, - const rtcan_msg_t* msg_ptr); - -/** - * @brief Initialises the CANRX service - * - * @param[in] canrx_ptr CANRX context - * @param[in] stack_pool_ptr Memory pool for service thread stack - * @param[in] rtcan_c_ptr RTCAN C instance for receiving broadcasts - * @param[in] rtcan_s_ptr RTCAN S instance for sending precharge cmd - * @param[in] config_ptr Configuration - */ -status_t canrx_init(canrx_context_t* canrx_ptr, - rtcan_handle_t* rtcan_c_ptr, - rtcan_handle_t* rtcan_s_ptr, - TX_BYTE_POOL* stack_pool_ptr, - const config_canrx_t* config_ptr) -{ - canrx_ptr->config_ptr = config_ptr; - canrx_ptr->rtcan_c_ptr = rtcan_c_ptr; - canrx_ptr->rtcan_s_ptr = rtcan_s_ptr; - canrx_ptr->error = CANRX_ERROR_NONE; - canrx_ptr->broadcasts_valid = false; - - status_t status = STATUS_OK; - - // create service thread - void* stack_ptr = NULL; - UINT tx_status = tx_byte_allocate(stack_pool_ptr, - &stack_ptr, - config_ptr->thread.stack_size, - TX_NO_WAIT); - - if (tx_status == TX_SUCCESS) - { - tx_status = tx_thread_create(&canrx_ptr->thread, - (CHAR*) config_ptr->thread.name, - canrx_thread_entry, - (ULONG) canrx_ptr, - stack_ptr, - config_ptr->thread.stack_size, - config_ptr->thread.priority, - config_ptr->thread.priority, - TX_NO_TIME_SLICE, - TX_AUTO_START); - } - - // create CAN receive queue - if (tx_status == TX_SUCCESS) - { - tx_status = tx_queue_create(&canrx_ptr->can_c_rx_queue, - NULL, - TX_1_ULONG, - canrx_ptr->can_rx_queue_mem, - sizeof(canrx_ptr->can_rx_queue_mem)); - } - - if (tx_status == TX_SUCCESS) - { - tx_status = tx_queue_create(&canrx_ptr->can_s_rx_queue, - NULL, - TX_1_ULONG, - canrx_ptr->can_rx_queue_mem, - sizeof(canrx_ptr->can_rx_queue_mem)); - } - - // create state mutex - if (tx_status == TX_SUCCESS) - { - tx_status = tx_mutex_create(&canrx_ptr->state_mutex, NULL, TX_INHERIT); - } - - if (tx_status != TX_SUCCESS) - { - status = STATUS_ERROR; - } - - // check all ok - if (status != STATUS_OK) - { - tx_thread_terminate(&canrx_ptr->thread); - } - - // turn off power - HAL_GPIO_WritePin(STATUS_GPIO_Port, // This pin used to be called STATUS - STATUS_Pin, - GPIO_PIN_RESET); - - return status; -} - -/** - * @brief CANRX service thread entry function - * - * @details This thread is responsible for receiving and processing broadcast - * messages from the CANRX - */ -void canrx_thread_entry(ULONG input) -{ - canrx_context_t* canrx_ptr = (canrx_context_t*) input; - const config_canrx_t* config_ptr = canrx_ptr->config_ptr; - - // set up RTCAN subscriptions - /* This is commented, as there are currently no can_c_subscriptions - Uncomment to add a can_c_subscription - - uint32_t can_c_subscriptions[0]; - - for (uint32_t i = 0; - i < sizeof(can_c_subscriptions) / sizeof(can_c_subscriptions[0]); - i++) - { - rtcan_status_t status = rtcan_subscribe(canrx_ptr->rtcan_c_ptr, - can_c_subscriptions[i], - &canrx_ptr->can_c_rx_queue); - - if (status != RTCAN_OK) - { - // TODO: update broadcast states with error - tx_thread_terminate(&canrx_ptr->thread); - } - } - */ - - uint32_t can_s_subscriptions[] = {CAN_S_MSGID_0_X202_FRAME_ID}; - - for (uint32_t i = 0; - i < sizeof(can_s_subscriptions) / sizeof(can_s_subscriptions[0]); - i++) - { - rtcan_status_t status = rtcan_subscribe(canrx_ptr->rtcan_c_ptr, - can_s_subscriptions[i], - &canrx_ptr->can_s_rx_queue); - - if (status != RTCAN_OK) - { - // TODO: update broadcast states with error - tx_thread_terminate(&canrx_ptr->thread); - } - } - - // process incoming messages, or timeout - while (1) - { - UINT status = TX_QUEUE_ERROR; - rtcan_msg_t* msg_ptr = NULL; - UINT can_s_status - = tx_queue_receive(&canrx_ptr->can_s_rx_queue, - &msg_ptr, - config_ptr->broadcast_timeout_ticks); - - if (can_s_status == TX_SUCCESS && msg_ptr != NULL) - { - canrx_ptr->broadcasts_valid = true; - process_broadcast(canrx_ptr, msg_ptr); - rtcan_msg_consumed(canrx_ptr->rtcan_s_ptr, msg_ptr); - status = TX_SUCCESS; - } - - msg_ptr = NULL; - UINT can_c_status - = tx_queue_receive(&canrx_ptr->can_c_rx_queue, - &msg_ptr, - config_ptr->broadcast_timeout_ticks); - - if (can_c_status == TX_SUCCESS && msg_ptr != NULL) - { - canrx_ptr->broadcasts_valid = true; - process_broadcast(canrx_ptr, msg_ptr); - rtcan_msg_consumed(canrx_ptr->rtcan_c_ptr, msg_ptr); - status = TX_SUCCESS; - } - - if (can_s_status == TX_QUEUE_EMPTY && can_c_status == TX_QUEUE_EMPTY) - { - canrx_ptr->broadcasts_valid = false; - LOG_ERROR("CANRX recieving timed out\n"); - } - else if (status == TX_SUCCESS) - { - canrx_ptr->broadcasts_valid = true; - process_broadcast(canrx_ptr, msg_ptr); - rtcan_msg_consumed(canrx_ptr->rtcan_c_ptr, msg_ptr); - } - else - { - LOG_ERROR("CANRX other error\n"); - } - } -} - -/** - * @brief Processes incoming broadcast messages - * - * @param[in] canrx_ptr CANRX context - * @param[in] msg_ptr Incoming message - */ -void process_broadcast(canrx_context_t* canrx_ptr, const rtcan_msg_t* msg_ptr) -{ - switch (msg_ptr->identifier) - { - - case CAN_S_MSGID_0_X202_FRAME_ID: - { - can_s_msgid_0_x202_unpack(&canrx_ptr->msgid_x202, - msg_ptr->data, - msg_ptr->length); - - break; - } - - default: - break; - } -} diff --git a/src/SUFST/Src/Services/ctrl.c b/src/SUFST/Src/Services/ctrl.c index 4cf9c01..0c6349f 100644 --- a/src/SUFST/Src/Services/ctrl.c +++ b/src/SUFST/Src/Services/ctrl.c @@ -34,7 +34,6 @@ bool ctrl_fan_passed_off_threshold(ctrl_context_t* ctrl_ptr); * @param[in] ctrl_ptr Control context * @param[in] dash_ptr Dash context * @param[in] canbc_ptr CANBC context - * @param[in] canrx_ptr CANRX context * @param[in] pm100_ptr PM100 context * @param[in] stack_pool_ptr Byte pool to allocate thread stack from * @param[in] config_ptr Configuration @@ -49,7 +48,6 @@ status_t ctrl_init(ctrl_context_t* ctrl_ptr, tick_context_t* tick_ptr, remote_ctrl_context_t* remote_ctrl_ptr, canbc_context_t* canbc_ptr, - canrx_context_t* canrx_ptr, TX_BYTE_POOL* stack_pool_ptr, const config_ctrl_t* config_ptr, const config_rtds_t* rtds_config_ptr, @@ -60,7 +58,6 @@ status_t ctrl_init(ctrl_context_t* ctrl_ptr, ctrl_ptr->pm100_ptr = pm100_ptr; ctrl_ptr->tick_ptr = tick_ptr; ctrl_ptr->canbc_ptr = canbc_ptr; - ctrl_ptr->canrx_ptr = canrx_ptr; ctrl_ptr->config_ptr = config_ptr; ctrl_ptr->rtds_config_ptr = rtds_config_ptr; ctrl_ptr->error = CTRL_ERROR_NONE; @@ -340,202 +337,6 @@ void ctrl_state_machine_tick(ctrl_context_t* ctrl_ptr) rtds_activate(ctrl_ptr->rtds_config_ptr); ctrl_ptr->pump_pwr = 1; -#ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_ON; -#else - next_state = CTRL_STATE_TS_ON; -#endif - LOG_INFO("R2D active\n"); - } - } - else - { - LOG_ERROR("BPS reading failed\n"); - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - } - } - break; - } - - // the TS is on - case (CTRL_STATE_TS_ON): - { - // read from the APPS - status_t pm100_status; - - status_t apps_status = ctrl_get_apps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->apps_reading); - status_t bps_status = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - if (dash_ptr->r2d_flag) - { - dash_clear_buttons(dash_ptr); -#ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_R2D_OFF; -#else - next_state = CTRL_STATE_R2D_OFF; -#endif - } - else if (apps_status == STATUS_OK && bps_status == STATUS_OK) - { - // Check for brake + accel pedal pressed - if (ctrl_ptr->apps_reading - >= ctrl_ptr->config_ptr->apps_bps_high_threshold - && ctrl_ptr->bps_reading > BPS_ON_THRESH) - { - LOG_ERROR("BP and AP pressed\n"); - - if (tx_time_get() >= ctrl_ptr->apps_bps_start - + (TX_TIMER_TICKS_PER_SECOND / 3)) - { - LOG_ERROR("BP-AP fault\n"); - // next_state = CTRL_STATE_APPS_BPS_FAULT; - } - } - else - { - ctrl_ptr->apps_bps_start = tx_time_get(); - } -#ifdef VCU_SIMULATION_MODE -#ifndef VCU_SIMULATION_ON_POWER - ctrl_ptr->torque_request - = remote_get_torque_reading(remote_ctrl_ptr); -#else - uint16_t power = remote_get_power_reading(remote_ctrl_ptr); - - int16_t motor_speed = pm100_motor_speed(ctrl_ptr->pm100_ptr); - uint16_t rad_s = 1; - - // this if to be removed - if (motor_speed < 10) - { - motor_speed = 10; - } - // rpm to rad/s - rad_s = (uint16_t) (motor_speed * 0.10472); - if (rad_s == 0) - rad_s = 1; - ctrl_ptr->torque_request = (uint16_t) (power / rad_s); - if (ctrl_ptr->torque_request > 1500) - ctrl_ptr->torque_request = 1500; -#endif -#else - ctrl_ptr->bms_temp - = ctrl_ptr->canrx_ptr->msgid_x202.bms_high_temperature; - ctrl_ptr->torque_request - = torque_map_apply(&ctrl_ptr->torque_map, - ctrl_ptr->apps_reading, - ctrl_ptr->bms_temp, - &ctrl_ptr->power_saving); -#endif - - LOG_INFO("ADC: %d, Torque: %d\n", - ctrl_ptr->apps_reading, - ctrl_ptr->torque_request); - - pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, - ctrl_ptr->torque_request); - - if (pm100_status != STATUS_OK) - { - next_state = CTRL_STATE_TS_RUN_FAULT; - } - } - else - { - LOG_ERROR("APPS / BPS fault\n"); - next_state = CTRL_STATE_TS_RUN_FAULT; - } - - break; - } - - case CTRL_STATE_R2D_OFF: - { - ctrl_ptr->torque_request = 0; - status_t pm100_status = pm100_request_torque(ctrl_ptr->pm100_ptr, 0); - ctrl_ptr->motor_torque_zero_start = tx_time_get(); - ctrl_ptr->pump_pwr = 0; - - break; - } - - // TS is ready, can initiate pre-charge sequence - // TS on LED turns solid - 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)) - { -#ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_TS_ON; -#else - next_state = CTRL_STATE_R2D_WAIT; -#endif - dash_clear_buttons(dash_ptr); - LOG_INFO("Precharge complete\n"); - } - else if (charge_time >= config_ptr->precharge_timeout_ticks) - { - ctrl_ptr->error |= CTRL_ERROR_PRECHARGE_TIMEOUT; - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - LOG_ERROR("Precharge timeout reached\n"); - } - - break; - } - - // pre-charge is complete, wait for R2D signal - // also wait for brake to be fully pressed (if enabled) - case (CTRL_STATE_R2D_WAIT): - { - if (!trc_ready()) - { - LOG_ERROR("SHDN opened\n"); - next_state = CTRL_STATE_TS_ACTIVATION_FAILURE; - } - else if (dash_ptr->tson_flag) // TSON pressed, disable TS - { - dash_ptr->tson_flag = false; - - ctrl_ptr->inverter_pwr = false; // Turn off inverter - trc_set_ts_on(GPIO_PIN_RESET); // Turn off AIRs - -#ifdef VCU_SIMULATION_MODE - next_state = CTRL_STATE_SIM_WAIT_TS_OFF; -#else - next_state = CTRL_STATE_TS_BUTTON_WAIT; -#endif - } - else if (dash_ptr->r2d_flag) // R2D pressed - { -#ifndef VCU_SIMULATION_MODE - dash_ptr->r2d_flag = false; -#endif - - status_t result = ctrl_get_bps_reading(ctrl_ptr->tick_ptr, - remote_ctrl_ptr, - &ctrl_ptr->bps_reading); - - bool r2d = false; - - if (result == STATUS_OK) - { - r2d = (config_ptr->r2d_requires_brake) - ? (ctrl_ptr->bps_reading > BPS_ON_THRESH) - : 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; - #ifdef VCU_SIMULATION_MODE next_state = CTRL_STATE_SIM_WAIT_R2D_ON; #else @@ -847,7 +648,6 @@ void ctrl_handle_ts_fault(ctrl_context_t* ctrl_ptr) ctrl_ptr->inverter_pwr = false; ctrl_ptr->pump_pwr = false; ctrl_ptr->fan_pwr = false; - ctrl_ptr->power_saving = false; trc_set_ts_on(GPIO_PIN_RESET); dash_blink_ts_on_led(dash_ptr, config_ptr->error_led_toggle_ticks); @@ -875,7 +675,6 @@ void ctrl_update_canbc_states(ctrl_context_t* ctrl_ptr) states->pdm.inverter = ctrl_ptr->inverter_pwr; states->pdm.pump = ctrl_ptr->pump_pwr; states->pdm.fan = ctrl_ptr->fan_pwr; - states->state.vcu_power_saving = ctrl_ptr->power_saving; canbc_unlock_state(ctrl_ptr->canbc_ptr); } } diff --git a/src/SUFST/Src/config.c b/src/SUFST/Src/config.c index 5fecab0..23b4d02 100644 --- a/src/SUFST/Src/config.c +++ b/src/SUFST/Src/config.c @@ -115,9 +115,6 @@ static const config_t config_instance = { .speed_min = 500, .speed_start = 2000, .speed_end = 3000 - .temp_min = 300, - .temp_start = 50, - .temp_end = 60 }, .pm100 = { .thread = { @@ -157,14 +154,6 @@ static const config_t config_instance = { }, .broadcast_period_ticks = SECONDS_TO_TICKS(0.1) }, - .canrx = { - .thread = { - .name = "CANRX", - .priority = 3, - .stack_size = 1024 - }, - .broadcast_timeout_ticks = SECONDS_TO_TICKS(10), - }, .heartbeat = { .thread = { .name = "HEARTBEAT", diff --git a/src/SUFST/Src/vcu.c b/src/SUFST/Src/vcu.c index ec7ddb1..69bf8c8 100644 --- a/src/SUFST/Src/vcu.c +++ b/src/SUFST/Src/vcu.c @@ -24,11 +24,11 @@ * @param[in] app_mem_pool Pointer to RTOS application memory pool * @param[in] config_ptr Pointer to VCU configuration instance */ -status_t vcu_init(vcu_context_t* vcu_ptr, - CAN_HandleTypeDef* can_c_h, - CAN_HandleTypeDef* can_s_h, - TX_BYTE_POOL* app_mem_pool, - const config_t* config_ptr) +status_t vcu_init(vcu_context_t *vcu_ptr, + CAN_HandleTypeDef *can_c_h, + CAN_HandleTypeDef *can_s_h, + TX_BYTE_POOL *app_mem_pool, + const config_t *config_ptr) { vcu_ptr->config_ptr = config_ptr; @@ -37,13 +37,12 @@ status_t vcu_init(vcu_context_t* vcu_ptr, // logging services (first so we can log errors) if (status == STATUS_OK) { - status - = log_init(&vcu_ptr->log, app_mem_pool, &vcu_ptr->config_ptr->log); + status = log_init(&vcu_ptr->log, app_mem_pool, &vcu_ptr->config_ptr->log); } // RTCAN services - rtcan_handle_t* rtcan_handles[] = {&vcu_ptr->rtcan_s, &vcu_ptr->rtcan_c}; - CAN_HandleTypeDef* can_handles[] = {can_s_h, can_c_h}; + rtcan_handle_t *rtcan_handles[] = {&vcu_ptr->rtcan_s, &vcu_ptr->rtcan_c}; + CAN_HandleTypeDef *can_handles[] = {can_s_h, can_c_h}; ULONG rtcan_priorities[] = {vcu_ptr->config_ptr->rtos.rtcan_s_priority, vcu_ptr->config_ptr->rtos.rtcan_c_priority}; @@ -83,16 +82,6 @@ status_t vcu_init(vcu_context_t* vcu_ptr, &vcu_ptr->config_ptr->canbc); } - // CAN recieve service - if (status == STATUS_OK) - { - status = canrx_init(&vcu_ptr->canrx, - &vcu_ptr->rtcan_c, - &vcu_ptr->rtcan_s, - app_mem_pool, - &vcu_ptr->config_ptr->canrx); - } - // dash if (status == STATUS_OK) { @@ -131,7 +120,6 @@ status_t vcu_init(vcu_context_t* vcu_ptr, &vcu_ptr->tick, &vcu_ptr->remote_ctrl, &vcu_ptr->canbc, - &vcu_ptr->canrx, app_mem_pool, &vcu_ptr->config_ptr->ctrl, &vcu_ptr->config_ptr->rtds, @@ -141,11 +129,12 @@ status_t vcu_init(vcu_context_t* vcu_ptr, // remote control if (status == STATUS_OK) { - status = remote_ctrl_init(&vcu_ptr->remote_ctrl, - &vcu_ptr->canbc, - app_mem_pool, - &vcu_ptr->rtcan_s, - &vcu_ptr->config_ptr->remote_ctrl); + status = remote_ctrl_init( + &vcu_ptr->remote_ctrl, + &vcu_ptr->canbc, + app_mem_pool, + &vcu_ptr->rtcan_s, + &vcu_ptr->config_ptr->remote_ctrl); } // heartbeat @@ -168,12 +157,11 @@ status_t vcu_init(vcu_context_t* vcu_ptr, * @param[in] vcu_ptr VCU instance * @param[in] can_h CAN handle from callback */ -status_t vcu_handle_can_tx_mailbox_callback(vcu_context_t* vcu_ptr, - CAN_HandleTypeDef* can_h) +status_t vcu_handle_can_tx_mailbox_callback(vcu_context_t *vcu_ptr, + CAN_HandleTypeDef *can_h) { // TODO: how to handle CAN C vs CAN S - rtcan_status_t status - = rtcan_handle_tx_mailbox_callback(&vcu_ptr->rtcan_c, can_h); + rtcan_status_t status = rtcan_handle_tx_mailbox_callback(&vcu_ptr->rtcan_c, can_h); if (status != RTCAN_OK) { @@ -202,8 +190,8 @@ status_t vcu_handle_can_tx_mailbox_callback(vcu_context_t* vcu_ptr, * @param[in] can_h CAN handle * @param[in] rx_fifo Rx FIFO number */ -status_t vcu_handle_can_rx_it(vcu_context_t* vcu_ptr, - CAN_HandleTypeDef* can_h, +status_t vcu_handle_can_rx_it(vcu_context_t *vcu_ptr, + CAN_HandleTypeDef *can_h, uint32_t rx_fifo) { rtcan_status_t status; @@ -232,7 +220,7 @@ status_t vcu_handle_can_rx_it(vcu_context_t* vcu_ptr, * @param[in] vcu_ptr VCU instance * @param[in] can_h CAN handle from callback */ -status_t vcu_handle_can_err(vcu_context_t* vcu_ptr, CAN_HandleTypeDef* can_h) +status_t vcu_handle_can_err(vcu_context_t *vcu_ptr, CAN_HandleTypeDef *can_h) { rtcan_status_t status; From 0aeacbc03d3e82a3608f87e25a5f011481f74cdd Mon Sep 17 00:00:00 2001 From: r-kirkbride <114223050+r-kirkbride@users.noreply.github.com> Date: Sun, 13 Jul 2025 21:40:27 +0100 Subject: [PATCH 11/11] fix: judder from start --- src/SUFST/Inc/Functions/torque_map.h | 3 +-- src/SUFST/Src/Functions/torque_map.c | 7 +++---- src/SUFST/Src/config.c | 8 ++++---- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/SUFST/Inc/Functions/torque_map.h b/src/SUFST/Inc/Functions/torque_map.h index 71df930..0fa6948 100644 --- a/src/SUFST/Inc/Functions/torque_map.h +++ b/src/SUFST/Inc/Functions/torque_map.h @@ -35,7 +35,6 @@ typedef struct _torque_map_t */ status_t torque_map_init(torque_map_t* map_ptr, const config_torque_map_t* config_ptr); -uint16_t -torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed); +uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input, int16_t speed); #endif \ No newline at end of file diff --git a/src/SUFST/Src/Functions/torque_map.c b/src/SUFST/Src/Functions/torque_map.c index 4a30566..bc9e5e1 100644 --- a/src/SUFST/Src/Functions/torque_map.c +++ b/src/SUFST/Src/Functions/torque_map.c @@ -7,7 +7,7 @@ static inline uint16_t apply_deadzone(torque_map_t* map_ptr, uint16_t input); static uint16_t null_torque_map(torque_map_t* map_ptr, uint16_t input); static uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input); static inline uint16_t -apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed); +apply_speed_limit(torque_map_t* map_ptr, uint16_t input, int16_t speed); /** * @brief Initialises the torque map @@ -61,7 +61,7 @@ status_t torque_map_init(torque_map_t* map_ptr, * @param[in] map_ptr Torque map * @param[in] input Input value */ -uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input, uint16_t speed) +uint16_t torque_map_apply(torque_map_t* map_ptr, uint16_t input, int16_t speed) { const uint16_t input_deadzone = apply_deadzone(map_ptr, input); const uint16_t torque = map_ptr->map_func(map_ptr, input_deadzone); @@ -118,8 +118,7 @@ uint16_t linear_torque_map(torque_map_t* map_ptr, uint16_t input) return torque; } -uint16_t -apply_speed_limit(torque_map_t* map_ptr, uint16_t input, uint16_t speed) +uint16_t apply_speed_limit(torque_map_t* map_ptr, uint16_t input, int16_t speed) { uint16_t result = 0; if (speed < map_ptr->speed_start) diff --git a/src/SUFST/Src/config.c b/src/SUFST/Src/config.c index 23b4d02..7722382 100644 --- a/src/SUFST/Src/config.c +++ b/src/SUFST/Src/config.c @@ -69,7 +69,7 @@ static const config_t config_instance = { .max_mapped = 100, .outside_bounds_fraction = 0.05f }, - .max_discrepancy = 15, + .max_discrepancy = 10, }, .bps = { @@ -110,11 +110,11 @@ static const config_t config_instance = { .torque_map = { .function = TORQUE_MAP_LINEAR, .input_max = 100, - .output_max = 1000, + .output_max = 1200, .deadzone_fraction = 0.28f, - .speed_min = 500, + .speed_min = 200, .speed_start = 2000, - .speed_end = 3000 + .speed_end = 4000 }, .pm100 = { .thread = {