Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: add and use SetFocusResult

This commit is contained in:
Peter Barker 2023-06-21 16:03:39 +10:00 committed by Peter Barker
parent cc0b2ab3bb
commit 82b0630c6a
10 changed files with 42 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Camera/AP_Camera.cpp
View File

@ -220,23 +220,24 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_SET_CAMERA_FOCUS:
// accept any of the auto focus types
if ((is_equal(packet.param1, (float)FOCUS_TYPE_AUTO) ||
is_equal(packet.param1, (float)FOCUS_TYPE_AUTO_SINGLE) ||
is_equal(packet.param1, (float)FOCUS_TYPE_AUTO_CONTINUOUS)) &&
set_focus(FocusType::AUTO, 0)) {
return MAV_RESULT_ACCEPTED;
}
switch ((SET_FOCUS_TYPE)packet.param1) {
case FOCUS_TYPE_AUTO:
case FOCUS_TYPE_AUTO_SINGLE:
case FOCUS_TYPE_AUTO_CONTINUOUS:
return (MAV_RESULT)set_focus(FocusType::AUTO, 0);
case FOCUS_TYPE_CONTINUOUS:
// accept continuous manual focus
if (is_equal(packet.param1, (float)FOCUS_TYPE_CONTINUOUS) &&
set_focus(FocusType::RATE, packet.param2)) {
return MAV_RESULT_ACCEPTED;
}
return (MAV_RESULT)set_focus(FocusType::RATE, packet.param2);
// accept focus as percentage
if (is_equal(packet.param1, (float)FOCUS_TYPE_RANGE) &&
set_focus(FocusType::PCT, packet.param2)) {
return MAV_RESULT_ACCEPTED;
case FOCUS_TYPE_RANGE:
return (MAV_RESULT)set_focus(FocusType::PCT, packet.param2);
case SET_FOCUS_TYPE_ENUM_END:
case FOCUS_TYPE_STEP:
case FOCUS_TYPE_METERS:
// unsupported focus (bad parameter)
break;
}
return MAV_RESULT_UNSUPPORTED;
return MAV_RESULT_DENIED;
case MAV_CMD_IMAGE_START_CAPTURE:
if (!is_zero(packet.param2) || !is_equal(packet.param3, 1.0f) || !is_zero(packet.param4)) {
// time interval is not supported
@ -474,25 +475,25 @@ bool AP_Camera::set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value)
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera::set_focus(FocusType focus_type, float focus_value)
SetFocusResult AP_Camera::set_focus(FocusType focus_type, float focus_value)
{
WITH_SEMAPHORE(_rsem);
if (primary == nullptr) {
return false;
return SetFocusResult::FAILED;
}
return primary->set_focus(focus_type, focus_value);
}
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
SetFocusResult AP_Camera::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
{
WITH_SEMAPHORE(_rsem);
auto *backend = get_instance(instance);
if (backend == nullptr) {
return false;
return SetFocusResult::FAILED;
}
// call each instance

4
libraries/AP_Camera/AP_Camera.h
View File

@ -126,8 +126,8 @@ public:
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool set_focus(FocusType focus_type, float focus_value);
bool set_focus(uint8_t instance, FocusType focus_type, float focus_value);
SetFocusResult set_focus(FocusType focus_type, float focus_value);
SetFocusResult 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

2
libraries/AP_Camera/AP_Camera_Backend.h
View File

@ -68,7 +68,7 @@ public:
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
virtual bool set_focus(FocusType focus_type, float focus_value) { return false; }
virtual SetFocusResult set_focus(FocusType focus_type, float focus_value) { return SetFocusResult::UNSUPPORTED; }
// 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

6
libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp
View File

@ -92,11 +92,11 @@ bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
SetFocusResult AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
{
// exit immediately if have not found camera or does not support focus
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
return false;
return SetFocusResult::FAILED;
}
// prepare and send message
@ -120,7 +120,7 @@ bool AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
_link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
return true;
return SetFocusResult::ACCEPTED;
}
// handle incoming mavlink message including CAMERA_INFORMATION

2
libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h
View File

@ -48,7 +48,7 @@ public:
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool set_focus(FocusType focus_type, float focus_value) override;
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;

4
libraries/AP_Camera/AP_Camera_Mount.cpp
View File

@ -39,13 +39,13 @@ bool AP_Camera_Mount::set_zoom(ZoomType zoom_type, float zoom_value)
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
SetFocusResult AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
{
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_focus(0, focus_type, focus_value);
}
return false;
return SetFocusResult::FAILED;
}
// send camera information message to GCS

2
libraries/AP_Camera/AP_Camera_Mount.h
View File

@ -44,7 +44,7 @@ public:
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool set_focus(FocusType focus_type, float focus_value) override;
SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan) const override;

4
libraries/AP_Camera/AP_Camera_Scripting.cpp
View File

@ -30,11 +30,11 @@ bool AP_Camera_Scripting::set_zoom(ZoomType zoom_type, float zoom_value)
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera_Scripting::set_focus(FocusType focus_type, float focus_value)
SetFocusResult AP_Camera_Scripting::set_focus(FocusType focus_type, float focus_value)
{
_cam_state.focus_type = (uint8_t)focus_type;
_cam_state.focus_value = focus_value;
return true;
return SetFocusResult::ACCEPTED;
}
// set tracking to none, point or rectangle (see TrackingType enum)

2
libraries/AP_Camera/AP_Camera_Scripting.h
View File

@ -44,7 +44,7 @@ public:
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool set_focus(FocusType focus_type, float focus_value) override;
SetFocusResult 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

10
libraries/AP_Camera/AP_Camera_shareddefs.h
View File

@ -20,9 +20,19 @@ enum class FocusType : uint8_t {
AUTO = 4 // focus automatically. Same as FOCUS_TYPE_AUTO
};
// result type of set_focus. Assumptions are made that this
// enumeration can be cast directly to MAV_RESULT.
enum class SetFocusResult : uint8_t {
ACCEPTED = 0,
INVALID_PARAMETERS = 2, // supported but invalid parameters, like MAV_RESULT_DENIED
UNSUPPORTED = 3,
FAILED = 4,
};
// tracking types when tracking an object in the video stream
enum class TrackingType : uint8_t {
TRK_NONE = 0, // tracking is inactive
TRK_POINT = 1, // tracking a point
TRK_RECTANGLE = 2 // tracking a rectangle
};