diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index fc5f1063fbb..24d8c5beafe 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -246,6 +246,21 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet) } take_picture(); return MAV_RESULT_ACCEPTED; + case MAV_CMD_CAMERA_TRACK_POINT: + if (set_tracking(TrackingType::POINT, Vector2f{packet.param1, packet.param2}, Vector2f{})) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_UNSUPPORTED; + case MAV_CMD_CAMERA_TRACK_RECTANGLE: + if (set_tracking(TrackingType::RECTANGLE, Vector2f{packet.param1, packet.param2}, Vector2f{packet.param3, packet.param4})) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_UNSUPPORTED; + case MAV_CMD_CAMERA_STOP_TRACKING: + if (set_tracking(TrackingType::NONE, Vector2f{}, Vector2f{})) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_UNSUPPORTED; case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: { @@ -458,6 +473,35 @@ bool AP_Camera::set_focus(uint8_t instance, FocusType focus_type, float focus_va return backend->set_focus(focus_type, focus_value); } +// 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) +{ + WITH_SEMAPHORE(_rsem); + + if (primary == nullptr) { + return false; + } + return primary->set_tracking(tracking_type, p1, p2); +} + +// 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) +{ + WITH_SEMAPHORE(_rsem); + + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + + // call each instance + return backend->set_tracking(tracking_type, p1, p2); +} + #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.h b/libraries/AP_Camera/AP_Camera.h index 99d28b91293..78da3ffedee 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -123,6 +123,12 @@ public: bool set_focus(FocusType focus_type, float focus_value); bool set_focus(uint8_t instance, FocusType focus_type, float focus_value); + // 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); + // set if vehicle is in AUTO mode void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; } @@ -135,6 +141,12 @@ public: float zoom_value; // percentage or zoom out = -1, hold = 0, zoom in = 1 uint8_t focus_type; // see FocusType enum (1:Rate, 2:Pct, 4:Auto) float focus_value; // If Rate, focus in = -1, focus hold = 0, focus out = 1. If PCT 0 to 100 + uint8_t tracking_type; // see TrackingType enum (0:NONE, 1:POINT, 2:RECTANGLE) + float tracking_p1x; // center or top-left tracking point's x-axis value (0 is left, 1 is right) + float tracking_p1y; // center or top-left tracking point's y-axis value (0 is top, 1 is bottom) + float tracking_p2x; // bottom-right tracking point's x-axis value (0 is left, 1 is right) + float tracking_p2y; // bottom-right tracking point's y-axis value (0 is top, 1 is bottom) + Vector2f tracking_p2; // bottom-right tracking point (0 is left or top, 1 is right or bottom) } camera_state_t; // accessor to allow scripting backend to retrieve state diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 32940d9e345..2551b9909c2 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -65,6 +65,11 @@ public: // focus in = -1, focus hold = 0, focus out = 1 virtual bool set_focus(FocusType focus_type, float focus_value) { return false; } + // 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; } + // handle MAVLink messages from the camera virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {} diff --git a/libraries/AP_Camera/AP_Camera_Scripting.cpp b/libraries/AP_Camera/AP_Camera_Scripting.cpp index f3afb009483..309b712f45c 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.cpp +++ b/libraries/AP_Camera/AP_Camera_Scripting.cpp @@ -37,6 +37,19 @@ bool AP_Camera_Scripting::set_focus(FocusType focus_type, float focus_value) return true; } +// 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) +{ + _cam_state.tracking_type = (uint8_t)tracking_type; + _cam_state.tracking_p1x = p1.x; + _cam_state.tracking_p1y = p1.y; + _cam_state.tracking_p2x = p2.x; + _cam_state.tracking_p2y = p2.y; + return true; +} + // access for scripting backend to retrieve state // returns true on success and cam_state is filled in bool AP_Camera_Scripting::get_state(AP_Camera::camera_state_t& cam_state) diff --git a/libraries/AP_Camera/AP_Camera_Scripting.h b/libraries/AP_Camera/AP_Camera_Scripting.h index 4e056019715..5cd19a5137a 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.h +++ b/libraries/AP_Camera/AP_Camera_Scripting.h @@ -46,6 +46,11 @@ public: // focus in = -1, focus hold = 0, focus out = 1 bool set_focus(FocusType focus_type, float focus_value) override; + // 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; + // 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_shareddefs.h b/libraries/AP_Camera/AP_Camera_shareddefs.h index 1687c71228f..dbf473450a8 100644 --- a/libraries/AP_Camera/AP_Camera_shareddefs.h +++ b/libraries/AP_Camera/AP_Camera_shareddefs.h @@ -19,3 +19,10 @@ enum class FocusType : uint8_t { PCT = 2, // focus to a percentage (from 0 to 100) of the full range. Same as FOCUS_TYPE_RANGE AUTO = 4 // focus automatically. Same as FOCUS_TYPE_AUTO }; + +// tracking types when tracking an object in the video stream +enum class TrackingType : uint8_t { + NONE = 0, // tracking is inactive + POINT = 1, // tracking a point + RECTANGLE = 2 // tracking a rectangle +};