void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
// RunOnceEvery(50, nav_periodic_task_10Hz())
BOOZ2_AUTOPILOT_CHECK_MOTORS_ON();
if(kill) booz2_autopilot_motors_on = FALSE;
booz2_autopilot_in_flight = _in_flight;
stabilization_attitude_read_rc(booz2_autopilot_in_flight);
stabilization_attitude_run(booz2_autopilot_in_flight);
booz2_guidance_v_run(booz2_autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_THROTTLE] * 105 / 7200 + 95;
CscSetCommands(stabilization_cmd,
booz2_autopilot_in_flight,booz2_autopilot_motors_on);
BOOZ2_SUPERVISION_RUN(mixed_commands, commands, booz2_autopilot_motors_on);
if(booz2_autopilot_motors_on && props_throttle_pass){
Bound(stabilization_cmd[COMMAND_THRUST],0,255);
for(uint8_t i = 0; i < PROPS_NB; i++)
mixed_commands[i] = stabilization_cmd[COMMAND_THRUST];
}
for(uint8_t i = 0; i < PROPS_NB; i++){
if(props_enable[i])
props_set(i,mixed_commands[i]);
else
props_set(i,0);
}
props_commit();
MERCURY_SURFACES_SUPERVISION_RUN(Actuator,
stabilization_cmd,
mixed_commands,
(!booz2_autopilot_in_flight));
ActuatorsCommit();
SendCscFromActuators();
}
void csc_servos_commit()
{
ActuatorsCommit();
}
