AP_Camera: Add Camera Tracking functionality (WIP)

libraries/AP_Camera/AP_Camera.cpp

@@ -467,7 +467,12 @@ bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message ms
         CHECK_PAYLOAD_SIZE2(CAMERA_CAPTURE_STATUS);
         send_camera_capture_status(chan);
         break;
-
+#if AP_CAMERA_TRACKING_ENABLED
+    case MSG_CAMERA_TRACKING_IMAGE_STATUS:
+        CHECK_PAYLOAD_SIZE2(CAMERA_TRACKING_IMAGE_STATUS);
+        send_camera_tracking_image_status(chan);
+        break;
+#endif
     default:
         // should not reach this; should only be called for specific IDs
         break;
@@ -621,6 +626,19 @@ void AP_Camera::send_camera_capture_status(mavlink_channel_t chan)
     }
 }
 
+// send camera tracking image status message to GCS
+void AP_Camera::send_camera_tracking_image_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_tracking_image_status(chan);
+        }
+    }
+}
+
 /*
   update; triggers by distance moved and camera trigger
 */
@@ -733,6 +751,20 @@ bool AP_Camera::set_tracking(uint8_t instance, TrackingType tracking_type, const
     // call each instance
     return backend->set_tracking(tracking_type, top_left, bottom_right);
 }
+
+bool AP_Camera::is_tracking_object_visible(uint8_t instance)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+
+    // call each instance
+    return backend->is_tracking_object_visible();
+}
+
 #endif
 
 #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED

libraries/AP_Camera/AP_Camera.h

@@ -144,6 +144,7 @@ class AP_Camera {
     // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
     bool set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right);
     bool set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right);
+    bool is_tracking_object_visible(uint8_t instance);
 #endif
 
 #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
@@ -239,6 +240,9 @@ class AP_Camera {
     // send camera capture status message to GCS
     void send_camera_capture_status(mavlink_channel_t chan);
 
+    // send camera tracking image status message
+    void send_camera_tracking_image_status(mavlink_channel_t chan);
+
     HAL_Semaphore _rsem;                // semaphore for multi-thread access
     AP_Camera_Backend *primary;         // primary camera backed
     bool _is_in_auto_mode;              // true if in AUTO mode

libraries/AP_Camera/AP_Camera_Backend.cpp

@@ -322,6 +322,24 @@ void AP_Camera_Backend::send_camera_capture_status(mavlink_channel_t chan) const
         image_index);     // total number of images captured
 }
 
+// send camera tracking image status message to GCS
+void AP_Camera_Backend::send_camera_tracking_image_status(mavlink_channel_t chan) const
+{
+    mavlink_msg_camera_tracking_image_status_send(
+        chan,
+        camera_settings._cam_tracking_status.tracking_status,  // uint8_t tracking_status
+        camera_settings._cam_tracking_status.tracking_mode,    // uint8_t tracking_mode
+        camera_settings._cam_tracking_status.target_data,      // uint8_t target_data
+        camera_settings._cam_tracking_status.point_x,          // float point_x
+        camera_settings._cam_tracking_status.point_y,          // float point_y
+        camera_settings._cam_tracking_status.radius,           // float radius
+        camera_settings._cam_tracking_status.rec_top_x,        // float rec_top_x
+        camera_settings._cam_tracking_status.rec_top_y,        // float rec_top_y
+        camera_settings._cam_tracking_status.rec_bottom_x,     // float rec_bottom_x
+        camera_settings._cam_tracking_status.rec_bottom_y      // float rec_bottom_y
+    );
+}
+
 // 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()
@@ -476,6 +494,11 @@ void AP_Camera_Backend::handle_message_camera_information(mavlink_channel_t chan
     camera_settings._got_camera_info = true;
 }
 
+void AP_Camera_Backend::handle_message_camera_tracking_image_status(mavlink_channel_t chan, const mavlink_message_t &msg)
+{
+    mavlink_msg_camera_tracking_image_status_decode(&msg, &camera_settings._cam_tracking_status);
+}
+
 // handle MAVLink messages from the camera
 void AP_Camera_Backend::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
 {
@@ -484,6 +507,8 @@ void AP_Camera_Backend::handle_message(mavlink_channel_t chan, const mavlink_mes
         case MAVLINK_MSG_ID_CAMERA_INFORMATION:
             handle_message_camera_information(chan,msg);
             break;
+        case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS:
+            handle_message_camera_tracking_image_status(chan,msg);
         default:
             break;
     }

libraries/AP_Camera/AP_Camera_Backend.h

@@ -86,6 +86,14 @@ class AP_Camera_Backend
     // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
     bool set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right);
 
+    // returns true if the objkect to be tracked is visible in the frame
+    bool is_tracking_object_visible() {
+        if (camera_settings._cam_tracking_status.tracking_status & CAMERA_TRACKING_STATUS_FLAGS::CAMERA_TRACKING_STATUS_FLAGS_ACTIVE) {
+            return true;
+        }
+        return false;
+    }
+
 #if AP_CAMERA_TRACKING_ENABLED
     // default tracking supported by camera
     virtual bool set_tracking_internal(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right) { return false; }
@@ -113,6 +121,9 @@ class AP_Camera_Backend
     // handle camera information message
     void handle_message_camera_information(mavlink_channel_t chan, const mavlink_message_t &msg);
 
+    // handle image tracking status messages
+    void handle_message_camera_tracking_image_status(mavlink_channel_t chan, const mavlink_message_t &msg);
+
     // configure camera
     virtual void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time) {}
 
@@ -139,6 +150,9 @@ class AP_Camera_Backend
     // send camera capture status message to GCS
     virtual void send_camera_capture_status(mavlink_channel_t chan) const;
 
+    // send camera tracking image status message to GCS 
+    void send_camera_tracking_image_status(mavlink_channel_t chan) const;
+
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in
@@ -207,9 +221,16 @@ class AP_Camera_Backend
     struct {
         bool _got_camera_info;      // true once camera has provided CAMERA_INFORMATION
         mavlink_camera_information_t _cam_info {}; // latest camera information received from camera
+        mavlink_camera_tracking_image_status_t _cam_tracking_status {};
         uint8_t _sysid_camera;      // sysid of camera
         uint8_t _compid_camera;     // component id of gimbal
     } camera_settings;
+
+    enum CAMERA_TRACKING_STATUS_FLAGS : uint8_t {
+        CAMERA_TRACKING_STATUS_FLAGS_IDLE,
+        CAMERA_TRACKING_STATUS_FLAGS_ACTIVE,
+        CAMERA_TRACKING_STATUS_FLAGS_ERROR
+    };
 };
 
 #endif // AP_CAMERA_ENABLED