AP_Mount: Retract mount when critical altitude is reached

ArduCopter/Copter.cpp

@@ -145,7 +145,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180),
     SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550),
 #if MOUNT == ENABLED
-    SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75),
+    SCHED_TASK(mount_update,          50,     75),
 #endif
 #if CAMERA == ENABLED
     SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update_trigger,  50,  75),
@@ -581,6 +581,13 @@ void Copter::publish_osd_info()
 }
 #endif
 
+#if MOUNT == ENABLED
+void Copter::mount_update() {
+    int32_t height_cm = flightmode->get_alt_above_ground_cm();
+    camera_mount.update(height_cm * 0.01f);
+}
+#endif
+
 /*
   constructor for main Copter class
  */

ArduCopter/Copter.h

@@ -882,6 +882,9 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void userhook_auxSwitch1(uint8_t ch_flag);
     void userhook_auxSwitch2(uint8_t ch_flag);
     void userhook_auxSwitch3(uint8_t ch_flag);
+#if MOUNT == ENABLED
+    void mount_update();
+#endif
 
 #if OSD_ENABLED == ENABLED
     void publish_osd_info();

ArduPlane/ArduPlane.cpp

@@ -69,7 +69,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(rpm_update,             10,    100),
     SCHED_TASK(airspeed_ratio_update,   1,    100),
 #if MOUNT == ENABLED
-    SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100),
+    SCHED_TASK(mount_update,           50,    100),
 #endif // MOUNT == ENABLED
 #if CAMERA == ENABLED
     SCHED_TASK_CLASS(AP_Camera, &plane.camera, update_trigger, 50, 100),
@@ -691,4 +691,10 @@ void Plane::publish_osd_info()
 }
 #endif
 
+#if MOUNT == ENABLED
+void Plane::mount_update() {
+    camera_mount.update(relative_ground_altitude_all_modes(g.rangefinder_landing));
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&plane);

ArduPlane/Plane.h

@@ -843,6 +843,7 @@ class Plane : public AP_HAL::HAL::Callbacks {
     void setup_glide_slope(void);
     int32_t get_RTL_altitude();
     float relative_ground_altitude(bool use_rangefinder_if_available);
+    float relative_ground_altitude_all_modes(bool use_rangefinder_if_available);
     void set_target_altitude_current(void);
     void set_target_altitude_current_adjusted(void);
     void set_target_altitude_location(const Location &loc);
@@ -1057,6 +1058,9 @@ class Plane : public AP_HAL::HAL::Callbacks {
 #if SOARING_ENABLED == ENABLED
     void update_soaring();
 #endif
+#if MOUNT == ENABLED
+    void mount_update();
+#endif
 
     bool reversed_throttle;
     bool have_reverse_throttle_rc_option;

ArduPlane/altitude.cpp

@@ -149,6 +149,27 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
     return relative_altitude;
 }
 
+/*
+  return relative altitude in meters (relative to terrain, if available,
+  or home otherwise)
+ */
+float Plane::relative_ground_altitude_all_modes(bool use_rangefinder_if_available)
+{
+   if (use_rangefinder_if_available && rangefinder_state.in_range) {
+        return rangefinder_state.height_estimate;
+    }
+
+#if AP_TERRAIN_AVAILABLE
+    float altitude;
+    if (terrain.status() == AP_Terrain::TerrainStatusOK &&
+        terrain.height_above_terrain(altitude, true)) {
+        return altitude;
+    }
+#endif
+
+    return relative_altitude;
+}
+
 /*
   set the target altitude to the current altitude. This is used when 
   setting up for altitude hold, such as when releasing elevator in

libraries/AP_Mount/AP_Mount.cpp

@@ -1,3 +1,4 @@
+#include <stdio.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include "AP_Mount.h"
@@ -206,7 +207,14 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
 
     // 23 formerly _K_RATE
 
-    // 24 is AVAILABLE
+    // @Param: _RETRAC_ALT
+    // @DisplayName: Altitude to retract mount
+    // @Description: Retracts the mount if altitude above the home is less than defined. Disabled when value is 0.
+    // @Units: m
+    // @Range: -5000 5000
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("_RETRAC_ALT", 24, AP_Mount, state[0]._retract_altitude, 0),
 
 #if AP_MOUNT_MAX_INSTANCES > 1
     // @Param: 2_DEFLT_MODE
@@ -388,6 +396,16 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial
     // @User: Standard
     AP_GROUPINFO("2_TYPE",           42, AP_Mount, state[1]._type, 0),
+
+    // @Param: 2_RETRAC_ALT
+    // @DisplayName: Altitude to retract mount
+    // @Description: Retracts the mount if altitude above the home is less than defined. Disabled when value is 0.
+    // @Units: m
+    // @Range: -5000 5000
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("2_RETRAC_ALT", 43, AP_Mount, state[1]._retract_altitude, 0),
+
 #endif // AP_MOUNT_MAX_INSTANCES > 1
 
     AP_GROUPEND
@@ -476,11 +494,22 @@ void AP_Mount::init()
 }
 
 // update - give mount opportunity to update servos.  should be called at 10hz or higher
-void AP_Mount::update()
+void AP_Mount::update(float height_above_ground_m)
 {
     // update each instance
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
+            if (state[instance]._retract_altitude != 0 && get_mode() != MAV_MOUNT_MODE_RETRACT) {
+                if (height_above_ground_m < state[instance]._retract_altitude) {
+                    if (_reached_altitude) {
+                        _backends[instance]->set_mode(MAV_MOUNT_MODE_RETRACT);
+                        gcs().send_text(MAV_SEVERITY_INFO, "Low altitude %d. Retracting mount.", (int)height_above_ground_m);
+                        _reached_altitude = false;
+                    }
+                } else {
+                    _reached_altitude = true;
+                }
+            }
             _backends[instance]->update();
         }
     }
@@ -692,7 +721,7 @@ void AP_Mount::send_gimbal_report(mavlink_channel_t chan)
         if (_backends[instance] != nullptr) {
             _backends[instance]->send_gimbal_report(chan);
         }
-    }    
+    }
 }
 
 

libraries/AP_Mount/AP_Mount.h

@@ -76,7 +76,7 @@ class AP_Mount
     void init();
 
     // update - give mount opportunity to update servos.  should be called at 10hz or higher
-    void update();
+    void update(float height_above_ground_m);
 
     // used for gimbals that need to read INS data at full rate
     void update_fast();
@@ -145,6 +145,7 @@ class AP_Mount
         // Parameters
         AP_Int8         _type;              // mount type (None, Servo or MAVLink, see MountType enum)
         AP_Int8         _default_mode;      // default mode on startup and when control is returned from autopilot
+        AP_Int16        _retract_altitude;   // retract altitude
         AP_Int8         _stab_roll;         // 1 = mount should stabilize earth-frame roll axis, 0 = no stabilization
         AP_Int8         _stab_tilt;         // 1 = mount should stabilize earth-frame pitch axis
         AP_Int8         _stab_pan;          // 1 = mount should stabilize earth-frame yaw axis
@@ -181,6 +182,7 @@ class AP_Mount
     MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
 
+    bool _reached_altitude;  // flag to mark that minimum altitude reached to retract the mount
 };
 
 namespace AP {