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;
}
}
