AP_Mount: add mount POI lock Aux Func/Switch

libraries/AP_Mount/AP_Mount.cpp

@@ -668,6 +668,42 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati
 }
 #endif
 
+#if AP_MOUNT_POI_LOCK_ENABLED
+// lock currently viewed GPS point and switch to GPS Targeting mode
+void AP_Mount::set_poi_lock(uint8_t instance)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return;
+    }
+
+    // call backend's set_poi_lock
+    backend->set_poi_lock();
+}
+
+void AP_Mount::clear_poi_lock(uint8_t instance)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return;
+    }
+
+    // call backend's clear_poi_lock
+    backend->clear_poi_lock();
+}
+
+void AP_Mount::suspend_poi_lock(uint8_t instance)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return;
+    }
+
+    // call backend's suspend_poi_lock
+    backend->suspend_poi_lock();
+}
+#endif // AP_MOUNT_POI_LOCK_ENABLED
+
 // get attitude as a quaternion.  returns true on success.
 // att_quat will be an earth-frame quaternion rotated such that
 // yaw is in body-frame.
@@ -797,7 +833,7 @@ void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
     if (backend == nullptr) {
         return;
     }
-    // call instance's set_roi_cmd
+    // call instance's set target SYSID cmd
     backend->set_target_sysid(sysid);
 }
 

libraries/AP_Mount/AP_Mount.h

@@ -177,6 +177,16 @@ public:
     // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
     void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); }
     void set_yaw_lock(uint8_t instance, bool yaw_lock);
+    
+#if AP_MOUNT_POI_LOCK_ENABLED
+    // controls POI lock which locks gimbal to GPS point currently in view and switches to GPS Targeting mode, suspends tracking poi or clears it
+    void set_poi_lock() { set_poi_lock(_primary); }
+    void set_poi_lock(uint8_t instance);
+    void clear_poi_lock() { clear_poi_lock(_primary); }
+    void clear_poi_lock(uint8_t instance);
+    void suspend_poi_lock() { suspend_poi_lock(_primary); }
+    void suspend_poi_lock(uint8_t instance);
+#endif
 
     // set angle target in degrees
     // roll and pitch are in earth-frame

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -45,6 +45,10 @@ void AP_Mount_Backend::update()
     const bool mount_open = (_mode == MAV_MOUNT_MODE_RETRACT);
     SRV_Channels::move_servo(_open_idx, mount_open, 0, 1);
 
+#if AP_MOUNT_POI_LOCK_ENABLED
+    update_poi_lock_target();
+ #endif // AP_MOUNT_POI_LOCK_ENABLED
+
     // location exists for mode
     Location current_loc;
     switch (_mode) {
@@ -129,7 +133,7 @@ void AP_Mount_Backend::update_mnt_target_from_rc_target()
 
         // yaw angle
         mnt_target.angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min));
-        
+
         // if in yaw ef lock, we use the captured and adjusted yaw_lock_heading rad to
         // adjust the yaw so that any RC yaw changes are reflected in locked heading
         if (mnt_target.angle_rad.yaw_is_ef) {
@@ -237,6 +241,62 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc)
     }
 }
 
+#if AP_MOUNT_POI_LOCK_ENABLED
+// set poi_lock - switch to GPS Targeting mode using current gimbal view's GPS point or save poi location as target
+void AP_Mount_Backend::set_poi_lock()
+{
+    saved_mount_mode = get_mode(); //save current mount mode for the suspend_poi_lock
+    if (!roi_is_set()) {
+        mnt_target.poi_start_ms = AP_HAL::millis();
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking r=%.1f p=%.1f y=%.1f", degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(mnt_target.angle_rad.yaw));
+    } else {  // there is a poi target, just turn POI tracking back on
+        set_mode(MAV_MOUNT_MODE_GPS_POINT);
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking");
+        mnt_target.poi_start_ms = 0;
+    }
+}
+
+// clear poi_lock - clear POI location and revert to default mode
+void AP_Mount_Backend::clear_poi_lock()
+{
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Cleared");
+        clear_roi_target();
+}
+
+// suspend_poi_lock - revert to saved targeting mode, if it exists and POI target exists, otherwise do nothing
+void AP_Mount_Backend::suspend_poi_lock()
+{
+    if (roi_is_set() && saved_mount_mode != MAV_MOUNT_MODE_ENUM_END) {
+        set_mode(saved_mount_mode);    // set back to mode before GPS_POINT if its been set by switch going HIGH
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Revert mode,target saved");;
+    }
+}
+
+// update_poi_lock_target - tries to obtain POI location and start tracking,caller only needs to set poi_start_ms to current time to execute this
+void AP_Mount_Backend::update_poi_lock_target()
+{
+    if (mnt_target.poi_start_ms == 0) {
+        return;
+    }
+    // POI calculation is running and but will silently give up after 3 seconds normally if it does not succeed
+    // try to resolve a AuxFunc POI command to a lat/lng/alt using get_poi
+    // set up variables for get_poi call
+    Quaternion quat;
+    Location vehicle_location;
+    Location target_location;
+    // if poi available, use it and start tracking, otherwise give warning, stop poi retrieval attempts
+    if (get_poi(_instance, quat, vehicle_location, target_location)) {
+        set_roi_target(target_location);
+        mnt_target.poi_start_ms = 0;
+    } else if (AP_HAL::millis() - mnt_target.poi_start_ms > 5000) {
+        // timeout
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Failed to find poi");
+        mnt_target.poi_start_ms = 0;
+    }
+}
+
+#endif // AP_MOUNT_POI_LOCK_ENABLED
+
 // set yaw lock - sets the _yaw_lock variable and captures current earth frame heading of mount for targeting in RC Targeting mode
 void AP_Mount_Backend::set_yaw_lock(bool yaw_lock)
 {
@@ -540,6 +600,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
         LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(LOG_MOUNT_MSG)),
         time_us       : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(),
         instance      : _instance,
+        mode          : static_cast<uint8_t>(get_mode()),
         desired_roll  : target_roll,
         actual_roll   : roll,
         desired_pitch : target_pitch,

libraries/AP_Mount/AP_Mount_Backend.h

@@ -93,6 +93,17 @@ public:
     // If false (aka "follow") the gimbal's tilt is maintained in body-frame meaning it will roll with the vehicle
     void set_roll_lock(bool roll_lock) { _roll_lock = roll_lock; }
 
+#if AP_MOUNT_POI_LOCK_ENABLED
+    // set poi_lock to switch to GPS Targeting mode using current GPS point in gimbal's view or current saved poi and save entry mode for suspend function
+    void set_poi_lock();
+    // clears poi_lock and reverts to default targeting mode
+    void clear_poi_lock();
+    // reverts to saved poi entry mode but maintains poi location if set_poi_lock is called again without clearing it
+    void suspend_poi_lock();
+    // check that poi_target has been set
+    bool roi_is_set() { return !_roi_target.is_zero(); }
+#endif // AP_MOUNT_POI_LOCK_ENABLED
+
     // set angle target in degrees
     // roll and pitch are in earth-frame
     // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
@@ -381,6 +392,7 @@ protected:
         MountAngleTarget angle_rad; // angle target in radians
         MountRateTarget rate_rads;  // rate target in rad/s
         uint32_t last_rate_request_ms;
+        uint32_t poi_start_ms;  // time we started trying to find the gimbal POI for an AuxFunc::MOUNT_POI_LOCK
     } mnt_target;
     
     // RP earth frame locks accessible by backend
@@ -403,7 +415,7 @@ private:
 #endif
 
     bool _yaw_lock;                 // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame
-    
+
     float _yaw_lock_heading_rad;            // mount earth frame direction captured upon calling set_yaw_lock
 
 #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
@@ -419,6 +431,14 @@ private:
 
     Location _roi_target;           // roi target location
 
+#if AP_MOUNT_POI_LOCK_ENABLED
+    void update_poi_lock_target();
+
+    // mount mode saved here entering poi lock for 
+    // switching poi lock back to previous mode with aux function middle position
+    MAV_MOUNT_MODE saved_mount_mode = MAV_MOUNT_MODE_ENUM_END;
+#endif // AP_MOUNT_POI_LOCK_ENABLED
+
     uint8_t _target_sysid;          // sysid to track
     Location _target_sysid_location;// sysid target location
 

libraries/AP_Mount/AP_Mount_config.h

@@ -62,8 +62,12 @@
 #define HAL_MOUNT_XFROBOT_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024
 #endif
 
+#ifndef AP_MOUNT_POI_LOCK_ENABLED
+#define AP_MOUNT_POI_LOCK_ENABLED 0
+#endif
+
 #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED
-#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && HAL_PROGRAM_SIZE_LIMIT_KB > 1024
+#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && (HAL_PROGRAM_SIZE_LIMIT_KB > 1024 || AP_MOUNT_POI_LOCK_ENABLED)
 #endif
 
 #ifndef HAL_MOUNT_TOPOTEK_ENABLED

libraries/AP_Mount/LogStructure.h

@@ -10,6 +10,7 @@
 // @Description: Mount's desired and actual roll, pitch and yaw angles
 // @Field: TimeUS: Time since system startup
 // @Field: I: Instance number
+// @Field: Mode: Mount mode
 // @Field: DRoll: Desired roll
 // @Field: Roll: Actual roll
 // @Field: DPitch: Desired pitch
@@ -24,6 +25,7 @@ struct PACKED log_Mount {
     LOG_PACKET_HEADER;
     uint64_t time_us;
     uint8_t  instance;
+    uint8_t  mode;
     float    desired_roll;
     float    actual_roll;
     float    desired_pitch;
@@ -38,7 +40,7 @@ struct PACKED log_Mount {
 #if HAL_MOUNT_ENABLED
 #define LOG_STRUCTURE_FROM_MOUNT \
     { LOG_MOUNT_MSG, sizeof(log_Mount), \
-      "MNT", "QBfffffffff","TimeUS,I,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#ddddddddm", "F---------0" },
+      "MNT", "QBBfffffffff","TimeUS,I,Mode,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#-ddddddddm", "F----------0" },
 #else
 #define LOG_STRUCTURE_FROM_MOUNT
 #endif