AP_Mount: poi calcs start/stop with requests

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -8,20 +8,22 @@
 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_REQ_TIMOUT_MS  3000    // POI calculations valid for 3 seconds
-#define AP_MOUNT_POI_DIST_M_MAX     10000   // POI calculations limit of 10,000m (10km)
+#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, "POI: failed to start thread");
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mount: failed to start POI thread");
     }
 #endif
 }
@@ -429,62 +431,76 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
 // 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);
-        if ((AP_HAL::millis() - poi_calculation.poi_update_ms) > AP_MOUNT_POI_REQ_TIMOUT_MS) {
-            // last lat,lon,alt values are stale
-            return false;
-        }
-        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;
+    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 poi
+// 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 location to AMSL
+        // change vehicle alt to AMSL
         curr_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
 
-        // retrieve gimbal attitude
-        Quaternion quat;
-        if (!get_attitude_quaternion(quat)) {
-            // gimbal attitude unavailable
-            continue;
-        }
-
         // 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;
-        // get terrain altitude (AMSL) at test_loc
-        if (!terrain->height_amsl(test_loc, terrain_amsl_m, true)) {
-            // failed to get terrain altitude
+        if ((terrain == nullptr) || !terrain->height_amsl(test_loc, terrain_amsl_m, true)) {
             continue;
         }
 
+        // get the terrain spacing
         float dist_increment_m;
         if (!terrain->get_terrain_spacing(dist_increment_m)) {
-            // failed to get the terrain spacing
+            continue;
+        }
+
+        // retrieve gimbal attitude
+        Quaternion quat;
+        if (!get_attitude_quaternion(quat)) {
+            // gimbal attitude unavailable
             continue;
         }
 
@@ -497,33 +513,31 @@ void AP_Mount_Backend::calculate_poi()
         while (total_dist_m < AP_MOUNT_POI_DIST_M_MAX && (test_loc.alt * 0.01) > terrain_amsl_m) {
             total_dist_m += dist_increment_m;
 
-            // Take backup of previous test location and terrain amsl
+            // backup previous test location and terrain amsl
             prev_test_loc = test_loc;
             prev_terrain_amsl_m = terrain_amsl_m;
 
-            // Move test location forward
+            // 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
+            // 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)) {
-                // failed to get terrain alt
                 get_terrain_alt_success = false;
                 continue;
             }
         }
-        // if a fail occurs in the loop while getting terrain alt then continue to next interation
+
+        // 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;
         }
-        if (is_negative(terrain_amsl_m)) {
-            // failed to retrieve terrain alt
-            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);

libraries/AP_Mount/AP_Mount_Backend.h

@@ -227,7 +227,7 @@ class AP_Mount_Backend
     virtual bool suppress_heartbeat() const { return false; }
 
 #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
-    // calculate poi information
+    // calculate the Location that the gimbal is pointing at
     void calculate_poi();
 #endif
 
@@ -282,6 +282,7 @@ class AP_Mount_Backend
 #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