void guidance_h_read_rc(bool in_flight)
{
switch (guidance_h.mode) {
case GUIDANCE_H_MODE_RC_DIRECT:
stabilization_none_read_rc();
break;
#if USE_STABILIZATION_RATE
case GUIDANCE_H_MODE_RATE:
#if SWITCH_STICKS_FOR_RATE_CONTROL
stabilization_rate_read_rc_switched_sticks();
#else
stabilization_rate_read_rc();
#endif
break;
#endif
case GUIDANCE_H_MODE_CARE_FREE:
stabilization_attitude_read_rc(in_flight, TRUE, FALSE);
break;
case GUIDANCE_H_MODE_FORWARD:
stabilization_attitude_read_rc(in_flight, FALSE, TRUE);
break;
case GUIDANCE_H_MODE_ATTITUDE:
stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
break;
case GUIDANCE_H_MODE_HOVER:
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
#if GUIDANCE_H_USE_SPEED_REF
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
/* enable x,y velocity setpoints */
SetBit(guidance_h.sp.mask, 5);
#endif
break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_read_rc();
break;
#endif
case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) {
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE);
} else {
INT_EULERS_ZERO(guidance_h.rc_sp);
}
break;
case GUIDANCE_H_MODE_FLIP:
stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
break;
default:
break;
}
}
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();
}
