diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 1feaf46b10..c48c1c5253 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -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 diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 231589c3ce..04382c7de5 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -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 diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 42254abcc0..f61a929196 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -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 diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp index 0b3134a20e..37ed06a4b4 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp +++ b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp @@ -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 diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h index d16205093a..44949bc02f 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h +++ b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h @@ -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; diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index 1f98936507..58f3177dd4 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -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 diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index 42216fa8ac..473f719669 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/libraries/AP_Camera/AP_Camera_Mount.h @@ -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; diff --git a/libraries/AP_Camera/AP_Camera_Scripting.cpp b/libraries/AP_Camera/AP_Camera_Scripting.cpp index 956221d608..add22ca4e4 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.cpp +++ b/libraries/AP_Camera/AP_Camera_Scripting.cpp @@ -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) diff --git a/libraries/AP_Camera/AP_Camera_Scripting.h b/libraries/AP_Camera/AP_Camera_Scripting.h index 5cd19a5137..c8e8a035a6 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.h +++ b/libraries/AP_Camera/AP_Camera_Scripting.h @@ -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 diff --git a/libraries/AP_Camera/AP_Camera_shareddefs.h b/libraries/AP_Camera/AP_Camera_shareddefs.h index 491260a0e4..6543eff0bc 100644 --- a/libraries/AP_Camera/AP_Camera_shareddefs.h +++ b/libraries/AP_Camera/AP_Camera_shareddefs.h @@ -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 }; +