Copter: add DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE option

This commit is contained in:
Jonathan Challinger 2016-01-06 14:05:22 -08:00 committed by Randy Mackay
parent 5cc969f01b
commit cc1f27ad95

View File

@ -1008,7 +1008,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_SET_MODE: // MAV ID: 11
{
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
if (!copter.failsafe.radio) {
handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::set_mode, bool, uint8_t));
} else {
// don't allow mode changes while in radio failsafe
mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, MAV_RESULT_FAILED);
}
#else
handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::set_mode, bool, uint8_t));
#endif
break;
}