diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 559a21dd21..a85b93ca5f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1575,6 +1575,68 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAV_CMD_SOLO_BTN_FLY_CLICK: { + result = MAV_RESULT_ACCEPTED; + + if (copter.failsafe.radio) { + break; + } + + if (!copter.set_mode(LOITER)) { + copter.set_mode(ALT_HOLD); + } + break; + } + + case MAV_CMD_SOLO_BTN_FLY_HOLD: { + result = MAV_RESULT_ACCEPTED; + + if (copter.failsafe.radio) { + break; + } + + if (!copter.motors.armed()) { + copter.init_arm_motors(true); + } else if (copter.ap.land_complete) { + if (copter.set_mode(LOITER)) { + copter.do_user_takeoff(packet.param1*100, true); + } + } else { + copter.set_mode(LAND); + } + break; + } + + case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { + result = MAV_RESULT_ACCEPTED; + + if (copter.failsafe.radio) { + break; + } + + if (copter.motors.armed()) { + if (copter.ap.land_complete) { + // if landed, disarm motors + copter.init_disarm_motors(); + } else { + // assume that shots modes are all done in guided. + // NOTE: this may need to change if we add a non-guided shot mode + bool shot_mode = (packet.param1 != 0.0f && copter.control_mode == GUIDED); + + if (!shot_mode) { + if (copter.set_mode(BRAKE)) { + copter.brake_timeout_to_loiter_ms(2500); + } else { + copter.set_mode(ALT_HOLD); + } + } else { + // SoloLink is expected to handle pause in shots + } + } + } + break; + } + default: result = MAV_RESULT_UNSUPPORTED; break;