Plane: allow GCS to turn safety switch on/off

This commit is contained in:
Randy Mackay 2014-09-14 17:50:35 +09:00 committed by Andrew Tridgell
parent 7a6b55368e
commit 82225de6fa

View File

@ -1156,6 +1156,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
// set the safety switch position
if (packet.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
if (packet.custom_mode == 0) {
// turn safety off (pwm outputs flow to the motors)
hal.rcout->force_safety_off();
} else if (packet.custom_mode == 1) {
// turn safety on (no pwm outputs to the motors)
hal.rcout->force_safety_on();
}
break;
}
// check if we are setting the flight mode
if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
// we ignore base_mode as there is no sane way to map
// from that bitmap to a APM flight mode. We rely on