AP_Mount: add get_poi

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>

libraries/AP_Mount/AP_Mount.cpp

@@ -609,6 +609,18 @@ void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan)
 }
 #endif  // HAL_GCS_ENABLED
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+// get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    return backend->get_poi(instance, quat, loc, poi_loc);
+}
+#endif
+
 // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
 // returns true on success
 bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)

libraries/AP_Mount/AP_Mount.h

@@ -195,6 +195,11 @@ public:
     // send a GIMBAL_MANAGER_STATUS message to GCS
     void send_gimbal_manager_status(mavlink_channel_t chan);
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+    bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const;
+#endif
+
     // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
     // returns true on success
     bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -3,16 +3,29 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Terrain/AP_Terrain.h>
 
 extern const AP_HAL::HAL& hal;
 
 #define AP_MOUNT_UPDATE_DT 0.02     // update rate in seconds.  update() should be called at this rate
+#define AP_MOUNT_POI_REQUEST_TIMEOUT_MS 30000   // POI calculations continue to be updated for this many seconds after last request
+#define AP_MOUNT_POI_RESULT_TIMEOUT_MS  3000    // POI calculations valid for 3 seconds
+#define AP_MOUNT_POI_DIST_M_MAX         10000   // POI calculations limit of 10,000m (10km)
 
 // Default init function for every mount
 void AP_Mount_Backend::init()
 {
     // setting default target sysid from parameters
     _target_sysid = _params.sysid_default.get();
+
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // create a calculation thread for poi.
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Mount_Backend::calculate_poi, void),
+                                      "mount_calc_poi",
+                                      8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mount: failed to start POI thread");
+    }
+#endif
 }
 
 // set device id of this instance, for MNTx_DEVID parameter
@@ -414,6 +427,127 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+// get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+bool AP_Mount_Backend::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc)
+{
+    WITH_SEMAPHORE(poi_calculation.sem);
+
+    // record time of request
+    const uint32_t now_ms = AP_HAL::millis();
+    poi_calculation.poi_request_ms = now_ms;
+
+    // check if poi calculated recently
+    if (now_ms - poi_calculation.poi_update_ms > AP_MOUNT_POI_RESULT_TIMEOUT_MS) {
+        return false;
+    }
+
+    // check attitude is valid
+    if (poi_calculation.att_quat.is_nan()) {
+        return false;
+    }
+
+    quat = poi_calculation.att_quat;
+    loc = poi_calculation.loc;
+    poi_loc = poi_calculation.poi_loc;
+    return true;
+}
+
+// calculate the Location that the gimbal is pointing at
+void AP_Mount_Backend::calculate_poi()
+{
+    while (true) {
+        // run this loop at 10hz
+        hal.scheduler->delay(100);
+
+        // calculate poi if requested within last 30 seconds
+        {
+            WITH_SEMAPHORE(poi_calculation.sem);
+            if ((poi_calculation.poi_request_ms == 0) ||
+                (AP_HAL::millis() - poi_calculation.poi_request_ms > AP_MOUNT_POI_REQUEST_TIMEOUT_MS)) {
+                continue;
+            }
+        }
+
+        // get the current location of vehicle
+        const AP_AHRS &ahrs = AP::ahrs();
+        Location curr_loc;
+        if (!ahrs.get_location(curr_loc)) {
+            continue;
+        }
+
+        // change vehicle alt to AMSL
+        curr_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
+
+        // project forward from vehicle looking for terrain
+        // start testing at vehicle's location
+        Location test_loc = curr_loc;
+        Location prev_test_loc = curr_loc;
+
+        // get terrain altitude (AMSL) at test_loc
+        auto terrain = AP_Terrain::get_singleton();
+        float terrain_amsl_m;
+        if ((terrain == nullptr) || !terrain->height_amsl(test_loc, terrain_amsl_m, true)) {
+            continue;
+        }
+
+        // retrieve gimbal attitude
+        Quaternion quat;
+        if (!get_attitude_quaternion(quat)) {
+            // gimbal attitude unavailable
+            continue;
+        }
+
+        // iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
+        const float dist_increment_m = MAX(terrain->get_grid_spacing(), 10);
+        const float mount_pitch_deg = degrees(quat.get_euler_pitch());
+        const float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + degrees(ahrs.get_yaw()));
+        float total_dist_m = 0;
+        bool get_terrain_alt_success = true;
+        float prev_terrain_amsl_m = terrain_amsl_m;
+        while (total_dist_m < AP_MOUNT_POI_DIST_M_MAX && (test_loc.alt * 0.01) > terrain_amsl_m) {
+            total_dist_m += dist_increment_m;
+
+            // backup previous test location and terrain amsl
+            prev_test_loc = test_loc;
+            prev_terrain_amsl_m = terrain_amsl_m;
+
+            // move test location forward
+            test_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m);
+
+            // get terrain's alt-above-sea-level (at test_loc)
+            // fail if terrain alt cannot be retrieved
+            if (!terrain->height_amsl(test_loc, terrain_amsl_m, true) || std::isnan(terrain_amsl_m)) {
+                get_terrain_alt_success = false;
+                continue;
+            }
+        }
+
+        // if a fail occurred above when getting terrain alt then restart calculations from the beginning
+        if (!get_terrain_alt_success) {
+            continue;
+        }
+
+        if (total_dist_m >= AP_MOUNT_POI_DIST_M_MAX) {
+            // unable to find terrain within dist_max
+            continue;
+        }
+
+        // test location has dropped below terrain
+        // interpolate along line between prev_test_loc and test_loc
+        float dist_interp_m = linear_interpolate(0, dist_increment_m, 0, prev_test_loc.alt * 0.01 - prev_terrain_amsl_m, test_loc.alt * 0.01 - terrain_amsl_m);
+        {
+            WITH_SEMAPHORE(poi_calculation.sem);
+            poi_calculation.poi_loc = prev_test_loc;
+            poi_calculation.poi_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m);
+            poi_calculation.att_quat = {quat[0], quat[1], quat[2], quat[3]};
+            poi_calculation.loc = curr_loc;
+            poi_calculation.poi_update_ms = AP_HAL::millis();
+        }
+    }
+}
+#endif
+
 // get pilot input (in the range -1 to +1) received through RC
 void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const
 {

libraries/AP_Mount/AP_Mount_Backend.h

@@ -182,6 +182,11 @@ public:
     // send camera settings message to GCS
     virtual void send_camera_settings(mavlink_channel_t chan) const {}
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+    bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc);
+#endif
+
     //
     // rangefinder
     //
@@ -221,6 +226,11 @@ protected:
     // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
     virtual bool suppress_heartbeat() const { return false; }
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // calculate the Location that the gimbal is pointing at
+    void calculate_poi();
+#endif
+
     // get pilot input (in the range -1 to +1) received through RC
     void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
 
@@ -269,6 +279,17 @@ protected:
         MountTarget rate_rads;      // rate target in rad/s
     } mnt_target;
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    struct {
+        HAL_Semaphore sem;        // semaphore protecting this structure
+        uint32_t poi_request_ms;  // system time POI was last requested
+        uint32_t poi_update_ms;   // system time POI was calculated
+        Location loc;             // gimbal location used for poi calculation
+        Location poi_loc;         // location of the POI
+        Quaternion att_quat;      // attitude quaternion of the gimbal
+    } poi_calculation;
+#endif
+
     Location _roi_target;           // roi target location
     bool _roi_target_set;           // true if the roi target has been set
 

libraries/AP_Mount/AP_Mount_config.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
+#include <AP_Terrain/AP_Terrain.h>
 
 #ifndef HAL_MOUNT_ENABLED
 #define HAL_MOUNT_ENABLED 1
@@ -48,3 +49,7 @@
 #ifndef HAL_MOUNT_VIEWPRO_ENABLED
 #define HAL_MOUNT_VIEWPRO_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
 #endif
+
+#ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED
+#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && BOARD_FLASH_SIZE > 1024
+#endif