Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: correct response codes when mode change fails 

Was "UNSUPPORTED", which is supposed to mean, "Command is not supported"

Now is either "DENIED" (Command is invalid (is supported but has invalid
parameters)) or "FAILED" (Command is valid, but execution has failed.)

We should probably returned DENIED if we try to change to a mode which
doesn't exist, but that would require another callback on AP_Vehicle.
It would also lead to questions around what a valid mode is - so
QSTABILIZE if quadplane is disabled in Plane, for example.
This commit is contained in:
Peter Barker 2020-05-02 13:44:29 +10:00 committed by Andrew Tridgell
parent f128d8c549
commit f0c22244a5
1 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2169,27 +2169,36 @@ void GCS_MAVLINK::handle_set_mode(const mavlink_message_t &msg)
*/
MAV_RESULT GCS_MAVLINK::_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode)
{
MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
// only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
if (uint32_t(_base_mode) & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
if (AP::vehicle()->set_mode(_custom_mode, ModeReason::GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
if (!AP::vehicle()->set_mode(_custom_mode, ModeReason::GCS_COMMAND)) {
// often we should be returning DENIED rather than FAILED
// here. Perhaps a "has_mode" callback on AP_::vehicle()
// would do?
return MAV_RESULT_FAILED;
}
} else if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
return MAV_RESULT_ACCEPTED;
}
if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
// set the safety switch position. Must be in a command by itself
if (_custom_mode == 0) {
// turn safety off (pwm outputs flow to the motors)
hal.rcout->force_safety_off();
result = MAV_RESULT_ACCEPTED;
} else if (_custom_mode == 1) {
return MAV_RESULT_ACCEPTED;
}
if (_custom_mode == 1) {
// turn safety on (no pwm outputs to the motors)
if (hal.rcout->force_safety_on()) {
result = MAV_RESULT_ACCEPTED;
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
return MAV_RESULT_DENIED;
}
return result;
// Command is invalid (is supported but has invalid parameters)
return MAV_RESULT_DENIED;
}
/*