From c128c80d5681d6f31730da619bfe93c808be0377 Mon Sep 17 00:00:00 2001
From: Peter Hall <33176108+IamPete1@users.noreply.github.com>
Date: Thu, 13 Jun 2019 18:16:29 +0100
Subject: [PATCH] Tracker: add disarmed pwm param

---
 AntennaTracker/Parameters.cpp     |  6 ++++
 AntennaTracker/Parameters.h       |  4 ++-
 AntennaTracker/control_manual.cpp |  5 +--
 AntennaTracker/defines.h          |  5 +++
 AntennaTracker/servos.cpp         | 10 +-----
 AntennaTracker/tracking.cpp       | 59 +++++++++++++++++++------------
 6 files changed, 53 insertions(+), 36 deletions(-)

diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp
index 25153ea6f9..015aa6b4b0 100644
--- a/AntennaTracker/Parameters.cpp
+++ b/AntennaTracker/Parameters.cpp
@@ -403,6 +403,12 @@ const AP_Param::Info Tracker::var_info[] = {
     // @User: Standard
     GSCALAR(initial_mode,            "INITIAL_MODE",     10),
 
+    // @Param: SAFE_DISARM_PWM
+    // @DisplayName: PWM that will be output when disarmed or in stop mode
+    // @Description: 0:zero pwm, 1:trim pwm
+    // @User: Standard
+    GSCALAR(disarm_pwm,              "SAFE_DISARM_PWM",        0),
+
     AP_VAREND
 };
 
diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h
index 95feeb4035..1de33ecaea 100644
--- a/AntennaTracker/Parameters.h
+++ b/AntennaTracker/Parameters.h
@@ -119,7 +119,8 @@ public:
         k_param_gcs_pid_mask = 225,
         k_param_scan_speed_yaw,
         k_param_scan_speed_pitch,
-        k_param_initial_mode
+        k_param_initial_mode,
+        k_param_disarm_pwm
     };
 
     AP_Int16 format_version;
@@ -156,6 +157,7 @@ public:
     AP_Int16 pitch_max;
     AP_Int16 gcs_pid_mask;
     AP_Int8  initial_mode;
+    AP_Int8 disarm_pwm;
 
     // Waypoints
     //
diff --git a/AntennaTracker/control_manual.cpp b/AntennaTracker/control_manual.cpp
index 5cc87a5dff..0213c48e34 100644
--- a/AntennaTracker/control_manual.cpp
+++ b/AntennaTracker/control_manual.cpp
@@ -13,10 +13,7 @@ void Tracker::update_manual(void)
     // copy yaw and pitch input to output
     SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, RC_Channels::rc_channel(CH_YAW)->get_radio_in());
     SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_yaw);
-    
+
     SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, RC_Channels::rc_channel(CH_PITCH)->get_radio_in());
     SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch);
-    
-    SRV_Channels::calc_pwm();
-    SRV_Channels::output_ch_all();
 }
diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h
index 830b13fe40..5e3d05f1d8 100644
--- a/AntennaTracker/defines.h
+++ b/AntennaTracker/defines.h
@@ -31,6 +31,11 @@ enum mode_reason_t {
     MODE_REASON_GCS_COMMAND,
 };
 
+enum class PWMDisarmed {
+    ZERO = 0,
+    TRIM,
+};
+
 //  Filter
 #define SERVO_OUT_FILT_HZ               0.1f
 #define G_Dt                            0.02f
diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp
index b42047aaaf..89bf3aa3b7 100644
--- a/AntennaTracker/servos.cpp
+++ b/AntennaTracker/servos.cpp
@@ -14,7 +14,7 @@ void Tracker::init_servos()
     SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch);
 
     // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
-    SRV_Channels::set_angle(SRV_Channel::k_tracker_yaw, g.yaw_range * 100/2);        
+    SRV_Channels::set_angle(SRV_Channel::k_tracker_yaw, g.yaw_range * 100/2);
 
     // pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees
     SRV_Channels::set_angle(SRV_Channel::k_tracker_pitch, (-g.pitch_min+g.pitch_max) * 100/2);
@@ -46,10 +46,6 @@ void Tracker::update_pitch_servo(float pitch)
         update_pitch_position_servo();
         break;
     }
-
-    // convert servo_out to radio_out and send to servo
-    SRV_Channels::calc_pwm();
-    SRV_Channels::output_ch_all();
 }
 
 /**
@@ -154,10 +150,6 @@ void Tracker::update_yaw_servo(float yaw)
         update_yaw_position_servo();
         break;
     }
-
-    // convert servo_out to radio_out and send to servo
-    SRV_Channels::calc_pwm();
-    SRV_Channels::output_ch_all();
 }
 
 /**
diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
index 81e41827aa..0c46fe6ab5 100644
--- a/AntennaTracker/tracking.cpp
+++ b/AntennaTracker/tracking.cpp
@@ -105,31 +105,46 @@ void Tracker::update_tracking(void)
     }
     // do not move if we are not armed:
     if (!hal.util->get_soft_armed()) {
-        return;
-    }
-
-    switch (control_mode) {
-    case AUTO:
-        if (vehicle.location_valid) {
-            update_auto();
-        } else if (tracker.target_set) {
-            update_scan();
+        switch ((PWMDisarmed)g.disarm_pwm.get()) {
+        case PWMDisarmed::TRIM:
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 0);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 0);
+            break;
+        default:
+        case PWMDisarmed::ZERO:
+            SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, 0);
+            SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, 0);
+            break;
         }
-        break;
-
-    case MANUAL:
-        update_manual();
-        break;
-
-    case SCAN:
-        update_scan();
-        break;
+    } else {
+        switch (control_mode) {
+        case AUTO:
+            if (vehicle.location_valid) {
+                update_auto();
+            } else if (tracker.target_set) {
+                update_scan();
+            }
+            break;
+
+        case MANUAL:
+            update_manual();
+            break;
+
+        case SCAN:
+            update_scan();
+            break;
 
-    case SERVO_TEST:
-    case STOP:
-    case INITIALISING:
-        break;
+        case SERVO_TEST:
+        case STOP:
+        case INITIALISING:
+            break;
+        }
     }
+
+    // convert servo_out to radio_out and send to servo
+    SRV_Channels::calc_pwm();
+    SRV_Channels::output_ch_all();
+    return;
 }
 
 /**