AP_Camera: add send_camera_fov_status support

ArduPilot · Sep 21, 2023 · f938fd3 · f938fd3
1 parent 96c6d0f
commit f938fd3
Show file tree

Hide file tree

Showing 5 changed files with 68 additions and 0 deletions.
diff --git a/libraries/AP_Camera/AP_Camera.cpp b/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
 */

diff --git a/libraries/AP_Camera/AP_Camera.h b/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);

diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/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
+    const 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()

diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/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

diff --git a/libraries/AP_Camera/AP_Camera_config.h b/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_LATLONALT_ENABLED
+#endif
+
 #ifndef AP_CAMERA_BACKEND_DEFAULT_ENABLED
 #define AP_CAMERA_BACKEND_DEFAULT_ENABLED AP_CAMERA_ENABLED
 #endif