Tracker: Changing pitch_range to pitch_min and pitch_max

AntennaTracker/Parameters.cpp

@@ -196,15 +196,6 @@ const AP_Param::Info Tracker::var_info[] = {
     // @User: Standard
     GSCALAR(yaw_range,              "YAW_RANGE", YAW_RANGE_DEFAULT),
 
-    // @Param: PITCH_RANGE
-    // @DisplayName: Pitch Range
-    // @Description: Pitch axis total range of motion in degrees
-    // @Units: degrees
-    // @Increment: 0.1
-    // @Range: 0 180
-    // @User: Standard
-    GSCALAR(pitch_range,            "PITCH_RANGE", PITCH_RANGE_DEFAULT),
-
     // @Param: DISTANCE_MIN
     // @DisplayName: Distance minimum to target
     // @Description: Tracker will track targets at least this distance away
@@ -230,6 +221,24 @@ const AP_Param::Info Tracker::var_info[] = {
     // @User: Standard
     GSCALAR(mavlink_update_rate,	"MAV_UPDATE_RATE",	1),
 
+    // @Param: PITCH_MIN
+    // @DisplayName: Minimum Pitch Angle
+    // @Description: The lowest angle the pitch can reach
+    // @Units: Degrees
+    // @Increment: 1
+    // @Range: 0 -90
+    // @User: Standard
+    GSCALAR(pitch_min,               "PITCH_MIN",	PITCH_MIN_DEFAULT),
+
+    // @Param: PITCH_MAX
+    // @DisplayName: Maximum Pitch Angle
+    // @Description: The highest angle the pitch can reach
+    // @Units: Degrees
+    // @Increment: 1
+    // @Range: 0 90
+    // @User: Standard
+    GSCALAR(pitch_max,               "PITCH_MAX",	PITCH_MAX_DEFAULT),
+
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane
     // @Group: GND_

AntennaTracker/Parameters.h

@@ -87,7 +87,7 @@ public:
         k_param_yaw_trim,
         k_param_pitch_trim,
         k_param_yaw_range,
-        k_param_pitch_range,
+        k_param_pitch_range,	//deprecated
         k_param_distance_min,
         k_param_sysid_target,       // 138
         k_param_gcs3,               // stream rates for fourth MAVLink port
@@ -101,6 +101,8 @@ public:
         k_param_servo_yaw_type,
         k_param_alt_source,
         k_param_mavlink_update_rate,
+        k_param_pitch_min,
+        k_param_pitch_max,
 
         //
         // 200 : Radio settings
@@ -149,8 +151,9 @@ public:
     AP_Float yaw_trim;
     AP_Float pitch_trim;
     AP_Int16 yaw_range;             // yaw axis total range of motion in degrees
-    AP_Int16 pitch_range;           // pitch axis total range of motion in degrees
     AP_Int16 distance_min;          // target's must be at least this distance from tracker to be tracked
+    AP_Int16 pitch_min;
+    AP_Int16 pitch_max;
 
     // Waypoints
     //

AntennaTracker/config.h

@@ -42,8 +42,11 @@
 #ifndef YAW_RANGE_DEFAULT
  # define YAW_RANGE_DEFAULT 360
 #endif
-#ifndef PITCH_RANGE_DEFAULT
+#ifndef PITCH_MIN_DEFAULT
- # define PITCH_RANGE_DEFAULT 180
+ # define PITCH_MIN_DEFAULT -90
+#endif
+#ifndef PITCH_MAX_DEFAULT
+ # define PITCH_MAX_DEFAULT 90
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

AntennaTracker/servos.cpp

@@ -11,7 +11,7 @@ void Tracker::init_servos()
 {
     // setup antenna control PWM channels
     channel_yaw.set_angle(g.yaw_range * 100/2);        // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
-    channel_pitch.set_angle(g.pitch_range * 100/2);    // pitch range is +/- (PITCH_RANGE parameter/2) converted to centi-degrees
+    channel_pitch.set_angle((-g.pitch_min+g.pitch_max) * 100/2);    // pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees
 
     // move servos to mid position
     channel_yaw.output_trim();
@@ -59,7 +59,8 @@ void Tracker::update_pitch_position_servo(float pitch)
     // servo_out is in 100ths of a degree
     float ahrs_pitch = ahrs.pitch_sensor*0.01f;
     int32_t angle_err = -(ahrs_pitch - pitch) * 100.0f;
-    int32_t pitch_limit_cd = g.pitch_range*100/2;
+    int32_t pitch_min_cd = g.pitch_min*100;
+    int32_t pitch_max_cd = g.pitch_max*100;
     // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky,
     // above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed
     // param set RC2_REV -1
@@ -83,7 +84,7 @@ void Tracker::update_pitch_position_servo(float pitch)
 
     // rate limit pitch servo
     if (g.pitch_slew_time > 0.02f) {
-        uint16_t max_change = 0.02f * (pitch_limit_cd) / g.pitch_slew_time;
+        uint16_t max_change = 0.02f * ((-pitch_min_cd+pitch_max_cd)/2) / g.pitch_slew_time;
         if (max_change < 1) {
             max_change = 1;
         }
@@ -97,12 +98,12 @@ void Tracker::update_pitch_position_servo(float pitch)
     channel_pitch.set_servo_out(new_servo_out);
 
     // position limit pitch servo
-    if (channel_pitch.get_servo_out() <= -pitch_limit_cd) {
+    if (channel_pitch.get_servo_out() <= pitch_min_cd) {
-        channel_pitch.set_servo_out(-pitch_limit_cd);
+        channel_pitch.set_servo_out(pitch_min_cd);
         g.pidPitch2Srv.reset_I();
     }
-    if (channel_pitch.get_servo_out() >= pitch_limit_cd) {
+    if (channel_pitch.get_servo_out() >= pitch_max_cd) {
-        channel_pitch.set_servo_out(pitch_limit_cd);
+        channel_pitch.set_servo_out(pitch_max_cd);
         g.pidPitch2Srv.reset_I();
     }
 }
@@ -119,15 +120,17 @@ void Tracker::update_pitch_onoff_servo(float pitch)
     // get_servo_out() is in 100ths of a degree
     float ahrs_pitch = degrees(ahrs.pitch);
     float err = ahrs_pitch - pitch;
+    int32_t pitch_min_cd = g.pitch_min*100;
+    int32_t pitch_max_cd = g.pitch_max*100;
 
     float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
     if (fabsf(err) < acceptable_error) {
         channel_pitch.set_servo_out(0);
-    } else if (err > 0) {
+    } else if ((err > 0) && (pitch*100>pitch_min_cd)) {
         // positive error means we are pointing too high, so push the
         // servo down
         channel_pitch.set_servo_out(-9000);
-    } else {
+    } else if (pitch*100<pitch_max_cd){
         // negative error means we are pointing too low, so push the
         // servo up
         channel_pitch.set_servo_out(9000);
@@ -141,8 +144,12 @@ void Tracker::update_pitch_cr_servo(float pitch)
 {
     float ahrs_pitch = degrees(ahrs.pitch);
     float err_cd = (pitch - ahrs_pitch) * 100.0f;
-    g.pidPitch2Srv.set_input_filter_all(err_cd);
+    int32_t pitch_min_cd = g.pitch_min*100;
-    channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
+    int32_t pitch_max_cd = g.pitch_max*100;
+    if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)){
+        g.pidPitch2Srv.set_input_filter_all(err_cd);
+        channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
+    }
 }
 
 /**