From 24445a0c2010d59515ac972d7255178386e0e5bf Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 29 Apr 2023 09:58:00 +0900
Subject: [PATCH] RC_Channel: aux function for camera image tracking

---
 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 b71c0f9bed..7b0d7bb170 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -237,6 +237,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Values{Copter, Rover, Plane, Blimp}: 171:Calibrate Compasses
     // @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{Rover}: 201:Roll
     // @Values{Rover}: 202:Pitch
     // @Values{Rover}: 207:MainSail
@@ -765,6 +766,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
     { AUX_FUNC::CAMERA_ZOOM, "Camera Zoom"},
     { AUX_FUNC::CAMERA_MANUAL_FOCUS, "Camera Manual Focus"},
     { AUX_FUNC::CAMERA_AUTO_FOCUS, "Camera Auto Focus"},
+    { AUX_FUNC::CAMERA_IMAGE_TRACKING, "Camera Image Tracking"},
 };
 
 /* lookup the announcement for switch change */
@@ -982,6 +984,17 @@ bool RC_Channel::do_aux_function_camera_auto_focus(const AuxSwitchPos ch_flag)
     }
     return false;
 }
+
+bool RC_Channel::do_aux_function_camera_image_tracking(const AuxSwitchPos ch_flag)
+{
+    AP_Camera *camera = AP::camera();
+    if (camera == nullptr) {
+        return false;
+    }
+    // High position enables tracking a POINT in middle of image
+    // Low or Mediums disables tracking.  (0.5,0.5) is still passed in but ignored
+    return camera->set_tracking(ch_flag == AuxSwitchPos::HIGH ? TrackingType::POINT : TrackingType::NONE, Vector2f{0.5, 0.5}, Vector2f{});
+}
 #endif
 
 void RC_Channel::do_aux_function_runcam_control(const AuxSwitchPos ch_flag)
@@ -1477,6 +1490,8 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos
     case AUX_FUNC::CAMERA_AUTO_FOCUS:
         return do_aux_function_camera_auto_focus(ch_flag);
 
+    case AUX_FUNC::CAMERA_IMAGE_TRACKING:
+        return do_aux_function_camera_image_tracking(ch_flag);
 #endif
 
 #if HAL_MOUNT_ENABLED
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index f19ab87a19..68a7f03f0d 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -246,6 +246,7 @@ public:
         MAG_CAL =            171, // Calibrate compasses (disarmed only)
         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
 
 
         // inputs from 200 will eventually used to replace RCMAP
@@ -344,6 +345,7 @@ protected:
     bool do_aux_function_camera_zoom(const AuxSwitchPos ch_flag);
     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);
     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);