static void autopilot_on_rc_frame(void)
{
/* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2)) {
//TODO, check whether the aircraft can actually be flown in manual mode
//most rotory aircraft can't, at least not without additional IMU aid
//for now, just turn set to failsafe instead of manual mode.
fbw_mode = FBW_MODE_FAILSAFE;
} else {
fbw_mode = FBW_MODE_AUTO;
}
/* Trying to switch to auto when AP is lost */
if ((inter_mcu.status == INTERMCU_LOST) &&
(fbw_mode == FBW_MODE_AUTO)) {
fbw_mode = AP_LOST_FBW_MODE;
}
/* if manual */
if (fbw_mode == FBW_MODE_MANUAL) {
autopilot_motors_on = true;
SetCommands(commands_failsafe);
#ifdef SetCommandsFromRC
SetCommandsFromRC(commands, radio_control.values);
#else
#warning "FBW: needs commands from RC in order to be useful."
#endif
}
/* Forward radiocontrol to AP */
intermcu_on_rc_frame(fbw_mode);
}
/** Callback when we received an RC frame */
static void fbw_on_rc_frame(void)
{
/* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2)) {
fbw_mode = FBW_MODE_MANUAL;
} else {
fbw_mode = FBW_MODE_AUTO;
}
/* Failsafe check if intermcu is lost while AP was in control */
if ((intermcu.status == INTERMCU_LOST) &&
(fbw_mode == FBW_MODE_AUTO)) {
fbw_mode = AP_LOST_FBW_MODE;
}
/* If the FBW is in control */
if (fbw_mode == FBW_MODE_MANUAL) {
fbw_motors_on = true;
SetCommands(commands_failsafe);
#ifdef SetCommandsFromRC
SetCommandsFromRC(commands, radio_control.values);
#else
#warning "FBW: needs commands from RC in order to be useful."
#endif
}
/* Forward radiocontrol to AP */
intermcu_on_rc_frame(fbw_mode);
}
