static msg_t uart_thread(void *arg) { (void)arg; chRegSetThreadName("UART"); uartStart(&HW_UART_DEV, &uart_cfg); palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP); palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP); systime_t time = chTimeNow(); for(;;) { time += MS2ST(1); if ((systime_t) ((float) chTimeElapsedSince(last_uart_update_time) / ((float) CH_FREQUENCY / 1000.0)) > (float)TIMEOUT) { mcpwm_set_brake_current(-10.0); } else { set_output(out_received); } chThdSleepUntil(time); } return 0; }
static state_t do_state_powered_ascent(instance_data_t *data) { state_estimation_trust_barometer = false; systime_t time_since_ignition = chTimeElapsedSince(data->t_last_ignition); if(data->state.a < 0.0f || time_since_ignition > BURNOUT_TIME) { return STATE_BURNOUT; } else { return STATE_POWERED_ASCENT; } }
static state_t do_state_main_descent(instance_data_t *data) { state_estimation_trust_barometer = true; systime_t time_since_apogee = chTimeElapsedSince(data->t_apogee); if(fabs(data->state.v) < 1.0f || time_since_apogee > LANDING_TIME) { return STATE_TOUCHDOWN; } else { return STATE_MAIN_DESCENT; } }
static state_t do_state_separated_ascent(instance_data_t *data) { state_estimation_trust_barometer = true; systime_t time_since_burnout = chTimeElapsedSince(data->t_last_burnout); if(data->state.v < 0 || time_since_burnout > APOGEE_TIME) { return STATE_APOGEE; } else { return STATE_SEPARATED_ASCENT; } }
static state_t do_state_free_ascent(instance_data_t *data) { state_estimation_trust_barometer = true; systime_t time_since_burnout = chTimeElapsedSince(data->t_last_burnout); if((data->state.h - data->h_pad) > IGNITE_ALTITUDE || time_since_burnout > IGNITE_TIME) { return STATE_IGNITE; } else { return STATE_FREE_ASCENT; } }
/** Clear unhealthy flags after some time. Flags are reset one per day. */ static void check_clear_unhealthy(void) { static systime_t ticks; if (chTimeElapsedSince(ticks) < S2ST(24*60*60)) return; ticks = chTimeNow(); for (u8 prn=0; prn<32; prn++) { if (acq_prn_param[prn].state == ACQ_PRN_UNHEALTHY) acq_prn_param[prn].state = ACQ_PRN_ACQUIRING; } }
static state_t do_state_wait_ignition(instance_data_t *data) { state_estimation_trust_barometer = true; systime_t time_in_state = chTimeElapsedSince(data->t_last_ignite_attempt); if(data->state.a > IGNITION_ACCEL) { data->current_stage_position--; return STATE_IGNITION; } else if(time_in_state > IGNITE_TIMEOUT) { return STATE_SEPARATED_ASCENT; } else { return STATE_WAIT_IGNITION; } }
static state_t do_state_drogue_descent(instance_data_t *data) { state_estimation_trust_barometer = true; systime_t time_since_apogee = chTimeElapsedSince(data->t_apogee); if((data->state.h - data->h_pad) < MAIN_ALTITUDE || time_since_apogee > MAIN_TIME) { if(GOT_MAIN) { return STATE_MAIN_DEPLOY; } else { return STATE_MAIN_DESCENT; } } else { return STATE_DROGUE_DESCENT; } }