diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 806cb673c6..c4c8cd91dd 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -618,15 +618,14 @@ bool AP_Mount::record_video(uint8_t instance, bool start_recording) return backend->record_video(start_recording); } -// set camera zoom step. returns true on success -// zoom out = -1, hold = 0, zoom in = 1 -bool AP_Mount::set_zoom_step(uint8_t instance, int8_t zoom_step) +// set zoom specified as a rate or percentage +bool AP_Mount::set_zoom(uint8_t instance, AP_Camera::ZoomType zoom_type, float zoom_value) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } - return backend->set_zoom_step(zoom_step); + return backend->set_zoom(zoom_type, zoom_value); } // set focus in, out or hold. returns true on success diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index e441eeca26..ff7524c5cd 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -27,6 +27,7 @@ #include #include #include +#include #include "AP_Mount_Params.h" // maximum number of mounts @@ -180,9 +181,8 @@ public: // set start_recording = true to start record, false to stop recording bool record_video(uint8_t instance, bool start_recording); - // set camera zoom step - // zoom out = -1, hold = 0, zoom in = 1 - bool set_zoom_step(uint8_t instance, int8_t zoom_step); + // set zoom specified as a rate or percentage + bool set_zoom(uint8_t instance, AP_Camera::ZoomType zoom_type, float zoom_value); // set focus in, out or hold // focus in = -1, focus hold = 0, focus out = 1 diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 7afd5545d8..d91bae8dc9 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -124,9 +124,8 @@ public: // set start_recording = true to start record, false to stop recording virtual bool record_video(bool start_recording) { return false; } - // set camera zoom step. returns true on success - // zoom out = -1, hold = 0, zoom in = 1 - virtual bool set_zoom_step(int8_t zoom_step) { return false; } + // set zoom specified as a rate or percentage + virtual bool set_zoom(AP_Camera::ZoomType zoom_type, float zoom_value) { return false; } // set focus in, out or hold. returns true on success // focus in = -1, focus hold = 0, focus out = 1 diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 1a7a2aaa0b..9f5a7e5ca5 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -625,11 +625,22 @@ bool AP_Mount_Siyi::record_video(bool start_recording) return ret; } -// set camera zoom step. returns true on success -// zoom out = -1, hold = 0, zoom in = 1 -bool AP_Mount_Siyi::set_zoom_step(int8_t zoom_step) +// set zoom specified as a rate or percentage +bool AP_Mount_Siyi::set_zoom(AP_Camera::ZoomType zoom_type, float zoom_value) { - return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, (uint8_t)zoom_step); + if (zoom_type == AP_Camera::ZoomType::RATE) { + uint8_t zoom_step = 0; + if (zoom_value > 0) { + zoom_step = 1; + } + if (zoom_value < 0) { + zoom_step = UINT8_MAX; + } + return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, zoom_step); + } + + // unsupported zoom type + return false; } // set focus in, out or hold. returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 7529a3080a..6d000a36e7 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -62,9 +62,8 @@ public: // set start_recording = true to start record, false to stop recording bool record_video(bool start_recording) override; - // set camera zoom step. returns true on success - // zoom out = -1, hold = 0, zoom in = 1 - bool set_zoom_step(int8_t zoom_step) override; + // set zoom specified as a rate or percentage + bool set_zoom(AP_Camera::ZoomType zoom_type, float zoom_value) override; // set focus in, out or hold. returns true on success // focus in = -1, focus hold = 0, focus out = 1