From dd3376a808f4bcef916a986711e310a951f6e985 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 14 Jul 2023 20:20:04 +0900 Subject: [PATCH] RC_Channel: add camera lens aux function --- libraries/RC_Channel/RC_Channel.cpp | 15 +++++++++++++++ libraries/RC_Channel/RC_Channel.h | 2 ++ 2 files changed, 17 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 0b155587e7..8b275ecf46 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -231,6 +231,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter, Rover, Plane, Blimp}: 172:Battery MPPT Enable // @Values{Plane}: 173:Plane AUTO Mode Landing Abort // @Values{Copter, Rover, Plane, Blimp}: 174:Camera Image Tracking + // @Values{Copter, Rover, Plane, Blimp}: 175:Camera Lens // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail @@ -762,6 +763,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::CAMERA_MANUAL_FOCUS, "Camera Manual Focus"}, { AUX_FUNC::CAMERA_AUTO_FOCUS, "Camera Auto Focus"}, { AUX_FUNC::CAMERA_IMAGE_TRACKING, "Camera Image Tracking"}, + { AUX_FUNC::CAMERA_LENS, "Camera Lens"}, }; /* lookup the announcement for switch change */ @@ -990,6 +992,16 @@ bool RC_Channel::do_aux_function_camera_image_tracking(const AuxSwitchPos ch_fla // Low or Mediums disables tracking. (0.5,0.5) is still passed in but ignored return camera->set_tracking(ch_flag == AuxSwitchPos::HIGH ? TrackingType::TRK_POINT : TrackingType::TRK_NONE, Vector2f{0.5, 0.5}, Vector2f{}); } + +bool RC_Channel::do_aux_function_camera_lens(const AuxSwitchPos ch_flag) +{ + AP_Camera *camera = AP::camera(); + if (camera == nullptr) { + return false; + } + // Low selects lens 0 (default), Mediums selects lens1, High selects lens2 + return camera->set_lens((uint8_t)ch_flag); +} #endif void RC_Channel::do_aux_function_runcam_control(const AuxSwitchPos ch_flag) @@ -1491,6 +1503,9 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos case AUX_FUNC::CAMERA_IMAGE_TRACKING: return do_aux_function_camera_image_tracking(ch_flag); + + case AUX_FUNC::CAMERA_LENS: + return do_aux_function_camera_lens(ch_flag); #endif #if HAL_MOUNT_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 871b3affda..cedc0cc787 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -247,6 +247,7 @@ public: BATTERY_MPPT_ENABLE = 172,// Battery MPPT Power enable. high = ON, mid = auto (controlled by mppt/batt driver), low = OFF. This effects all MPPTs. PLANE_AUTO_LANDING_ABORT = 173, // Abort Glide-slope or VTOL landing during payload place or do_land type mission items CAMERA_IMAGE_TRACKING = 174, // camera image tracking + CAMERA_LENS = 175, // camera lens selection // inputs from 200 will eventually used to replace RCMAP @@ -346,6 +347,7 @@ protected: bool do_aux_function_camera_manual_focus(const AuxSwitchPos ch_flag); bool do_aux_function_camera_auto_focus(const AuxSwitchPos ch_flag); bool do_aux_function_camera_image_tracking(const AuxSwitchPos ch_flag); + bool do_aux_function_camera_lens(const AuxSwitchPos ch_flag); void do_aux_function_runcam_control(const AuxSwitchPos ch_flag); void do_aux_function_runcam_osd_control(const AuxSwitchPos ch_flag); void do_aux_function_fence(const AuxSwitchPos ch_flag);