Copter: use handle_set_mode()
This commit is contained in:
parent
c4b17b74e2
commit
f2e6fa3fb0
@ -887,38 +887,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAVLINK_MSG_ID_SET_MODE: // MAV ID: 11
|
||||
{
|
||||
// decode
|
||||
mavlink_set_mode_t packet;
|
||||
mavlink_msg_set_mode_decode(msg, &packet);
|
||||
|
||||
// exit immediately if this command is not meant for this vehicle
|
||||
if (mavlink_check_target(packet.target_system, 0)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
|
||||
if (packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
|
||||
if (set_mode(packet.custom_mode)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
|
||||
// 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();
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else if (packet.custom_mode == 1) {
|
||||
// turn safety on (no pwm outputs to the motors)
|
||||
if (hal.rcout->force_safety_on()) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// send ACK or NAK
|
||||
mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, result);
|
||||
handle_set_mode(msg, set_mode);
|
||||
break;
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user