diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index e8340155e8..f0b7ae3b46 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -425,6 +425,64 @@ void AP_Mount::set_roi_target(uint8_t instance, const Location &target_loc) } } +// +// camera controls for gimbals that include a camera +// + +// take a picture. returns true on success +bool AP_Mount::take_picture(uint8_t instance) +{ + // call instance's take_picture + if (!check_instance(instance)) { + return false; + } + return _backends[instance]->take_picture(); +} + +// start or stop video recording. returns true on success +// set start_recording = true to start record, false to stop recording +bool AP_Mount::record_video(uint8_t instance, bool start_recording) +{ + // call instance's record_video + if (!check_instance(instance)) { + return false; + } + return _backends[instance]->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) +{ + // call instance's set_zoom_step + if (!check_instance(instance)) { + return false; + } + return _backends[instance]->set_zoom_step(zoom_step); +} + +// set focus in, out or hold. returns true on success +// focus in = -1, focus hold = 0, focus out = 1 +bool AP_Mount::set_manual_focus_step(uint8_t instance, int8_t focus_step) +{ + // call instance's set_manual_focus_step + if (!check_instance(instance)) { + return false; + } + return _backends[instance]->set_manual_focus_step(focus_step); +} + +// auto focus. returns true on success +bool AP_Mount::set_auto_focus(uint8_t instance) +{ + // call instance's set_auto_focus + if (!check_instance(instance)) { + return false; + } + return _backends[instance]->set_auto_focus(); +} + + bool AP_Mount::check_primary() const { return check_instance(_primary); diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index b5576aef72..a00e306d86 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -158,6 +158,28 @@ public: // any failure_msg returned will not include a prefix bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); + // + // camera controls for gimbals that include a camera + // + + // take a picture + bool take_picture(uint8_t instance); + + // start or stop video recording + // 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 focus in, out or hold + // focus in = -1, focus hold = 0, focus out = 1 + bool set_manual_focus_step(uint8_t instance, int8_t focus_step); + + // auto focus + bool set_auto_focus(uint8_t instance); + // parameter var table static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index b9dedcaf88..119e30bde4 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -100,6 +100,28 @@ public: // handle GIMBAL_DEVICE_ATTITUDE_STATUS message virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {} + // + // camera controls for gimbals that include a camera + // + + // take a picture. returns true on success + virtual bool take_picture() { return false; } + + // start or stop video recording. returns true on success + // 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 focus in, out or hold. returns true on success + // focus in = -1, focus hold = 0, focus out = 1 + virtual bool set_manual_focus_step(int8_t focus_step) { return false; } + + // auto focus. returns true on success + virtual bool set_auto_focus() { return false; } + protected: enum class MountTargetType {