Line data    Source code

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