Line data    Source code

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