temporary commit

Tools/scripts/build_options.py

@@ -122,6 +122,7 @@ def __init__(self,
     Feature('Camera', 'Camera_Relay', 'AP_CAMERA_RELAY_ENABLED', 'Enable Camera Trigger via Relay support', 0, 'Camera,RELAY'),
     Feature('Camera', 'Camera_Servo', 'AP_CAMERA_SERVO_ENABLED', 'Enable Camera Trigger via Servo support', 0, 'Camera'),
     Feature('Camera', 'Camera_Solo', 'AP_CAMERA_SOLOGIMBAL_ENABLED', 'Enable Solo Gimbal support', 0, 'Camera'),
+    Feature('Camera', 'Camera', 'AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'Enable getting field of view info', 0, 'Camera,Mount'),
 
     Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None),
 

libraries/AP_Camera/AP_Camera.cpp

@@ -418,6 +418,21 @@ void AP_Camera::send_camera_settings(mavlink_channel_t chan)
     }
 }
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+// send camera field of view status
+void AP_Camera::send_camera_fov_status(mavlink_channel_t chan)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    // call each instance
+    for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_fov_status(chan);
+        }
+    }
+}
+#endif
+
 /*
   update; triggers by distance moved and camera trigger
 */

libraries/AP_Camera/AP_Camera.h

@@ -95,6 +95,11 @@ class AP_Camera {
     // send camera settings message to GCS
     void send_camera_settings(mavlink_channel_t chan);
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+    // send camera field of view status
+    void send_camera_fov_status(mavlink_channel_t chan);
+#endif
+
     // configure camera
     void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
     void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);

libraries/AP_Camera/AP_Camera_Backend.cpp

@@ -241,6 +241,45 @@ void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
         NaN);               // focusLevel float, percentage from 0 to 100, NaN if unknown
 }
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+// send camera field of view status
+void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
+{
+    // getting corresponding mount instance for camera
+    AP_Mount* mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    Quaternion quat;
+    Location loc;
+    Location poi_loc;
+    if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) {
+        return;
+    }
+    // send camera fov status message only if the last calculated values aren't stale
+    const float NaN = nanf("0x4152");
+    const float quat_array[4] = {
+        quat.q1,
+        quat.q2,
+        quat.q3,
+        quat.q4
+    };
+    mavlink_msg_camera_fov_status_send(
+        chan,
+        AP_HAL::millis(),
+        loc.lat,
+        loc.lng,
+        loc.alt,
+        poi_loc.lat,
+        poi_loc.lng,
+        poi_loc.alt,
+        quat_array,
+        NaN, // currently setting hfov as NaN
+        NaN  // currently setting vfov as NaN
+    );
+}
+#endif
+
 // setup a callback for a feedback pin. When on PX4 with the right FMU
 // mode we can use the microsecond timer.
 void AP_Camera_Backend::setup_feedback_callback()

libraries/AP_Camera/AP_Camera_Backend.h

@@ -103,6 +103,11 @@ class AP_Camera_Backend
     // send camera settings message to GCS
     virtual void send_camera_settings(mavlink_channel_t chan) const;
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+    // send camera field of view status
+    void send_camera_fov_status(mavlink_channel_t chan) const;
+#endif
+
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in

libraries/AP_Camera/AP_Camera_config.h

@@ -9,6 +9,10 @@
 #define AP_CAMERA_ENABLED 1
 #endif
 
+#ifndef AP_CAMERA_SEND_FOV_STATUS_ENABLED
+#define AP_CAMERA_SEND_FOV_STATUS_ENABLED AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+#endif
+
 #ifndef AP_CAMERA_BACKEND_DEFAULT_ENABLED
 #define AP_CAMERA_BACKEND_DEFAULT_ENABLED AP_CAMERA_ENABLED
 #endif

libraries/AP_Mount/AP_Mount.cpp

@@ -617,6 +617,18 @@ void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan)
 }
 #endif  // HAL_GCS_ENABLED
 
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+// get poi information
+bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc)
+{
+    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

@@ -198,6 +198,11 @@ class AP_Mount
     // send a GIMBAL_MANAGER_STATUS message to GCS
     void send_gimbal_manager_status(mavlink_channel_t chan);
 
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+    // get poi information
+    bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc);
+#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,6 +3,11 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Terrain/AP_Terrain.h>
+
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+#define POI_REQ_TIMOUT_MS 3000
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -13,6 +18,14 @@ void AP_Mount_Backend::init()
 {
     // setting default target sysid from parameters
     _target_sysid = _params.sysid_default.get();
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+    // create a calculation thread for poi.
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Mount_Backend::calculate_poi, void),
+                                      "poi_calculation_thread",
+                                      8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "POI: failed to start thread");
+    }
+#endif
 }
 
 // set device id of this instance, for MNTx_DEVID parameter
@@ -414,6 +427,121 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+// get poi information
+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) > 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;
+    }
+}
+
+// calculate poi information
+void AP_Mount_Backend::calculate_poi()
+{
+    while (true) {
+        // run this loop at 10hz
+        hal.scheduler->delay(100);
+
+        // 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
+        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;
+
+        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
+            continue;
+        }
+
+        float dist_increment_m;
+        if (!terrain->get_terrain_spacing(dist_increment_m)) {
+            // failed to get the terrain spacing
+            continue;
+        }
+
+        float dist_max = 10000; // Hardcoded for testing. probably need a parameter
+        float total_dist = 0;
+        float mount_pitch_deg = degrees(quat.get_euler_pitch());
+        float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + degrees(ahrs.get_yaw()));
+        bool get_terrain_alt_success = true;
+        float prev_terrain_amsl_m = terrain_amsl_m;
+        // iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
+        while (total_dist < dist_max && (test_loc.alt * 0.01) > terrain_amsl_m) {
+            total_dist += dist_increment_m;
+
+            // Take backup of 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)) {
+                // 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 (!get_terrain_alt_success) {
+            continue;
+        }
+        if (total_dist >= dist_max) {
+            // POI: unable to find terrain within dist_max
+            continue;
+        }
+        if (is_negative(terrain_amsl_m)) {
+            // POI: 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);
+        {
+            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 @@ class AP_Mount_Backend
     // send camera settings message to GCS
     virtual void send_camera_settings(mavlink_channel_t chan) const {}
 
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+    // get poi information
+    bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc);
+#endif
+
     //
     // rangefinder
     //
@@ -221,6 +226,11 @@ class AP_Mount_Backend
     // 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_LATLNGALT_ENABLED
+    // calculate poi information
+    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,16 @@ class AP_Mount_Backend
         MountTarget rate_rads;      // rate target in rad/s
     } mnt_target;
 
+#if AP_MOUNT_POI_TO_LATLNGALT_ENABLED
+    struct {
+        uint32_t poi_update_ms;   // last time when poi info was filled successfully
+        HAL_Semaphore sem;        // semaphore
+        Location loc;             // location of the vehicle on which poi is calculated
+        Location poi_loc;         // location of the poi
+        Quaternion att_quat;      // attitude quaternion of the camera
+    } 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_LATLNGALT_ENABLED
+#define AP_MOUNT_POI_TO_LATLNGALT_ENABLED AP_TERRAIN_AVAILABLE
+#endif

libraries/AP_Terrain/AP_Terrain.h

@@ -182,6 +182,11 @@ class AP_Terrain {
      */
     void get_statistics(uint16_t &pending, uint16_t &loaded) const;
 
+    /*
+      get terrain spacing
+     */
+    bool get_terrain_spacing(float &terrain_spacing);
+
     /*
       returns true if initialisation failed because out-of-memory
      */

libraries/AP_Terrain/TerrainGCS.cpp

@@ -196,6 +196,17 @@ void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded) const
     }
 }
 
+/*
+  get some statistics for TERRAIN_REPORT
+ */
+bool AP_Terrain::get_terrain_spacing(float &terrain_spacing)
+{
+    terrain_spacing = grid_spacing;
+    if (isnan(terrain_spacing)) {
+        return false;
+    }
+    return true;
+}
 
 /* 
    handle terrain messages from GCS