diff --git a/src/SUFST/Inc/Functions/torque_map.h b/src/SUFST/Inc/Functions/torque_map.h index 9c3b268f..0fa69483 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,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 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/Inc/config.h b/src/SUFST/Inc/config.h index 46111bac..64362464 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 a25e8292..bc9e5e13 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, int16_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, 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); - - 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,53 @@ 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, int16_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/Interfaces/bps.c b/src/SUFST/Src/Interfaces/bps.c index 684793a7..017e4c2a 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/ctrl.c b/src/SUFST/Src/Services/ctrl.c index b31b3fe9..0c6349f8 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/Services/pm100.c b/src/SUFST/Src/Services/pm100.c index 0a31e95d..3c468aee 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 { diff --git a/src/SUFST/Src/Services/tick.c b/src/SUFST/Src/Services/tick.c index 87598b43..71fda994 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 f97d3b1c..77223822 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,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 = 1200, + .deadzone_fraction = 0.28f, + .speed_min = 200, + .speed_start = 2000, + .speed_end = 4000 }, .pm100 = { .thread = {