From 766f08a4f7abed92eb8ecad936f39116ed2d43cd Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Mon, 29 Jul 2024 00:23:06 +0530 Subject: [PATCH 1/5] AP_Camera: Add Camera Tracking functionality --- libraries/AP_Camera/AP_Camera.cpp | 24 +++-- libraries/AP_Camera/AP_Camera.h | 6 +- libraries/AP_Camera/AP_Camera_Backend.cpp | 73 ++++++++++++++++ libraries/AP_Camera/AP_Camera_Backend.h | 30 ++++++- .../AP_Camera/AP_Camera_MAVLinkCamV2.cpp | 4 + libraries/AP_Camera/AP_Camera_Mount.cpp | 10 ++- libraries/AP_Camera/AP_Camera_Mount.h | 2 +- libraries/AP_Camera/AP_Camera_Params.cpp | 21 +++++ libraries/AP_Camera/AP_Camera_Params.h | 7 +- libraries/AP_Camera/AP_Camera_Scripting.cpp | 12 +-- libraries/AP_Camera/AP_Camera_Scripting.h | 2 +- libraries/AP_Camera/AP_Camera_Tracking.cpp | 87 +++++++++++++++++++ libraries/AP_Camera/AP_Camera_Tracking.h | 52 +++++++++++ libraries/AP_Camera/AP_Camera_config.h | 5 ++ libraries/AP_Mount/AP_Mount.cpp | 8 +- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 12 +-- libraries/AP_Mount/AP_Mount_Viewpro.h | 2 +- 19 files changed, 323 insertions(+), 38 deletions(-) create mode 100644 libraries/AP_Camera/AP_Camera_Tracking.cpp create mode 100644 libraries/AP_Camera/AP_Camera_Tracking.h diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index b3869c4818d77b..79c9ff9b2ab30e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index dd9f4ddfde5654..412052e3c15383 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 4c2a1658364687..dec00b88a49156 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -18,6 +18,17 @@ AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params ¶ _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 diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 6c5543cf826a76..531dea4f05f1fc 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -25,6 +25,7 @@ #include "AP_Camera.h" #include #include +#include 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 diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp index a47816c70e1d01..a2baad007d574f 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index 29f07591165c8c..63c4bdc7eac0a1 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index d001cb979d3c9a..5f3b45fb87054f 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/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; diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 749d2985f0ab40..77d57c24eb1944 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/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 }; diff --git a/libraries/AP_Camera/AP_Camera_Params.h b/libraries/AP_Camera/AP_Camera_Params.h index cba713a16319ea..7bd69e41058c51 100644 --- a/libraries/AP_Camera/AP_Camera_Params.h +++ b/libraries/AP_Camera/AP_Camera_Params.h @@ -2,6 +2,7 @@ #include #include +#include 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; diff --git a/libraries/AP_Camera/AP_Camera_Scripting.cpp b/libraries/AP_Camera/AP_Camera_Scripting.cpp index add22ca4e46bfd..237e9ec58ea58d 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.cpp +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Scripting.h b/libraries/AP_Camera/AP_Camera_Scripting.h index c8e8a035a6456d..6d7cdff52f8a2a 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.h +++ b/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; diff --git a/libraries/AP_Camera/AP_Camera_Tracking.cpp b/libraries/AP_Camera/AP_Camera_Tracking.cpp new file mode 100644 index 00000000000000..2e8b54bf45a99a --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_Tracking.cpp @@ -0,0 +1,87 @@ +#include "AP_Camera_Tracking.h" + +#if AP_CAMERA_TRACKING_ENABLED + +extern const AP_HAL::HAL& hal; + +// 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_Tracking::set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right, uint8_t tracking_device_sysid, uint8_t tracking_device_compid, mavlink_camera_information_t _cam_info) +{ + // if we don't support the required tracking then return + switch (tracking_type) { + case TrackingType::TRK_NONE: + break; + case TrackingType::TRK_POINT: + if (!(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_TRACKING_POINT)) { + return false; + } + break; + case TrackingType::TRK_RECTANGLE: + if (!(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE)) { + return false; + } + break; + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Tracking: New Tracking request"); + + if (_link == nullptr) { + uint8_t proxy_device_compid = tracking_device_compid; + uint8_t proxy_device_sysid {}; + _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_ONBOARD_CONTROLLER, proxy_device_compid, proxy_device_sysid); + if (proxy_device_sysid != tracking_device_sysid) { + // means the tracking device sysid we found is not same as what we declared through parameters + _link = nullptr; + GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"AP_Camera: Found Controller but its different from the declared one in parameters"); + return false; + } + if (_link == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"AP_Camera: Could Not find any onboard controller registered"); + return false; + } + } + + // prepare and send message + mavlink_command_long_t pkt = { + 0, // param1 + 0, // param2 + 0, // param3 + 0, // param4 + 0, // param5 + 0, // param6 + 0, // param7 + 0, // command + tracking_device_sysid, // target_system + tracking_device_compid, // target_component + 0, // confirmation + }; + + switch (tracking_type) { + case TrackingType::TRK_POINT: + pkt.command = MAV_CMD_CAMERA_TRACK_POINT; + pkt.param1 = top_left.x; + pkt.param2 = top_left.y; + break; + case TrackingType::TRK_RECTANGLE: + pkt.command = MAV_CMD_CAMERA_TRACK_RECTANGLE; + pkt.param1 = top_left.x; + pkt.param2 = top_left.y; + pkt.param3 = bottom_right.x; + pkt.param4 = bottom_right.y; + break; + case TrackingType::TRK_NONE: + pkt.command = MAV_CMD_CAMERA_STOP_TRACKING; + break; + default: + // Unknown + return false; + } + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"sent message to device sysid %d and comp %d",tracking_device_sysid,tracking_device_compid); + return true; +} + +#endif // AP_CAMERA_TRACKING_ENABLED \ No newline at end of file diff --git a/libraries/AP_Camera/AP_Camera_Tracking.h b/libraries/AP_Camera/AP_Camera_Tracking.h new file mode 100644 index 00000000000000..6e070379f767c8 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_Tracking.h @@ -0,0 +1,52 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + Camera Tracking Library + */ +#pragma once + +#include "AP_Camera_config.h" +#if AP_CAMERA_TRACKING_ENABLED +#include +#include "AP_Camera_shareddefs.h" +#include "AP_Camera_config.h" + +// #include "AP_Camera.h" + +class AP_Camera_Tracking +{ +public: + + // Constructor + AP_Camera_Tracking() {} + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Camera_Tracking); + + // initialize the camera tracking library + void init(); + + // set tracking to non nj klbhnjk nm,e, 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& top_left, const Vector2f& bottom_right, uint8_t tracking_device_sysid, uint8_t tracking_device_compid, mavlink_camera_information_t _cam_info); + + // handle MAVLink messages from the camera + void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg); + +private: + GCS_MAVLINK* _link; +}; + +#endif // AP_CAMERA_TRACKING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 24b51275be72cf..373fba77ed4aa2 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -53,3 +53,8 @@ #ifndef HAL_RUNCAM_ENABLED #define HAL_RUNCAM_ENABLED 1 #endif + +// enable camera tracking all cameras +#ifndef AP_CAMERA_TRACKING_ENABLED +#define AP_CAMERA_TRACKING_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index f3225bbd63880c..84e8b91a350ae1 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -843,15 +843,15 @@ SetFocusResult AP_Mount::set_focus(uint8_t instance, FocusType focus_type, float } // 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_Mount::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_Mount::set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } - return backend->set_tracking(tracking_type, p1, p2); + return backend->set_tracking(tracking_type, top_left, bottom_right); } // set camera lens as a value from 0 to 5 diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index f4e36949c7926c..b6b5c78bcaa1a3 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -251,7 +251,7 @@ class AP_Mount // 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(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2); + bool set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right); // set camera lens as a value from 0 to 5 bool set_lens(uint8_t instance, uint8_t lens); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 07ccd096636613..296540ae1b5858 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -178,7 +178,7 @@ class AP_Mount_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; } + virtual bool set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right) { return false; } // set camera lens as a value from 0 to 5 virtual bool set_lens(uint8_t lens) { return false; } diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index e04510962ef07d..9ba4870cc27ec0 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -801,9 +801,9 @@ SetFocusResult AP_Mount_Viewpro::set_focus(FocusType focus_type, float focus_val } // 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_Mount_Viewpro::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_Mount_Viewpro::set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right) { // exit immediately if not initialised if (!_initialised) { @@ -816,13 +816,13 @@ bool AP_Mount_Viewpro::set_tracking(TrackingType tracking_type, const Vector2f& break; case TrackingType::TRK_POINT: { return (send_tracking_command(TrackingCommand::START, 0) && - send_tracking_command2(TrackingCommand2::SET_POINT, (p1.x - 0.5) * 960, (p1.y - 0.5) * 540)); + send_tracking_command2(TrackingCommand2::SET_POINT, (top_left.x - 0.5) * 960, (top_left.y - 0.5) * 540)); break; } case TrackingType::TRK_RECTANGLE: return (send_tracking_command(TrackingCommand::START, 0) && - send_tracking_command2(TrackingCommand2::SET_RECT_TOPLEFT, (p1.x - 0.5) * 960, (p1.y - 0.5) * 540) && - send_tracking_command2(TrackingCommand2::SET_RECT_BOTTOMRIGHT, (p2.x - 0.5) * 960, (p2.y - 0.5) * 540)); + send_tracking_command2(TrackingCommand2::SET_RECT_TOPLEFT, (top_left.x - 0.5) * 960, (top_left.y - 0.5) * 540) && + send_tracking_command2(TrackingCommand2::SET_RECT_BOTTOMRIGHT, (bottom_right.x - 0.5) * 960, (bottom_right.y - 0.5) * 540)); break; } diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index 1e045c8fd1ae29..a1fa3f1e4916e4 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -70,7 +70,7 @@ class AP_Mount_Viewpro : public AP_Mount_Backend_Serial // 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(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; From 0cee9f73e17dd96583c5fdc8e454cd91ebfee703 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Wed, 2 Oct 2024 16:03:59 +0530 Subject: [PATCH 2/5] AP_Camera: add Follow functionality for the tracked object --- libraries/AP_Camera/AP_Camera.cpp | 34 ++++++++++++++++++++++- libraries/AP_Camera/AP_Camera.h | 4 +++ libraries/AP_Camera/AP_Camera_Backend.cpp | 25 +++++++++++++++++ libraries/AP_Camera/AP_Camera_Backend.h | 21 ++++++++++++++ 4 files changed, 83 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 79c9ff9b2ab30e..ea2b37e6a8cb7c 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 412052e3c15383..9c4fa9f0d6bd36 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/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 diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index dec00b88a49156..b11d093a9df1f1 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/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; } diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 531dea4f05f1fc..d686a0f83cb110 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/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 From 48b797181b1d5caf99bbebec05e7038970713b59 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Wed, 2 Oct 2024 16:03:59 +0530 Subject: [PATCH 3/5] AP_Follow: add Follow functionality for the tracked object --- libraries/AP_Follow/AP_Follow.cpp | 50 ++++++++++++++++++++++++++----- libraries/AP_Follow/AP_Follow.h | 7 +++-- 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index b4acb2fc0499a1..47913a374a51e1 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -26,6 +26,8 @@ #include #include #include +#include +#include extern const AP_HAL::HAL& hal; @@ -137,7 +139,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @Param: _OPTIONS // @DisplayName: Follow options // @Description: Follow options bitmask - // @Values: 0:None,1: Mount Follows lead vehicle on mode enter + // @Values: 0:None,1: Mount Follows lead vehicle on mode enter, 2: Follows vehicle tracked by the gimbal target. // @User: Standard AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0), @@ -166,7 +168,7 @@ void AP_Follow::clear_offsets_if_required() } // get target's estimated location -bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const +bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) { // exit immediately if not enabled if (!_enabled) { @@ -175,19 +177,50 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne // check for timeout if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) { - return false; + if (option_is_enabled(Option::MOUNT_FOLLOW_ON_ENTER)) { + return false; + } } // calculate time since last actual position update const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f; // get velocity estimate - if (!get_velocity_ned(vel_ned, dt)) { - return false; + if (option_is_enabled(Option::OBJECT_FOLLOW_ON_ENTER)) { + vel_ned = {0,0,0}; + } else { + if (!get_velocity_ned(vel_ned, dt)) { + return false; + } } // project the vehicle position - Location last_loc = _target_location; + Location last_loc; + if (option_is_enabled(Option::OBJECT_FOLLOW_ON_ENTER)) { + uint8_t instance = 0; + Quaternion quat; + Location poi_loc; + if (AP_Camera::get_singleton()->is_tracking_object_visible(0)) { + if (AP_Mount::get_singleton()->get_poi(instance,quat,loc,poi_loc)) { + poi_loc.change_alt_frame(last_loc.get_alt_frame()); + if (last_loc.lat == poi_loc.lat && last_loc.lng == poi_loc.lng && last_loc.alt == poi_loc.alt) { + // Do nothing + } else { + last_loc = poi_loc; + _last_location_update_ms = AP_HAL::millis(); + } + } else { + return false; + } + } else { + printf("No objet detected"); + return false; + } + + } else if (option_is_enabled(Option::MOUNT_FOLLOW_ON_ENTER)) { + last_loc = _target_location; + } + last_loc.offset(vel_ned.x * dt, vel_ned.y * dt); last_loc.alt -= vel_ned.z * 100.0f * dt; // convert m/s to cm/s, multiply by dt. minus because NED @@ -211,6 +244,7 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w Vector3f veh_vel; if (!get_target_location_and_velocity(target_loc, veh_vel)) { clear_dist_and_bearing_to_target(); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Returning false 0"); return false; } @@ -225,6 +259,7 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w // fail if too far if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) { clear_dist_and_bearing_to_target(); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Returning false 1"); return false; } @@ -235,6 +270,7 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w Vector3f offsets; if (!get_offsets_ned(offsets)) { clear_dist_and_bearing_to_target(); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Returning false 2"); return false; } @@ -538,7 +574,7 @@ void AP_Follow::clear_dist_and_bearing_to_target() } // get target's estimated location and velocity (in NED), with offsets added -bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const +bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) { Vector3f ofs; if (!get_offsets_ned(ofs) || diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 48c43fc9ff4706..f5056aab31f114 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -33,7 +33,8 @@ class AP_Follow // enum for FOLLOW_OPTIONS parameter enum class Option { - MOUNT_FOLLOW_ON_ENTER = 1 + MOUNT_FOLLOW_ON_ENTER = 1, + OBJECT_FOLLOW_ON_ENTER = 2 }; // enum for YAW_BEHAVE parameter @@ -69,10 +70,10 @@ class AP_Follow bool have_target() const; // get target's estimated location and velocity (in NED) - bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const; + bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned); // get target's estimated location and velocity (in NED), with offsets added - bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const; + bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned); // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned); From 83b995d877b1296004ed7f047e5b7202b5a13b6f Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Wed, 2 Oct 2024 16:03:59 +0530 Subject: [PATCH 4/5] GCS_MAVLink: add Follow functionality for the tracked object --- libraries/GCS_MAVLink/GCS_Common.cpp | 3 +++ libraries/GCS_MAVLink/ap_message.h | 1 + 2 files changed, 4 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 4e22558349cbbc..5e60923b5dac13 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1060,6 +1060,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS}, { MAVLINK_MSG_ID_CAMERA_FOV_STATUS, MSG_CAMERA_FOV_STATUS}, { MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, MSG_CAMERA_CAPTURE_STATUS}, + { MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, MSG_CAMERA_TRACKING_IMAGE_STATUS}, #endif #if HAL_MOUNT_ENABLED { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, @@ -4122,6 +4123,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_DIGICAM_CONTROL: case MAVLINK_MSG_ID_GOPRO_HEARTBEAT: // heartbeat from a GoPro in Solo gimbal case MAVLINK_MSG_ID_CAMERA_INFORMATION: + case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS: { AP_Camera *camera = AP::camera(); if (camera == nullptr) { @@ -6108,6 +6110,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #if AP_CAMERA_SEND_FOV_STATUS_ENABLED case MSG_CAMERA_FOV_STATUS: #endif + case MSG_CAMERA_TRACKING_IMAGE_STATUS: case MSG_CAMERA_CAPTURE_STATUS: { AP_Camera *camera = AP::camera(); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 4923317266025c..c46cc76101c14d 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -56,6 +56,7 @@ enum ap_message : uint8_t { MSG_CAMERA_INFORMATION, MSG_CAMERA_SETTINGS, MSG_CAMERA_FOV_STATUS, + MSG_CAMERA_TRACKING_IMAGE_STATUS, MSG_CAMERA_CAPTURE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, From d4b3b55f9904c63daf9b09be14842e843dd80316 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Wed, 2 Oct 2024 16:03:59 +0530 Subject: [PATCH 5/5] ArduCopter: add Follow functionality for the tracked object --- ArduCopter/mode_follow.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 9d9ef816a0e866..a71bde2d2b582e 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -2,6 +2,7 @@ #if MODE_FOLLOW_ENABLED == ENABLED +#include /* * mode_follow.cpp - follow another mavlink-enabled vehicle by system id *