From bcee4b7b82b88e1b3394b0f80ef29e198cecb732 Mon Sep 17 00:00:00 2001
From: Hugo Trippaers <htrippaers@schubergphilis.com>
Date: Mon, 25 Jan 2021 09:39:51 +0100
Subject: [PATCH] AP_Mount: Add parameter to control relative pan option for
 servo mounts

---
 libraries/AP_Mount/AP_Mount.cpp       | 14 +++++++++++++-
 libraries/AP_Mount/AP_Mount.h         |  2 ++
 libraries/AP_Mount/AP_Mount_Servo.cpp |  6 +++---
 3 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 1a7af63454..1ec401bdf7 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -213,7 +213,12 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
 
     // 23 formerly _K_RATE
 
-    // 24 is AVAILABLE
+    // @Param: _REL_PAN
+    // @DisplayName: Relative pan flag for Servo Mount
+    // @Description: Enable to calculate pan angle to GPS location relative to vehicle orientation (for type servo (1))
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("_REL_PAN",        24, AP_Mount, state[0]._rel_pan, 0),
 
 #if AP_MOUNT_MAX_INSTANCES > 1
     // @Param: 2_DEFLT_MODE
@@ -395,6 +400,13 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial
     // @User: Standard
     AP_GROUPINFO("2_TYPE",           42, AP_Mount, state[1]._type, 0),
+
+    // @Param: 2_REL_PAN
+    // @DisplayName: Relative pan flag for Servo Mount 2
+    // @Description: Enable to calculate pan angle to GPS location relative to vehicle orientation (for type servo (1))
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("2_REL_PAN",        43, AP_Mount, state[0]._rel_pan, 0),
 #endif // AP_MOUNT_MAX_INSTANCES > 1
 
     AP_GROUPEND
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index fc1d85979a..7d65e40ce2 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -190,6 +190,8 @@ protected:
         Location _target_sysid_location; // sysid target location
         bool _target_sysid_location_set;
 
+        AP_Int8         _rel_pan;           // Use relative pan for servo mounts (0=Disable, 1=Enable)
+
     } state[AP_MOUNT_MAX_INSTANCES];
 
 private:
diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp
index cbee4c1f15..c430f2f0ff 100644
--- a/libraries/AP_Mount/AP_Mount_Servo.cpp
+++ b/libraries/AP_Mount/AP_Mount_Servo.cpp
@@ -70,7 +70,7 @@ void AP_Mount_Servo::update()
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
         {
-            if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false)) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, _state._rel_pan)) {
                 stabilize();
             }
             break;
@@ -83,7 +83,7 @@ void AP_Mount_Servo::update()
             }
             _state._roi_target = AP::ahrs().get_home();
             _state._roi_target_set = true;
-            if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false)) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, _state._rel_pan)) {
                 stabilize();
             }
             break;
@@ -92,7 +92,7 @@ void AP_Mount_Servo::update()
             if (calc_angle_to_sysid_target(_angle_ef_target_rad,
                                            _flags.tilt_control,
                                            _flags.pan_control,
-                                           false)) {
+                                           _state._rel_pan)) {
                 stabilize();
             }
             break;