Add Mount option to retract on landing

ArduCopter/Copter.h

@@ -591,6 +591,11 @@ class Copter : public AP_Vehicle {
 
     bool standby_active;
 
+#if HAL_MOUNT_ENABLED
+    // true if the last time we checked the flightmode was landing:
+    bool was_landing;
+#endif
+
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
     static const struct LogStructure log_structure[];

ArduCopter/mode.cpp

@@ -428,6 +428,16 @@ void Copter::update_flight_mode()
     attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation
 
     flightmode->run();
+
+#if HAL_MOUNT_ENABLED
+    {
+        const bool is_landing = flightmode->is_landing();
+        if (is_landing && !was_landing) {
+            camera_mount.vehicle_has_started_to_land();
+        }
+        was_landing = is_landing;
+    }
+#endif
 }
 
 // exit_mode - high level call to organise cleanup as a flight mode is exited

Tools/autotest/arducopter.py

@@ -6074,7 +6074,7 @@
                 raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
         else:
             if reverse is not None:
                 raise NotAchievedException(
                     "Detected motor peak at %fHz, throttle %f%%, %fdB" %
                     (freq, hover_throttle, peakdb))
             else:
@@ -6570,7 +6570,7 @@
         self.reboot_sitl()
 
         if ex is not None:
             raise ex
 
     def GyroFFTHarmonic(self):
         """Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""
@@ -11512,6 +11512,38 @@
 
         self.wait_disarmed()
 
+    def MountRetractOnLanding(self):
+        '''test mount retracts on landing when asked to'''
+        self.context_push()
+        self.setup_servo_mount()
+        self.set_parameter("MNT1_OPTIONS", 2)
+        self.reboot_sitl() # to handle MNT_TYPE changing
+
+        self.takeoff(10, mode='GUIDED')
+
+        self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
+
+        angle = -10
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
+            p1=angle,   # pitch angle in degrees
+            p2=0,     # yaw angle in degrees
+            p3=0,     # pitch rate in degrees (NaN to ignore)
+            p4=0,     # yaw rate in degrees (NaN to ignore)
+            p5=0,     # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame)
+            p6=0,     # unused
+            p7=0,     # gimbal id
+        )
+        self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
+
+        self.change_mode('LAND')
+        self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT)
+
+        self.wait_disarmed()
+
+        self.context_pop()
+        self.reboot_sitl()
+
     def CameraLogMessages(self):
         '''ensure Camera log messages are good'''
         self.set_parameter("RC12_OPTION", 9) # CameraTrigger
@@ -12231,6 +12263,7 @@
             self.ScriptMountPOI,
             self.ScriptCopterPosOffsets,
             self.MountSolo,
+            self.MountRetractOnLanding,
             self.FlyMissionTwice,
             self.FlyMissionTwiceWithReset,
             self.MissionIndexValidity,

libraries/AP_Mount/AP_Mount.cpp

@@ -743,6 +743,19 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_
     backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
 }
 
+// called by a vehicle to indicate the vehicle has moved into a
+// landing phase.  For example, a Copter moving into LAND mode
+// should call this method.
+void AP_Mount::vehicle_has_started_to_land()
+{
+    // each instance writes log
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->vehicle_has_started_to_land();
+        }
+    }
+}
+
 #if HAL_LOGGING_ENABLED
 // write mount log packet for all backends
 void AP_Mount::write_log()

libraries/AP_Mount/AP_Mount.h

@@ -290,6 +290,11 @@ class AP_Mount
     // enable/disable rangefinder.  Returns true on success
     bool set_rangefinder_enable(uint8_t instance, bool enable);
 
+    // called by a vehicle to indicate the vehicle has moved into a
+    // landing phase.  For example, a Copter moving into LAND mode
+    // should call this method.
+    void vehicle_has_started_to_land();
+
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
 

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -930,4 +930,14 @@ void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str)
     _last_warning_ms = now_ms;
 }
 
+// called by a vehicle to indicate the vehicle has moved into a
+// landing phase.  For example, a Copter moving into LAND mode
+// should call this method.
+void AP_Mount_Backend::vehicle_has_started_to_land()
+{
+    if (option_set(Options::RETRACT_ON_LANDING)) {
+        UNUSED_RESULT(set_mode(MAV_MOUNT_MODE_RETRACT));
+    }
+}
+
 #endif // HAL_MOUNT_ENABLED

libraries/AP_Mount/AP_Mount_Backend.h

@@ -221,6 +221,11 @@ class AP_Mount_Backend
     // enable/disable rangefinder.  Returns true on success
     virtual bool set_rangefinder_enable(bool enable) { return false; }
 
+    // called by a vehicle to indicate the vehicle has moved into a
+    // landing phase.  For example, a Copter moving into LAND mode
+    // should call this method.
+    void vehicle_has_started_to_land();
+
 protected:
 
     enum class MountTargetType {
@@ -249,6 +254,7 @@ class AP_Mount_Backend
     // options parameter bitmask handling
     enum class Options : uint8_t {
         RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode
+        RETRACT_ON_LANDING = (1U << 1), // Retract the gimbal when vehicle enters landing mode
     };
     bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; }
 

libraries/AP_Mount/AP_Mount_Params.cpp

@@ -168,7 +168,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
     // @Param: _OPTIONS
     // @DisplayName: Mount options
     // @Description: Mount options bitmask
-    // @Bitmask: 0:RC lock state from previous mode
+    // @Bitmask: 0:RC lock state from previous mode, 1:Retract mount on land start
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0),