Skip to content

Commit

Permalink
AP_Camera: Add Camera Tracking functionality reviews Aug20
Browse files Browse the repository at this point in the history
  • Loading branch information
khanasif786 committed Aug 20, 2024
1 parent 42e0f5a commit d4359cd
Show file tree
Hide file tree
Showing 19 changed files with 187 additions and 120 deletions.
24 changes: 16 additions & 8 deletions libraries/AP_Camera/AP_Camera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -395,19 +395,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:
Expand Down Expand Up @@ -699,23 +705,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);

Expand All @@ -725,8 +732,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
Expand Down
6 changes: 4 additions & 2 deletions libraries/AP_Camera/AP_Camera.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
50 changes: 30 additions & 20 deletions libraries/AP_Camera/AP_Camera_Backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,10 @@ AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params &para
{}

// init - performs any required initialisation
void AP_Camera_Backend::init() {
// initialise the tracking object
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();
}
Expand Down Expand Up @@ -425,53 +426,62 @@ void AP_Camera_Backend::log_picture()
}
#endif

bool AP_Camera_Backend::set_tracking_external(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) {
#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, p1, p2, _params.track_sysid, _params.track_compid, _cam_info);
return tracker->set_tracking(tracking_type, top_left, bottom_right, _params.track_sysid, _params.track_compid, camera_settings._cam_info);
#else
return false;
#endif
}

bool AP_Camera_Backend::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) {
// 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, p1, p2);
} else {
return set_tracking_internal(tracking_type, p1, p2);
return set_tracking_external(tracking_type, top_left, bottom_right);
}
return set_tracking_internal(tracking_type, top_left, bottom_right);
}
#endif

void AP_Camera_Backend::handle_message_camera_information(mavlink_channel_t chan, const mavlink_message_t &msg) {
// 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 != _sysid_camera || msg.compid != _compid_camera) {
if (msg.sysid != camera_settings._sysid_camera || msg.compid != camera_settings._compid_camera) {
if (msg.sysid != _params.track_sysid || msg.compid != _params.track_compid) {
return;
}
}

mavlink_msg_camera_information_decode(&msg, &_cam_info);
mavlink_msg_camera_information_decode(&msg, &camera_settings._cam_info);

const uint8_t fw_ver_major = _cam_info.firmware_version & 0x000000FF;
const uint8_t fw_ver_minor = (_cam_info.firmware_version & 0x0000FF00) >> 8;
const uint8_t fw_ver_revision = (_cam_info.firmware_version & 0x00FF0000) >> 16;
const uint8_t fw_ver_build = (_cam_info.firmware_version & 0xFF000000) >> 24;
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",
_cam_info.vendor_name,
_cam_info.model_name,
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);

_got_camera_info = true;
camera_settings._got_camera_info = true;
}

void AP_Camera_Backend::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {
// 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:
Expand Down
23 changes: 15 additions & 8 deletions libraries/AP_Camera/AP_Camera_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,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
bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2);
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& p1, const Vector2f& p2) { return false; }
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& p1, const Vector2f& p2);
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; }
Expand All @@ -102,7 +104,8 @@ class AP_Camera_Backend
float vertical_fov() const { return MAX(0, _params.vfov); }

// handle MAVLink messages from the camera
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);
Expand Down Expand Up @@ -196,10 +199,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
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

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
41 changes: 37 additions & 4 deletions libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,39 @@ SetFocusResult AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float foc
return SetFocusResult::ACCEPTED;
}

// handle incoming mavlink message including CAMERA_INFORMATION
void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
{
// exit immediately if this is not our message
if (msg.sysid != _sysid || msg.compid != _compid) {
return;
}

// handle CAMERA_INFORMATION
if (msg.msgid == MAVLINK_MSG_ID_CAMERA_INFORMATION) {
mavlink_msg_camera_information_decode(&msg, &_cam_info);

const uint8_t fw_ver_major = _cam_info.firmware_version & 0x000000FF;
const uint8_t fw_ver_minor = (_cam_info.firmware_version & 0x0000FF00) >> 8;
const uint8_t fw_ver_revision = (_cam_info.firmware_version & 0x00FF0000) >> 16;
const uint8_t fw_ver_build = (_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",
_cam_info.vendor_name,
_cam_info.model_name,
(unsigned)fw_ver_major,
(unsigned)fw_ver_minor,
(unsigned)fw_ver_revision,
(unsigned)fw_ver_build);

_got_camera_info = true;
}

// call the parents handle message
AP_Camera_Backend::handle_message(chan, msg);
}

// send camera information message to GCS
void AP_Camera_MAVLinkCamV2::send_camera_information(mavlink_channel_t chan) const
{
Expand Down Expand Up @@ -168,12 +201,12 @@ void AP_Camera_MAVLinkCamV2::find_camera()
if (_link == nullptr) {
// we expect that instance 0 has compid = MAV_COMP_ID_CAMERA, instance 1 has compid = MAV_COMP_ID_CAMERA2, etc
uint8_t compid = MIN(MAV_COMP_ID_CAMERA + _instance, MAV_COMP_ID_CAMERA6);
_link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_CAMERA, compid, _sysid_camera);
_link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_CAMERA, compid, _sysid);
if (_link == nullptr) {
// have not yet found a camera so return
return;
}
_compid_camera = compid;
_compid = compid;
}

// request CAMERA_INFORMATION
Expand Down Expand Up @@ -204,8 +237,8 @@ void AP_Camera_MAVLinkCamV2::request_camera_information() const
0, // param6
0, // param7
MAV_CMD_REQUEST_MESSAGE,
_sysid_camera,
_compid_camera,
_sysid,
_compid,
0 // confirmation
};

Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,9 @@ class AP_Camera_MAVLinkCamV2 : public AP_Camera_Backend
// focus in = -1, focus hold = 0, focus out = 1
SetFocusResult set_focus(FocusType focus_type, float focus_value) override;

// handle MAVLink messages from the camera
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override;

// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan) const override;

Expand All @@ -63,8 +66,12 @@ class AP_Camera_MAVLinkCamV2 : public AP_Camera_Backend

// internal members
bool _initialised; // true once the camera has provided a CAMERA_INFORMATION
bool _got_camera_info; // true once camera has provided CAMERA_INFORMATION
mavlink_camera_information_t _cam_info {}; // latest camera information received from camera
uint32_t _last_caminfo_req_ms; // system time that CAMERA_INFORMATION was last requested (used to throttle requests)
class GCS_MAVLINK *_link; // link we have found the camera on. nullptr if not seen yet
uint8_t _sysid; // sysid of camera
uint8_t _compid; // component id of gimbal
};

#endif // AP_CAMERA_MAVLINKCAMV2_ENABLED
10 changes: 6 additions & 4 deletions libraries/AP_Camera/AP_Camera_Mount.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_internal(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
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Camera/AP_Camera_Mount.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_internal(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;
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Camera/AP_Camera_Params.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,7 @@ 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
Expand All @@ -121,6 +122,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
// @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

Expand Down
12 changes: 7 additions & 5 deletions libraries/AP_Camera/AP_Camera_Scripting.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_internal(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
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Camera/AP_Camera_Scripting.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_internal(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;
Expand Down
Loading

0 comments on commit d4359cd

Please sign in to comment.