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