Tracker: roll compensated slewing

AntennaTracker/Tracker.h

@@ -132,6 +132,12 @@ private:
     RC_Channel channel_yaw{CH_YAW};
     RC_Channel channel_pitch{CH_PITCH};
 
+    LowPassFilterFloat yaw_servo_out_filt;
+    LowPassFilterFloat pitch_servo_out_filt;
+
+    bool yaw_servo_out_filt_init = false;
+    bool pitch_servo_out_filt_init = false;
+
     AP_SerialManager serial_manager;
     const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
     GCS_MAVLINK_Tracker gcs[MAVLINK_COMM_NUM_BUFFERS];
@@ -175,8 +181,6 @@ private:
 
     uint8_t one_second_counter = 0;
     bool target_set = false;
-    int8_t slew_dir = 0;
-    uint32_t slew_start_ms = 0;
 
     // use this to prevent recursion during sensor init
     bool in_mavlink_delay = false;
@@ -254,8 +258,10 @@ private:
     void start_logging();
     void log_init(void);
     bool should_log(uint32_t mask);
-    void calc_angle_error(float pitch, float yaw);
+    void calc_angle_error(float pitch, float yaw, bool direction_reversed);
     void calc_body_frame_target(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
+    bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
+    bool get_ef_yaw_direction();
 
 public:
     void mavlink_snoop(const mavlink_message_t* msg);

AntennaTracker/control_auto.cpp

@@ -20,7 +20,9 @@ void Tracker::update_auto(void)
     float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
     float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90) * 100; // target pitch in centidegrees
 
-    calc_angle_error(pitch, yaw);
+    bool direction_reversed = get_ef_yaw_direction();
+
+    calc_angle_error(pitch, yaw, direction_reversed);
 
     float bf_pitch;
     float bf_yaw;
@@ -32,23 +34,31 @@ void Tracker::update_auto(void)
         update_yaw_servo(bf_yaw);
     }
 }
-void Tracker::calc_angle_error( float pitch, float yaw)
+void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
 {
-	// Pitch angle error in centidegrees
-	// Positive error means the target is above current pitch
+    // Pitch angle error in centidegrees
+    // Positive error means the target is above current pitch
     // Negative error means the target is below current pitch
     float ahrs_pitch = ahrs.pitch_sensor;
-    nav_status.angle_error_pitch = pitch - ahrs_pitch;
+    int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
 
     // Yaw angle error in centidegrees
-	// Positive error means the target is right of current yaw
+    // Positive error means the target is right of current yaw
     // Negative error means the target is left of current yaw
     int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
-    nav_status.angle_error_yaw = wrap_180_cd(yaw - ahrs_yaw_cd);
+    int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);
+    if (direction_reversed) {
+        if (ef_yaw_angle_error > 0) {
+            ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;
+        } else {
+            ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd);
+        }
+    }
 
     // earth frame to body frame angle error conversion
-    float bf_pitch_err   = ahrs.cos_roll()     * nav_status.angle_error_pitch    + ahrs.sin_roll()  * ahrs.cos_pitch() * nav_status.angle_error_yaw;
-    float bf_yaw_err     = -ahrs.sin_roll()    * nav_status.angle_error_pitch    + ahrs.cos_pitch() * ahrs.cos_roll()  * nav_status.angle_error_yaw;
+    float bf_pitch_err;
+    float bf_yaw_err;
+    calc_body_frame_target(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
     nav_status.angle_error_pitch = bf_pitch_err;
     nav_status.angle_error_yaw = bf_yaw_err;
 }
@@ -58,3 +68,65 @@ void Tracker::calc_body_frame_target(float pitch, float yaw, float& bf_pitch, fl
     bf_pitch   = ahrs.cos_roll()     * pitch    + ahrs.sin_roll()  * ahrs.cos_pitch() * yaw;
     bf_yaw     = -ahrs.sin_roll()    * pitch    + ahrs.cos_pitch() * ahrs.cos_roll()  * yaw;
 }
+
+bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
+{
+    // avoid divide by zero
+    if (is_zero(ahrs.cos_pitch())) {
+        return false;
+    }
+    // convert earth frame angle or rates to body frame
+    ef_pitch =  ahrs.cos_roll()                     * pitch -  ahrs.sin_roll()                     * yaw; //pitch
+    ef_yaw =   (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw; //yaw
+    return true;
+}
+
+// return value is true if takign the long road to the target, false if normal, shortest direction should be used
+bool Tracker::get_ef_yaw_direction()
+{
+    // calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
+    float yaw_angle_limit_lower =   (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
+    float yaw_angle_limit_upper =   (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
+    float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get();
+    float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get();
+
+    // distances to earthframe angle limits in centi-degrees
+    float ef_yaw_limit_lower = yaw_angle_limit_lower;
+    float ef_yaw_limit_upper = yaw_angle_limit_upper;
+    float ef_pitch_limit_lower = pitch_angle_limit_lower;
+    float ef_pitch_limit_upper = pitch_angle_limit_upper;
+    convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
+    convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
+
+    float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
+    float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
+    float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);
+
+    // calculate angle error to target in both directions (i.e. moving up/right or lower/left)
+    float yaw_angle_error_upper;
+    float yaw_angle_error_lower;
+    if (ef_yaw_angle_error >= 0) {
+        yaw_angle_error_upper = ef_yaw_angle_error;
+        yaw_angle_error_lower = ef_yaw_angle_error - 36000;
+    } else {
+        yaw_angle_error_upper = 36000 + ef_yaw_angle_error;
+        yaw_angle_error_lower = ef_yaw_angle_error;
+    }
+
+    // checks that the vehicle is outside the tracker's range
+    if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {
+        // if the tracker is trying to move clockwise to reach the vehicle,
+        // but the tracker coudl get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
+        if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {
+            return true;
+        }
+        // if the tracker is trying to move counter-clockwise to reach the vehicle,
+        // but the tracker coudl get closer to the vehicle by moving then set direction_reversed to true
+        if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {
+            return true;
+        }
+    }
+
+    // if we get this far we can use the regular, shortest path to the target
+    return false;
+}

AntennaTracker/defines.h

@@ -30,6 +30,10 @@ enum AltSource {
 	ALT_SOURCE_GPS_VEH_ONLY=2
 };
 
+//  Filter
+#define SERVO_OUT_FILT_HZ               0.1f
+#define G_Dt                            0.02f
+
 //  Logging parameters
 #define MASK_LOG_ATTITUDE               (1<<0)
 #define MASK_LOG_GPS                    (1<<1)

AntennaTracker/servos.cpp

@@ -20,6 +20,9 @@ void Tracker::init_servos()
     // initialise output to servos
     channel_yaw.calc_pwm();
     channel_pitch.calc_pwm();
+
+    yaw_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
+    pitch_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
 }
 
 /**
@@ -77,30 +80,24 @@ void Tracker::update_pitch_position_servo()
     g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
     int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
 
-    // rate limit pitch servo
-    if (g.pitch_slew_time > 0.02f) {
-        uint16_t max_change = 0.02f * ((-pitch_min_cd+pitch_max_cd)/2) / g.pitch_slew_time;
-        if (max_change < 1) {
-            max_change = 1;
-        }
-        if (new_servo_out <= channel_pitch.get_servo_out() - max_change) {
-            new_servo_out = channel_pitch.get_servo_out() - max_change;
-        }
-        if (new_servo_out >= channel_pitch.get_servo_out() + max_change) {
-            new_servo_out = channel_pitch.get_servo_out() + max_change;
-        }
-    }
-    channel_pitch.set_servo_out(new_servo_out);
-
     // position limit pitch servo
-    if (channel_pitch.get_servo_out() <= pitch_min_cd) {
-        channel_pitch.set_servo_out(pitch_min_cd);
+    if (new_servo_out <= pitch_min_cd) {
+        new_servo_out = pitch_min_cd;
         g.pidPitch2Srv.reset_I();
     }
-    if (channel_pitch.get_servo_out() >= pitch_max_cd) {
-        channel_pitch.set_servo_out(pitch_max_cd);
+    if (new_servo_out >= pitch_max_cd) {
+        new_servo_out = pitch_max_cd;
         g.pidPitch2Srv.reset_I();
     }
+    // rate limit pitch servo
+    channel_pitch.set_servo_out(new_servo_out);
+
+    if (pitch_servo_out_filt_init) {
+        pitch_servo_out_filt.apply(new_servo_out, G_Dt);
+    } else {
+        pitch_servo_out_filt.reset(new_servo_out);
+        pitch_servo_out_filt_init = true;
+    }
 }
 
 
@@ -120,7 +117,7 @@ void Tracker::update_pitch_onoff_servo(float pitch)
         // positive error means we are pointing too low, so push the
         // servo up
         channel_pitch.set_servo_out(-9000);
-    } else if (pitch*100<pitch_max_cd){
+    } else if (pitch*100<pitch_max_cd) {
         // negative error means we are pointing too high, so push the
         // servo down
         channel_pitch.set_servo_out(9000);
@@ -134,7 +131,7 @@ void Tracker::update_pitch_cr_servo(float pitch)
 {
     int32_t pitch_min_cd = g.pitch_min*100;
     int32_t pitch_max_cd = g.pitch_max*100;
-    if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)){
+    if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)) {
         g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
         channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
     }
@@ -173,7 +170,6 @@ void Tracker::update_yaw_servo(float yaw)
 void Tracker::update_yaw_position_servo()
 {
     int32_t yaw_limit_cd = g.yaw_range*100/2;
-    const int16_t margin = MAX(500, wrap_360_cd(36000-yaw_limit_cd)/2);
 
     // Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation
     // the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking.
@@ -181,7 +177,6 @@ void Tracker::update_yaw_position_servo()
     // This algorithm accounts for the fact that the antenna mount may not be aligned with North
     // (in fact, any alignment is permissible), and that the alignment may change (possibly rapidly) over time
     // (as when the antenna is mounted on a moving, turning vehicle)
-    // When the servo is being forced beyond its limits, it rapidly slews to the 'other side' then normal tracking takes over.
     //
     // With my antenna mount, large pwm output drives the antenna anticlockise, so need:
     // param set RC1_REV -1
@@ -203,81 +198,29 @@ void Tracker::update_yaw_position_servo()
       Use our current yawspeed to determine if we are moving in the
       right direction
      */
-    int8_t new_slew_dir = slew_dir;
-
-    // get earth frame z-axis rotation rate in radians
-    Vector3f earth_rotation = ahrs.get_gyro() * ahrs.get_rotation_body_to_ned();
-
 
-    bool making_progress;
-    if (slew_dir != 0) {
-        making_progress = (-slew_dir * earth_rotation.z >= 0);
-    } else {
-        making_progress = (nav_status.angle_error_yaw * earth_rotation.z <= 0);
-    }
-
-    // handle hitting servo limits
-    if (abs(channel_yaw.get_servo_out()) == 18000 &&
-        labs(nav_status.angle_error_yaw) > margin &&
-        making_progress &&
-        AP_HAL::millis() - slew_start_ms > g.min_reverse_time*1000) {
-        // we are at the limit of the servo and are not moving in the
-        // right direction, so slew the other way
-        new_slew_dir = -channel_yaw.get_servo_out() / 18000;
-        slew_start_ms = AP_HAL::millis();
-    }
-
-    /*
-      stop slewing and revert to normal control when normal control
-      should move us in the right direction
-     */
-    if (slew_dir * nav_status.angle_error_yaw > margin) {
-        new_slew_dir = 0;
-    }
+    g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
+    float servo_change = g.pidYaw2Srv.get_pid();
+    servo_change = constrain_float(servo_change, -18000, 18000);
+    float new_servo_out = constrain_float(channel_yaw.get_servo_out() + servo_change, -18000, 18000);
 
-    if (new_slew_dir != slew_dir) {
-        tracker.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Slew: %d/%d err=%ld servo=%ld ez=%.3f",
-                                  (int)slew_dir, (int)new_slew_dir,
-                                  (long)nav_status.angle_error_yaw,
-                                  (long)channel_yaw.get_servo_out(),
-                                  (double)degrees(ahrs.get_gyro().z));
+    // position limit yaw servo
+    if (new_servo_out <= -yaw_limit_cd) {
+        new_servo_out = -yaw_limit_cd;
+        g.pidYaw2Srv.reset_I();
     }
-
-    slew_dir = new_slew_dir;
-    int16_t new_servo_out;
-    if (slew_dir != 0) {
-        new_servo_out = slew_dir * 18000;
+    if (new_servo_out >= yaw_limit_cd) {
+        new_servo_out = yaw_limit_cd;
         g.pidYaw2Srv.reset_I();
-    } else {
-        g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
-        float servo_change = g.pidYaw2Srv.get_pid();
-        servo_change = constrain_float(servo_change, -18000, 18000);
-        new_servo_out = constrain_float(channel_yaw.get_servo_out() + servo_change, -18000, 18000);
     }
 
-    // rate limit yaw servo
-    if (g.yaw_slew_time > 0.02f) {
-        uint16_t max_change = 0.02f * yaw_limit_cd / g.yaw_slew_time;
-        if (max_change < 1) {
-            max_change = 1;
-        }
-        if (new_servo_out <= channel_yaw.get_servo_out() - max_change) {
-            new_servo_out = channel_yaw.get_servo_out() - max_change;
-        }
-        if (new_servo_out >= channel_yaw.get_servo_out() + max_change) {
-            new_servo_out = channel_yaw.get_servo_out() + max_change;
-        }
-    }
     channel_yaw.set_servo_out(new_servo_out);
 
-    // position limit yaw servo
-    if (channel_yaw.get_servo_out() <= -yaw_limit_cd) {
-        channel_yaw.set_servo_out(-yaw_limit_cd);
-        g.pidYaw2Srv.reset_I();
-    }
-    if (channel_yaw.get_servo_out() >= yaw_limit_cd) {
-        channel_yaw.set_servo_out(yaw_limit_cd);
-        g.pidYaw2Srv.reset_I();
+    if(yaw_servo_out_filt_init){
+        yaw_servo_out_filt.apply(new_servo_out, G_Dt);
+    } else {
+        yaw_servo_out_filt.reset(new_servo_out);
+        yaw_servo_out_filt_init = true;
     }
 }