+// ============================================================
+// Firmware initialization
+void fmw_init(const FMW_InitInfo *info) {
+ FMW_ASSERT(info->message_exchange.huart != NULL);
+ FMW_ASSERT(info->message_exchange.hcrc != NULL);
+ FMW_ASSERT(info->message_exchange.handler != NULL);
+ FMW_ASSERT(info->emergency.on_begin != NULL);
+ FMW_ASSERT(info->emergency.on_end != NULL);
+ FMW_ASSERT(info->motors_count >= 0);
+ if (info->motors_count > 0) { FMW_ASSERT(info->motors != NULL); }
+
+ fmw_state.motors = info->motors;
+ fmw_state.motors_count = info->motors_count;
+ fmw_state.huart = info->message_exchange.huart;
+ fmw_state.hcrc = info->message_exchange.hcrc;
+ fmw_state.emergency_mode_grace_period_ms = info->emergency.wait_at_most_ms_before_emergency;
+ fmw_state.callback_message_handler = info->message_exchange.handler;
+ fmw_state.callback_emergency_begin = info->emergency.on_begin;
+ fmw_state.callback_emergency_end = info->emergency.on_end;
+ fmw_state.emergency_timer = info->emergency.timer;
+
+ HAL_StatusTypeDef timer_init_res = HAL_TIM_Base_Start_IT(info->emergency.timer);
+ FMW_ASSERT(timer_init_res == HAL_OK);
+
+ HAL_StatusTypeDef dma_init_res = HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer);
+ FMW_ASSERT(dma_init_res == HAL_OK);
+}
+
+void fmw_uart_message_dispatch(void) {
+ fmw_state.emergency_last_message_received_time = HAL_GetTick();
+ fmw_state.callback_message_handler((FMW_Message*)&fmw_state.uart_buffer, fmw_state.hcrc);
+ // NOTE(lb): listen for the next message "recursively".
+ HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer);
+}
+
+void fmw_uart_error(void) {
+ // NOTE(lb): i don't know how to determine if the error that cause the jump here
+ // was during a receive or a send of a message over UART, so i'm just
+ // going to stop the motors and abort the receive just in case.
+ fmw_motors_stop(fmw_state.motors, fmw_state.motors_count);
+ HAL_UART_AbortReceive(fmw_state.huart);
+
+ FMW_Message response = {0};
+ response.response.result = fmw_result_from_uart_error();
+ fmw_uart_message_send(&response);
+ HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer);
+}
+
+void fmw_emergency_begin(void) {
+ if (fmw_state.mode_current == FMW_Mode_Emergency) { return; }
+ if (fmw_state.motors_count > 0) {
+ fmw_state.motors_active_before_emergency = fmw_state.motors[0].active;
+ if (fmw_state.motors[0].active) {
+ fmw_motors_stop(fmw_state.motors, fmw_state.motors_count);
+ fmw_motors_disable(fmw_state.motors, fmw_state.motors_count);
+ }
+ }
+
+ fmw_state.mode_previous = fmw_state.mode_current;
+ fmw_state.mode_current = FMW_Mode_Emergency;
+
+ if (fmw_state.callback_emergency_begin) { fmw_state.callback_emergency_begin(); }
+}
+
+void fmw_emergency_end(void) {
+ if (fmw_state.mode_previous == FMW_Mode_None) { return; }
+ fmw_state.mode_current = fmw_state.mode_previous;
+ fmw_state.mode_previous = FMW_Mode_None;
+ if (fmw_state.motors_count > 0 && fmw_state.motors_active_before_emergency) {
+ fmw_motors_enable(fmw_state.motors, fmw_state.motors_count);
+ }
+ if (fmw_state.callback_emergency_end) { fmw_state.callback_emergency_end(); }
+}
+
+void fmw_emergency_timer_update(void) {
+ if (fmw_state.mode_current != FMW_Mode_Run) { return; }
+ uint32_t time_now = HAL_GetTick();
+ if (time_now - fmw_state.emergency_last_message_received_time > fmw_state.emergency_mode_grace_period_ms) {
+ fmw_emergency_begin();
+ }
+}
+
+FMW_Mode fmw_mode_current(void) {
+ return fmw_state.mode_current;
+}
+
+FMW_Mode fmw_mode_transition(FMW_Mode mode) {
+ FMW_ASSERT(mode > FMW_Mode_None);
+ FMW_ASSERT(mode < FMW_Mode_COUNT);
+ FMW_Mode old = fmw_state.mode_previous;
+ fmw_state.mode_previous = fmw_state.mode_current;
+ fmw_state.mode_current = mode;
+ return old;
+}
+
+// ============================================================
+// Misc
projects / pioneer-stm32 / commitdiff