AP_Camera: Add Camera Tracking functionality

libraries/AP_Camera/AP_Camera.cpp

@@ -394,19 +394,25 @@ MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet)
         }
         return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_CAMERA_TRACK_POINT:
+#if AP_CAMERA_TRACKING_ENABLED
         if (set_tracking(TrackingType::TRK_POINT, Vector2f{packet.param1, packet.param2}, Vector2f{})) {
             return MAV_RESULT_ACCEPTED;
         }
+#endif
         return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_CAMERA_TRACK_RECTANGLE:
+#if AP_CAMERA_TRACKING_ENABLED
         if (set_tracking(TrackingType::TRK_RECTANGLE, Vector2f{packet.param1, packet.param2}, Vector2f{packet.param3, packet.param4})) {
             return MAV_RESULT_ACCEPTED;
         }
+#endif
         return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_CAMERA_STOP_TRACKING:
+#if AP_CAMERA_TRACKING_ENABLED
         if (set_tracking(TrackingType::TRK_NONE, Vector2f{}, Vector2f{})) {
             return MAV_RESULT_ACCEPTED;
         }
+#endif
         return MAV_RESULT_UNSUPPORTED;
     case MAV_CMD_VIDEO_START_CAPTURE:
     case MAV_CMD_VIDEO_STOP_CAPTURE:
@@ -698,23 +704,24 @@ SetFocusResult AP_Camera::set_focus(uint8_t instance, FocusType focus_type, floa
     return backend->set_focus(focus_type, focus_value);
 }
 
+#if AP_CAMERA_TRACKING_ENABLED
 // set tracking to none, point or rectangle (see TrackingType enum)
-// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
-// p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
-bool AP_Camera::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
+// if POINT only then top_left is the point
+// top_left,bottom_right are in range 0 to 1.  0 is left or top, 1 is right or bottom
+bool AP_Camera::set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right)
 {
     WITH_SEMAPHORE(_rsem);
 
     if (primary == nullptr) {
         return false;
     }
-    return primary->set_tracking(tracking_type, p1, p2);
+    return primary->set_tracking(tracking_type, top_left, bottom_right);
 }
 
 // set tracking to none, point or rectangle (see TrackingType enum)
-// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
-// p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
-bool AP_Camera::set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
+// if POINT only then top_left is the point
+// top_left,bottom_right are in range 0 to 1.  0 is left or top, 1 is right or bottom
+bool AP_Camera::set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right)
 {
     WITH_SEMAPHORE(_rsem);
 
@@ -724,8 +731,9 @@ bool AP_Camera::set_tracking(uint8_t instance, TrackingType tracking_type, const
     }
 
     // call each instance
-    return backend->set_tracking(tracking_type, p1, p2);
+    return backend->set_tracking(tracking_type, top_left, bottom_right);
 }
+#endif
 
 #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
 // set camera lens as a value from 0 to 5

libraries/AP_Camera/AP_Camera.h

@@ -138,11 +138,13 @@ class AP_Camera {
     SetFocusResult set_focus(FocusType focus_type, float focus_value);
     SetFocusResult set_focus(uint8_t instance, FocusType focus_type, float focus_value);
 
+#if AP_CAMERA_TRACKING_ENABLED
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
     // 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& p1, const Vector2f& p2);
-    bool set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2);
+    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);
+#endif
 
 #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
     // set camera lens as a value from 0 to 5, instance starts from 0

libraries/AP_Camera/AP_Camera_Backend.cpp

@@ -18,6 +18,17 @@ AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params &para
     _instance(instance)
 {}
 
+// init - performs any required initialisation
+void AP_Camera_Backend::init()
+{
+#if AP_CAMERA_TRACKING_ENABLED
+    // initialise the tracking object
+    if (_params.track_enable == 1) {
+        tracker = NEW_NOTHROW AP_Camera_Tracking();
+    }
+#endif
+}
+
 // update - should be called at 50hz
 void AP_Camera_Backend::update()
 {
@@ -416,4 +427,66 @@ void AP_Camera_Backend::log_picture()
 }
 #endif
 
+#if AP_CAMERA_TRACKING_ENABLED
+// set tracking to none, point or rectangle (see TrackingType enum)
+// if POINT only then top_left is the point
+// top_left,bottom_right are in range 0 to 1.  0 is left or top, 1 is right or bottom
+bool AP_Camera_Backend::set_tracking_external(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right)
+{
+    if (tracker == nullptr) {
+        return false;
+    }
+    return tracker->set_tracking(tracking_type, top_left, bottom_right, _params.track_sysid, _params.track_compid, camera_settings._cam_info);
+}
+
+// start object tracking
+bool AP_Camera_Backend::set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right)
+{
+    if (_params.track_enable == 1) {
+        return set_tracking_external(tracking_type, top_left, bottom_right);
+    }
+    return set_tracking_internal(tracking_type, top_left, bottom_right);
+}
+#endif
+
+// handle camera information message
+void AP_Camera_Backend::handle_message_camera_information(mavlink_channel_t chan, const mavlink_message_t &msg)
+{
+    // accept the camera information only if its coming either from camera or from external tracking system
+    if (msg.sysid != _params.track_sysid || msg.compid != _params.track_compid) {
+        return;
+    }
+
+    mavlink_msg_camera_information_decode(&msg, &camera_settings._cam_info);
+
+    const uint8_t fw_ver_major = camera_settings._cam_info.firmware_version & 0x000000FF;
+    const uint8_t fw_ver_minor = (camera_settings._cam_info.firmware_version & 0x0000FF00) >> 8;
+    const uint8_t fw_ver_revision = (camera_settings._cam_info.firmware_version & 0x00FF0000) >> 16;
+    const uint8_t fw_ver_build = (camera_settings._cam_info.firmware_version & 0xFF000000) >> 24;
+
+    // display camera info to user
+    gcs().send_text(MAV_SEVERITY_INFO, "Camera: %s.32 %s.32 fw:%u.%u.%u.%u",
+            camera_settings._cam_info.vendor_name,
+            camera_settings._cam_info.model_name,
+            (unsigned)fw_ver_major,
+            (unsigned)fw_ver_minor,
+            (unsigned)fw_ver_revision,
+            (unsigned)fw_ver_build);
+
+    camera_settings._got_camera_info = true;
+}
+
+// handle MAVLink messages from the camera
+void AP_Camera_Backend::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
+{
+    // handle CAMERA_INFORMATION
+    switch (msg.msgid) {
+        case MAVLINK_MSG_ID_CAMERA_INFORMATION:
+            handle_message_camera_information(chan,msg);
+            break;
+        default:
+            break;
+    }
+}
+
 #endif // AP_CAMERA_ENABLED

libraries/AP_Camera/AP_Camera_Backend.h

@@ -25,6 +25,7 @@
 #include "AP_Camera.h"
 #include <AP_Common/Location.h>
 #include <AP_Logger/LogStructure.h>
+#include <AP_Camera/AP_Camera_Tracking.h>
 
 class AP_Camera_Backend
 {
@@ -45,7 +46,7 @@ class AP_Camera_Backend
     }
 
     // init - performs any required initialisation
-    virtual void init() {};
+    void init();
 
     // update - should be called at 50hz
     virtual void update();
@@ -83,7 +84,15 @@ class AP_Camera_Backend
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
     // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
-    virtual bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) { return false; }
+    bool set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right);
+
+#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; }
+#endif
+
+    // Onboard controller specific tracking
+    bool set_tracking_external(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right);
 
     // set camera lens as a value from 0 to 5
     virtual bool set_lens(uint8_t lens) { return false; }
@@ -98,7 +107,11 @@ class AP_Camera_Backend
     float vertical_fov() const { return MAX(0, _params.vfov); }
 
     // handle MAVLink messages from the camera
-    virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {}
+    // child classes must call the parent's i.e. AP_Camera_Backend's handle_message also
+    virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
+
+    // handle camera information message
+    void handle_message_camera_information(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) {}
@@ -137,6 +150,9 @@ class AP_Camera_Backend
     // references
     AP_Camera &_frontend;       // reference to the front end which holds parameters
     AP_Camera_Params &_params;  // parameters for this backend
+#if AP_CAMERA_TRACKING_ENABLED
+    AP_Camera_Tracking* tracker; // reference to the camera tracking object
+#endif
 
     // feedback pin related methods
     void setup_feedback_callback();
@@ -186,6 +202,14 @@ class AP_Camera_Backend
     Location last_location;         // Location that last picture was taken at (used for trigg_dist calculation)
     uint16_t image_index;           // number of pictures taken since boot
     bool last_is_armed;             // stores last arm/disarm state. true if it was armed lastly
+
+private:
+    struct {
+        bool _got_camera_info;      // true once camera has provided CAMERA_INFORMATION
+        mavlink_camera_information_t _cam_info {}; // latest camera information received from camera
+        uint8_t _sysid_camera;      // sysid of camera
+        uint8_t _compid_camera;     // component id of gimbal
+    } camera_settings;
 };
 
 #endif // AP_CAMERA_ENABLED

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp

@@ -128,6 +128,9 @@ void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlin
 {
     // exit immediately if this is not our message
     if (msg.sysid != _sysid || msg.compid != _compid) {
+        // if its not from the camera device maybe its from the tracking device
+        // so call backends default handle message function
+        AP_Camera_Backend::handle_message(chan, msg);
         return;
     }
 
@@ -151,6 +154,7 @@ void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlin
 
         _got_camera_info = true;
     }
+
 }
 
 // send camera information message to GCS

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -47,17 +47,19 @@ SetFocusResult AP_Camera_Mount::set_focus(FocusType focus_type, float focus_valu
     return SetFocusResult::FAILED;
 }
 
+#if AP_CAMERA_TRACKING_ENABLED
 // set tracking to none, point or rectangle (see TrackingType enum)
-// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
-// p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
-bool AP_Camera_Mount::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
+// if POINT only then top_left is the point
+// top_left,bottom_right are in range 0 to 1.  0 is left or top, 1 is right or bottom
+bool AP_Camera_Mount::set_tracking_internal(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right)
 {
     AP_Mount* mount = AP::mount();
     if (mount != nullptr) {
-        return mount->set_tracking(get_mount_instance(), tracking_type, p1, p2);
+        return mount->set_tracking(get_mount_instance(), tracking_type, top_left, bottom_right);
     }
     return false;
 }
+#endif
 
 
 // set camera lens as a value from 0 to 5

libraries/AP_Camera/AP_Camera_Mount.h

@@ -51,7 +51,7 @@ class AP_Camera_Mount : public AP_Camera_Backend
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
     // 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& p1, const Vector2f& p2) override;
+    bool set_tracking_internal(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right) override;
 
     // set camera lens as a value from 0 to 5
     bool set_lens(uint8_t lens) override;

libraries/AP_Camera/AP_Camera_Params.cpp

@@ -103,6 +103,27 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_VFOV", 13, AP_Camera_Params, vfov, 0),
 
+#if AP_CAMERA_TRACKING_ENABLED
+    // @Param: _TRK_ENABLE
+    // @DisplayName: Companion based tracking enable or disable
+    // @Description: This enables tracking functionality by using companion as the tracking device
+    // @Values: 0:Disabled, 1:MAVLink (or maybe Companion Computer)
+    // @User: Standard
+    AP_GROUPINFO("_TRK_ENABLE", 14, AP_Camera_Params, track_enable, 0),
+
+    // @Param: _TRK_SYSID
+    // @DisplayName: Tracking device sysid
+    // @Description: This is the system id for the tracking device or companion
+    // @User: Standard
+    AP_GROUPINFO("_TRK_SYSID", 15, AP_Camera_Params, track_sysid, 0),
+
+    // @Param: _TRK_COMPID
+    // @DisplayName: Tracking device compid
+    // @Description: This is the component id for the tracking device or companion
+    // @User: Standard
+    AP_GROUPINFO("_TRK_COMPID", 16, AP_Camera_Params, track_compid, 0),
+#endif
+
     AP_GROUPEND
 
 };

libraries/AP_Camera/AP_Camera_Params.h

@@ -2,6 +2,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Camera/AP_Camera_config.h>
 
 class AP_Camera_Params {
 
@@ -25,7 +26,11 @@ class AP_Camera_Params {
     AP_Int8 mount_instance;     // mount instance to which camera is associated with
     AP_Float hfov;              // horizontal field of view in degrees
     AP_Float vfov;              // vertical field of view in degrees
-
+#if AP_CAMERA_TRACKING_ENABLED
+    AP_Int8 track_enable;       // enable or disable camera tracking
+    AP_Int16 track_sysid;        // system id for the tracking device
+    AP_Int16 track_compid;       // component id for the tracking device
+#endif
     // pin number for accurate camera feedback messages
     AP_Int8 feedback_pin;
     AP_Int8 feedback_polarity;

libraries/AP_Camera/AP_Camera_Scripting.cpp

@@ -37,16 +37,18 @@ SetFocusResult AP_Camera_Scripting::set_focus(FocusType focus_type, float focus_
     return SetFocusResult::ACCEPTED;
 }
 
+#if AP_CAMERA_TRACKING_ENABLED
 // set tracking to none, point or rectangle (see TrackingType enum)
-// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
-// p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
-bool AP_Camera_Scripting::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
+// if POINT only then top_left is the point
+// top_left,bottom_right are in range 0 to 1.  0 is left or top, 1 is right or bottom
+bool AP_Camera_Scripting::set_tracking_internal(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right)
 {
     _cam_state.tracking_type = (uint8_t)tracking_type;
-    _cam_state.tracking_p1 = p1;
-    _cam_state.tracking_p2 = p2;
+    _cam_state.tracking_p1 = top_left;
+    _cam_state.tracking_p2 = bottom_right;
     return true;
 }
+#endif
 
 // access for scripting backend to retrieve state
 // returns true on success and cam_state is filled in

libraries/AP_Camera/AP_Camera_Scripting.h

@@ -49,7 +49,7 @@ class AP_Camera_Scripting : public AP_Camera_Backend
     // set tracking to none, point or rectangle (see TrackingType enum)
     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
     // 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& p1, const Vector2f& p2) override;
+    bool set_tracking_internal(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right) override;
 
     // returns true on success and cam_state is filled in
     bool get_state(AP_Camera::camera_state_t& cam_state) override;