Copter: implement SOLO_BTN commands

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;