Browse Source

GCS_MAVLink: return MAV_RESULT_FAILED from do_aux_function if invalid function

c415-sdk
Peter Barker 4 years ago committed by Peter Barker
parent
commit
115e895c82
  1. 6
      libraries/GCS_MAVLink/GCS_Common.cpp

6
libraries/GCS_MAVLink/GCS_Common.cpp

@ -2473,7 +2473,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_lon @@ -2473,7 +2473,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_lon
}
const RC_Channel::AUX_FUNC aux_func = (RC_Channel::AUX_FUNC)packet.param1;
const RC_Channel::AuxSwitchPos position = (RC_Channel::AuxSwitchPos)packet.param2;
rc().do_aux_function(aux_func, position);
if (!rc().do_aux_function(aux_func, position)) {
// note that this is not quite right; we could be more nuanced
// about our return code here.
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}

Loading…
Cancel
Save