AntennaTracker/mode.cpp

   1 #include "mode.h"
   2
   3 #include "Tracker.h"
   4
   5 void Mode::update_auto(void)
   6 {
   7     struct Tracker::NavStatus &nav_status = tracker.nav_status;
   8
   9     Parameters &g = tracker.g;
  10
  11     float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
  12     float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees
  13
  14     bool direction_reversed = get_ef_yaw_direction();
  15
  16     calc_angle_error(pitch, yaw, direction_reversed);
  17
  18     float bf_pitch;
  19     float bf_yaw;
  20     convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
  21
  22     // only move servos if target is at least distance_min away if we  have a target
  23     if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !tracker.vehicle.location_valid) {
  24         tracker.update_pitch_servo(bf_pitch);
  25         tracker.update_yaw_servo(bf_yaw);
  26     }
  27 }
  28
  29 void Mode::update_scan(void)
  30 {
  31     struct Tracker::NavStatus &nav_status = tracker.nav_status;
  32
  33     Parameters &g = tracker.g;
  34
  35     if (!nav_status.manual_control_yaw) {
  36         float yaw_delta = g.scan_speed_yaw * 0.02f;
  37         nav_status.bearing   += yaw_delta   * (nav_status.scan_reverse_yaw?-1:1);
  38         if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) {
  39             nav_status.scan_reverse_yaw = false;
  40         }
  41         if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) {
  42             nav_status.scan_reverse_yaw = true;
  43         }
  44         nav_status.bearing = constrain_float(nav_status.bearing, 0, 360);
  45     }
  46
  47     if (!nav_status.manual_control_pitch) {
  48         const float pitch_delta = g.scan_speed_pitch * 0.02f;
  49         if (nav_status.scan_reverse_pitch) {
  50             nav_status.pitch -= pitch_delta;
  51             if (nav_status.pitch < g.pitch_min) {
  52                 nav_status.scan_reverse_pitch = false;
  53             }
  54         } else {
  55             nav_status.pitch += pitch_delta;
  56             if (nav_status.pitch > g.pitch_max) {
  57                 nav_status.scan_reverse_pitch = true;
  58             }
  59         }
  60         nav_status.pitch = constrain_float(nav_status.pitch, g.pitch_min, g.pitch_max);
  61     }
  62
  63     update_auto();
  64 }
  65
  66 void Mode::calc_angle_error(float pitch, float yaw, bool direction_reversed)
  67 {
  68     // Pitch angle error in centidegrees
  69     // Positive error means the target is above current pitch
  70     // Negative error means the target is below current pitch
  71     const AP_AHRS &ahrs = AP::ahrs();
  72     float ahrs_pitch = ahrs.pitch_sensor;
  73     int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
  74
  75     // Yaw angle error in centidegrees
  76     // Positive error means the target is right of current yaw
  77     // Negative error means the target is left of current yaw
  78     int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
  79     int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);
  80     if (direction_reversed) {
  81         if (ef_yaw_angle_error > 0) {
  82             ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;
  83         } else {
  84             ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd);
  85         }
  86     }
  87
  88     // earth frame to body frame angle error conversion
  89     float bf_pitch_err;
  90     float bf_yaw_err;
  91     convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
  92     struct Tracker::NavStatus &nav_status = tracker.nav_status;
  93     nav_status.angle_error_pitch = bf_pitch_err;
  94     nav_status.angle_error_yaw = bf_yaw_err;
  95
  96     // set actual and desired for logging, note we are using angles not rates
  97     Parameters &g = tracker.g;
  98     g.pidPitch2Srv.set_target_rate(pitch * 0.01);
  99     g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
 100     g.pidYaw2Srv.set_target_rate(yaw * 0.01);
 101     g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
 102 }
 103
 104 void Mode::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
 105 {
 106         // earth frame to body frame pitch and yaw conversion
 107     const AP_AHRS &ahrs = AP::ahrs();
 108     bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
 109     bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
 110 }
 111
 112 bool Mode::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
 113 {
 114     const AP_AHRS &ahrs = AP::ahrs();
 115     // avoid divide by zero
 116     if (is_zero(ahrs.cos_pitch())) {
 117         return false;
 118     }
 119     // convert earth frame angle or rates to body frame
 120     ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw;
 121     ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw;
 122     return true;
 123 }
 124
 125 // return value is true if taking the long road to the target, false if normal, shortest direction should be used
 126 bool Mode::get_ef_yaw_direction()
 127 {
 128     // calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
 129     Parameters &g = tracker.g;
 130     float yaw_angle_limit_lower =   (-g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
 131     float yaw_angle_limit_upper =   (g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
 132     float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - tracker.pitch_servo_out_filt.get();
 133     float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - tracker.pitch_servo_out_filt.get();
 134
 135     // distances to earthframe angle limits in centi-degrees
 136     float ef_yaw_limit_lower = yaw_angle_limit_lower;
 137     float ef_yaw_limit_upper = yaw_angle_limit_upper;
 138     float ef_pitch_limit_lower = pitch_angle_limit_lower;
 139     float ef_pitch_limit_upper = pitch_angle_limit_upper;
 140     convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
 141     convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
 142
 143     const AP_AHRS &ahrs = AP::ahrs();
 144     float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
 145     struct Tracker::NavStatus &nav_status = tracker.nav_status;
 146     float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
 147     float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);
 148
 149     // calculate angle error to target in both directions (i.e. moving up/right or lower/left)
 150     float yaw_angle_error_upper;
 151     float yaw_angle_error_lower;
 152     if (ef_yaw_angle_error >= 0) {
 153         yaw_angle_error_upper = ef_yaw_angle_error;
 154         yaw_angle_error_lower = ef_yaw_angle_error - 36000;
 155     } else {
 156         yaw_angle_error_upper = 36000 + ef_yaw_angle_error;
 157         yaw_angle_error_lower = ef_yaw_angle_error;
 158     }
 159
 160     // checks that the vehicle is outside the tracker's range
 161     if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {
 162         // if the tracker is trying to move clockwise to reach the vehicle,
 163         // but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
 164         if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {
 165             return true;
 166         }
 167         // if the tracker is trying to move counter-clockwise to reach the vehicle,
 168         // but the tracker could get closer to the vehicle by moving then set direction_reversed to true
 169         if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {
 170             return true;
 171         }
 172     }
 173
 174     // if we get this far we can use the regular, shortest path to the target
 175     return false;
 176 }