Tracker: add disarmed pwm param

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
 };
 

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
     //

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();
 }

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

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();
 }
 
 /**

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 ((PWMDisarmed)g.disarm_pwm.get()) {
-    }
+        case PWMDisarmed::TRIM:
-
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 0);
-    switch (control_mode) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 0);
-    case AUTO:
+            break;
-        if (vehicle.location_valid) {
+        default:
-            update_auto();
+        case PWMDisarmed::ZERO:
-        } else if (tracker.target_set) {
+            SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, 0);
-            update_scan();
+            SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, 0);
+            break;
         }
-        break;
+    } else {
+        switch (control_mode) {
+        case AUTO:
+            if (vehicle.location_valid) {
+                update_auto();
+            } else if (tracker.target_set) {
+                update_scan();
+            }
+            break;
 
-    case MANUAL:
+        case MANUAL:
-        update_manual();
+            update_manual();
-        break;
+            break;
 
-    case SCAN:
+        case SCAN:
-        update_scan();
+            update_scan();
-        break;
+            break;
 
-    case SERVO_TEST:
+        case SERVO_TEST:
-    case STOP:
+        case STOP:
-    case INITIALISING:
+        case INITIALISING:
-        break;
+            break;
+        }
     }
+
+    // convert servo_out to radio_out and send to servo
+    SRV_Channels::calc_pwm();
+    SRV_Channels::output_ch_all();
+    return;
 }
 
 /**