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

@@ -16,7 +16,4 @@ void Tracker::update_manual(void)
 
     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

@@ -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,9 +105,18 @@ 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);
+            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;
         }
-
+    } else {
         switch (control_mode) {
         case AUTO:
             if (vehicle.location_valid) {
@@ -132,6 +141,12 @@ void Tracker::update_tracking(void)
         }
     }
 
+    // convert servo_out to radio_out and send to servo
+    SRV_Channels::calc_pwm();
+    SRV_Channels::output_ch_all();
+    return;
+}
+
 /**
    handle an updated position from the aircraft
  */