AntennaTracker/servos.cpp

   1 #include "Tracker.h"
   2
   3 /*
   4  * Code to move pitch and yaw servos to attain a target heading or pitch
   5  */
   6
   7 // init_servos - initialises the servos
   8 void Tracker::init_servos()
   9 {
  10     // update assigned functions and enable auxiliary servos
  11     SRV_Channels::enable_aux_servos();
  12
  13     SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_tracker_yaw);
  14     SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch);
  15
  16     // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
  17     SRV_Channels::set_angle(SRV_Channel::k_tracker_yaw, g.yaw_range * 100/2);
  18
  19     // pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees
  20     SRV_Channels::set_angle(SRV_Channel::k_tracker_pitch, (-g.pitch_min+g.pitch_max) * 100/2);
  21
  22     SRV_Channels::calc_pwm();
  23     SRV_Channels::output_ch_all();
  24
  25     yaw_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
  26     pitch_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
  27 }
  28
  29 /**
  30    update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
  31    requested pitch, so the board (and therefore the antenna) will be pointing at the target
  32  */
  33 void Tracker::update_pitch_servo(float pitch)
  34 {
  35     switch ((enum ServoType)g.servo_pitch_type.get()) {
  36     case SERVO_TYPE_ONOFF:
  37         update_pitch_onoff_servo(pitch);
  38         break;
  39
  40     case SERVO_TYPE_CR:
  41         update_pitch_cr_servo(pitch);
  42         break;
  43
  44     case SERVO_TYPE_POSITION:
  45     default:
  46         update_pitch_position_servo();
  47         break;
  48     }
  49 }
  50
  51 /**
  52    update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
  53    requested pitch, so the board (and therefore the antenna) will be pointing at the target
  54  */
  55 void Tracker::update_pitch_position_servo()
  56 {
  57     int32_t pitch_min_cd = g.pitch_min*100;
  58     int32_t pitch_max_cd = g.pitch_max*100;
  59     // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky,
  60     // above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed
  61     // param set RC2_REV -1
  62     //
  63     // The pitch servo (RC channel 2) is configured for servo_out of -9000-0-9000 servo_out,
  64     // which will drive the servo from RC2_MIN to RC2_MAX usec pulse width.
  65     // Therefore, you must set RC2_MIN and RC2_MAX so that your servo drives the antenna altitude between -90 to 90 exactly
  66     // To drive my HS-645MG servos through their full 180 degrees of rotational range, I have to set:
  67     // param set RC2_MAX 2540
  68     // param set RC2_MIN 640
  69     //
  70     // You will also need to tune the pitch PID to suit your antenna and servos. I use:
  71     // PITCH2SRV_P      0.100000
  72     // PITCH2SRV_I      0.020000
  73     // PITCH2SRV_D      0.000000
  74     // PITCH2SRV_IMAX   4000.000000
  75
  76     // calculate new servo position
  77     float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch, G_Dt);
  78
  79     // position limit pitch servo
  80     if (new_servo_out <= pitch_min_cd) {
  81         new_servo_out = pitch_min_cd;
  82         g.pidPitch2Srv.reset_I();
  83     }
  84     if (new_servo_out >= pitch_max_cd) {
  85         new_servo_out = pitch_max_cd;
  86         g.pidPitch2Srv.reset_I();
  87     }
  88     // rate limit pitch servo
  89     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, new_servo_out);
  90
  91     if (pitch_servo_out_filt_init) {
  92         pitch_servo_out_filt.apply(new_servo_out, G_Dt);
  93     } else {
  94         pitch_servo_out_filt.reset(new_servo_out);
  95         pitch_servo_out_filt_init = true;
  96     }
  97 }
  98
  99
 100 /**
 101    update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
 102    requested pitch, so the board (and therefore the antenna) will be pointing at the target
 103  */
 104 void Tracker::update_pitch_onoff_servo(float pitch) const
 105 {
 106     int32_t pitch_min_cd = g.pitch_min*100;
 107     int32_t pitch_max_cd = g.pitch_max*100;
 108
 109     float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
 110     if (fabsf(nav_status.angle_error_pitch) < acceptable_error) {
 111         SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 0);
 112     } else if ((nav_status.angle_error_pitch > 0) && (pitch*100>pitch_min_cd)) {
 113         // positive error means we are pointing too low, so push the
 114         // servo up
 115         SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, -9000);
 116     } else if (pitch*100<pitch_max_cd) {
 117         // negative error means we are pointing too high, so push the
 118         // servo down
 119         SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, 9000);
 120     }
 121 }
 122
 123 /**
 124    update the pitch for continuous rotation servo
 125 */
 126 void Tracker::update_pitch_cr_servo(float pitch)
 127 {
 128     const float pitch_out = constrain_float(g.pidPitch2Srv.update_error(nav_status.angle_error_pitch, G_Dt), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
 129     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, pitch_out);
 130 }
 131
 132 /**
 133    update the yaw (azimuth) servo.
 134  */
 135 void Tracker::update_yaw_servo(float yaw)
 136 {
 137         switch ((enum ServoType)g.servo_yaw_type.get()) {
 138     case SERVO_TYPE_ONOFF:
 139         update_yaw_onoff_servo(yaw);
 140         break;
 141
 142     case SERVO_TYPE_CR:
 143         update_yaw_cr_servo(yaw);
 144         break;
 145
 146     case SERVO_TYPE_POSITION:
 147     default:
 148         update_yaw_position_servo();
 149         break;
 150     }
 151 }
 152
 153 /**
 154    update the yaw (azimuth) servo. The aim is to drive the boards ahrs
 155    yaw to the requested yaw, so the board (and therefore the antenna)
 156    will be pointing at the target
 157  */
 158 void Tracker::update_yaw_position_servo()
 159 {
 160     int32_t yaw_limit_cd = g.yaw_range*100/2;
 161
 162     // Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation
 163     // the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking.
 164     //
 165     // This algorithm accounts for the fact that the antenna mount may not be aligned with North
 166     // (in fact, any alignment is permissible), and that the alignment may change (possibly rapidly) over time
 167     // (as when the antenna is mounted on a moving, turning vehicle)
 168     //
 169     // With my antenna mount, large pwm output drives the antenna anticlockwise, so need:
 170     // param set RC1_REV -1
 171     // to reverse the servo. Yours may be different
 172     //
 173     // You MUST set RC1_MIN and RC1_MAX so that your servo drives the antenna azimuth from -180 to 180 relative
 174     // to the mount.
 175     // To drive my HS-645MG servos through their full 180 degrees of rotational range and therefore the
 176     // antenna through a full 360 degrees, I have to set:
 177     // param set RC1_MAX 2380
 178     // param set RC1_MIN 680
 179     // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html,
 180     // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1
 181
 182     /*
 183       a positive error means that we need to rotate clockwise
 184       a negative error means that we need to rotate counter-clockwise
 185
 186       Use our current yawspeed to determine if we are moving in the
 187       right direction
 188      */
 189
 190     float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw, G_Dt);
 191     servo_change = constrain_float(servo_change, -18000, 18000);
 192     float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_yaw) + servo_change, -18000, 18000);
 193
 194     // position limit yaw servo
 195     if (new_servo_out <= -yaw_limit_cd) {
 196         new_servo_out = -yaw_limit_cd;
 197         g.pidYaw2Srv.reset_I();
 198     }
 199     if (new_servo_out >= yaw_limit_cd) {
 200         new_servo_out = yaw_limit_cd;
 201         g.pidYaw2Srv.reset_I();
 202     }
 203
 204     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, new_servo_out);
 205
 206     if (yaw_servo_out_filt_init) {
 207         yaw_servo_out_filt.apply(new_servo_out, G_Dt);
 208     } else {
 209         yaw_servo_out_filt.reset(new_servo_out);
 210         yaw_servo_out_filt_init = true;
 211     }
 212 }
 213
 214
 215 /**
 216    update the yaw (azimuth) servo. The aim is to drive the boards ahrs
 217    yaw to the requested yaw, so the board (and therefore the antenna)
 218    will be pointing at the target
 219  */
 220 void Tracker::update_yaw_onoff_servo(float yaw) const
 221 {
 222     float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime;
 223     if (fabsf(nav_status.angle_error_yaw * 0.01f) < acceptable_error) {
 224         SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 0);
 225     } else if (nav_status.angle_error_yaw * 0.01f > 0) {
 226         // positive error means we are counter-clockwise of the target, so
 227         // move clockwise
 228         SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, 18000);
 229     } else {
 230         // negative error means we are clockwise of the target, so
 231         // move counter-clockwise
 232         SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, -18000);
 233     }
 234 }
 235
 236 /**
 237    update the yaw continuous rotation servo
 238  */
 239 void Tracker::update_yaw_cr_servo(float yaw)
 240 {
 241     const float yaw_out = constrain_float(-g.pidYaw2Srv.update_error(nav_status.angle_error_yaw, G_Dt), -g.yaw_range * 100/2, g.yaw_range * 100/2);
 242     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out);
 243 }