From 90d74e55d39251304f535e7bddb7c00b0e22f424 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 17 Apr 2015 17:45:26 -0700 Subject: [PATCH] AntennaTracker: fix newlines --- AntennaTracker/control_auto.pde | 44 +-- AntennaTracker/control_manual.pde | 40 +- AntennaTracker/control_scan.pde | 76 ++-- AntennaTracker/servos.pde | 618 +++++++++++++++--------------- 4 files changed, 389 insertions(+), 389 deletions(-) diff --git a/AntennaTracker/control_auto.pde b/AntennaTracker/control_auto.pde index 1e4bf32075..19b1a1d95a 100644 --- a/AntennaTracker/control_auto.pde +++ b/AntennaTracker/control_auto.pde @@ -1,22 +1,22 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * control_auto.pde - auto control mode - */ - -/* - * update_auto - runs the auto controller - * called at 50hz while control_mode is 'AUTO' - */ -static void update_auto(void) -{ - // exit immediately if we do not have a valid vehicle position - if (!vehicle.location_valid) { - return; - } - - float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f; - float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90); - update_pitch_servo(pitch); - update_yaw_servo(yaw); -} +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * control_auto.pde - auto control mode + */ + +/* + * update_auto - runs the auto controller + * called at 50hz while control_mode is 'AUTO' + */ +static void update_auto(void) +{ + // exit immediately if we do not have a valid vehicle position + if (!vehicle.location_valid) { + return; + } + + float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f; + float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90); + update_pitch_servo(pitch); + update_yaw_servo(yaw); +} diff --git a/AntennaTracker/control_manual.pde b/AntennaTracker/control_manual.pde index 822fcdb809..3d56f234d4 100644 --- a/AntennaTracker/control_manual.pde +++ b/AntennaTracker/control_manual.pde @@ -1,20 +1,20 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * control_manual.pde - manual control mode - */ - -/* - * update_manual - runs the manual controller - * called at 50hz while control_mode is 'MANUAL' - */ -static void update_manual(void) -{ - // copy yaw and pitch input to output - channel_yaw.radio_out = constrain_int16(channel_yaw.radio_in, channel_yaw.radio_min, channel_yaw.radio_max); - channel_pitch.radio_out = constrain_int16(channel_pitch.radio_in, channel_pitch.radio_min, channel_pitch.radio_max); - - // send output to servos - channel_yaw.output(); - channel_pitch.output(); -} +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * control_manual.pde - manual control mode + */ + +/* + * update_manual - runs the manual controller + * called at 50hz while control_mode is 'MANUAL' + */ +static void update_manual(void) +{ + // copy yaw and pitch input to output + channel_yaw.radio_out = constrain_int16(channel_yaw.radio_in, channel_yaw.radio_min, channel_yaw.radio_max); + channel_pitch.radio_out = constrain_int16(channel_pitch.radio_in, channel_pitch.radio_min, channel_pitch.radio_max); + + // send output to servos + channel_yaw.output(); + channel_pitch.output(); +} diff --git a/AntennaTracker/control_scan.pde b/AntennaTracker/control_scan.pde index 47ad9f6488..15e7d1c7db 100644 --- a/AntennaTracker/control_scan.pde +++ b/AntennaTracker/control_scan.pde @@ -1,38 +1,38 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * control_scan.pde - scan control mode - */ - -/* - * update_scan - runs the scan controller - * called at 50hz while control_mode is 'SCAN' - */ -static void update_scan(void) -{ - if (!nav_status.manual_control_yaw) { - float yaw_delta = g.scan_speed * 0.02f; - nav_status.bearing += yaw_delta * (nav_status.scan_reverse_yaw?-1:1); - if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) { - nav_status.scan_reverse_yaw = false; - } - if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) { - nav_status.scan_reverse_yaw = true; - } - nav_status.bearing = constrain_float(nav_status.bearing, 0, 360); - } - - if (!nav_status.manual_control_pitch) { - float pitch_delta = g.scan_speed * 0.02f; - nav_status.pitch += pitch_delta * (nav_status.scan_reverse_pitch?-1:1); - if (nav_status.pitch < -90 && nav_status.scan_reverse_pitch) { - nav_status.scan_reverse_pitch = false; - } - if (nav_status.pitch > 90 && !nav_status.scan_reverse_pitch) { - nav_status.scan_reverse_pitch = true; - } - nav_status.pitch = constrain_float(nav_status.pitch, -90, 90); - } - - update_auto(); -} +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * control_scan.pde - scan control mode + */ + +/* + * update_scan - runs the scan controller + * called at 50hz while control_mode is 'SCAN' + */ +static void update_scan(void) +{ + if (!nav_status.manual_control_yaw) { + float yaw_delta = g.scan_speed * 0.02f; + nav_status.bearing += yaw_delta * (nav_status.scan_reverse_yaw?-1:1); + if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) { + nav_status.scan_reverse_yaw = false; + } + if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) { + nav_status.scan_reverse_yaw = true; + } + nav_status.bearing = constrain_float(nav_status.bearing, 0, 360); + } + + if (!nav_status.manual_control_pitch) { + float pitch_delta = g.scan_speed * 0.02f; + nav_status.pitch += pitch_delta * (nav_status.scan_reverse_pitch?-1:1); + if (nav_status.pitch < -90 && nav_status.scan_reverse_pitch) { + nav_status.scan_reverse_pitch = false; + } + if (nav_status.pitch > 90 && !nav_status.scan_reverse_pitch) { + nav_status.scan_reverse_pitch = true; + } + nav_status.pitch = constrain_float(nav_status.pitch, -90, 90); + } + + update_auto(); +} diff --git a/AntennaTracker/servos.pde b/AntennaTracker/servos.pde index 1fdc9c329f..5a43f8d560 100644 --- a/AntennaTracker/servos.pde +++ b/AntennaTracker/servos.pde @@ -1,309 +1,309 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * servos.pde - code to move pitch and yaw servos to attain a target heading or pitch - */ - -// init_servos - initialises the servos -static void init_servos() -{ - // setup antenna control PWM channels - channel_yaw.set_angle(g.yaw_range * 100/2); // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees - channel_pitch.set_angle(g.pitch_range * 100/2); // pitch range is +/- (PITCH_RANGE parameter/2) converted to centi-degrees - - // move servos to mid position - channel_yaw.output_trim(); - channel_pitch.output_trim(); - - // initialise output to servos - channel_yaw.calc_pwm(); - channel_pitch.calc_pwm(); -} - -/** - update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the - requested pitch, so the board (and therefore the antenna) will be pointing at the target - */ -static void update_pitch_servo(float pitch) -{ - switch ((enum ServoType)g.servo_type.get()) { - case SERVO_TYPE_ONOFF: - update_pitch_onoff_servo(pitch); - break; - - case SERVO_TYPE_POSITION: - default: - update_pitch_position_servo(pitch); - break; - } - - // convert servo_out to radio_out and send to servo - channel_pitch.calc_pwm(); - channel_pitch.output(); -} - -/** - update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the - requested pitch, so the board (and therefore the antenna) will be pointing at the target - */ -static void update_pitch_position_servo(float pitch) -{ - // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal - // pitch argument is -90 to 90, where 0 is horizontal - // servo_out is in 100ths of a degree - float ahrs_pitch = ahrs.pitch_sensor*0.01f; - int32_t angle_err = -(ahrs_pitch - pitch) * 100.0; - int32_t pitch_limit_cd = g.pitch_range*100/2; - // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky, - // above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed - // param set RC2_REV -1 - // - // The pitch servo (RC channel 2) is configured for servo_out of -9000-0-9000 servo_out, - // which will drive the servo from RC2_MIN to RC2_MAX usec pulse width. - // Therefore, you must set RC2_MIN and RC2_MAX so that your servo drives the antenna altitude between -90 to 90 exactly - // To drive my HS-645MG servos through their full 180 degrees of rotational range, I have to set: - // param set RC2_MAX 2540 - // param set RC2_MIN 640 - // - // You will also need to tune the pitch PID to suit your antenna and servos. I use: - // PITCH2SRV_P 0.100000 - // PITCH2SRV_I 0.020000 - // PITCH2SRV_D 0.000000 - // PITCH2SRV_IMAX 4000.000000 - - // calculate new servo position - int32_t new_servo_out = channel_pitch.servo_out + g.pidPitch2Srv.get_pid(angle_err); - - // initialise limit flags - servo_limit.pitch_lower = false; - servo_limit.pitch_upper = false; - - // rate limit pitch servo - if (g.pitch_slew_time > 0.02f) { - uint16_t max_change = 0.02f * (pitch_limit_cd) / g.pitch_slew_time; - if (max_change < 1) { - max_change = 1; - } - if (new_servo_out <= channel_pitch.servo_out - max_change) { - new_servo_out = channel_pitch.servo_out - max_change; - servo_limit.pitch_lower = true; - } - if (new_servo_out >= channel_pitch.servo_out + max_change) { - new_servo_out = channel_pitch.servo_out + max_change; - servo_limit.pitch_upper = true; - } - } - channel_pitch.servo_out = new_servo_out; - - // position limit pitch servo - if (channel_pitch.servo_out <= -pitch_limit_cd) { - channel_pitch.servo_out = -pitch_limit_cd; - servo_limit.pitch_lower = true; - } - if (channel_pitch.servo_out >= pitch_limit_cd) { - channel_pitch.servo_out = pitch_limit_cd; - servo_limit.pitch_upper = true; - } -} - - -/** - update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the - requested pitch, so the board (and therefore the antenna) will be pointing at the target - */ -static void update_pitch_onoff_servo(float pitch) -{ - // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal - // pitch argument is -90 to 90, where 0 is horizontal - // servo_out is in 100ths of a degree - float ahrs_pitch = degrees(ahrs.pitch); - float err = ahrs_pitch - pitch; - - float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime; - if (fabsf(err) < acceptable_error) { - channel_pitch.servo_out = 0; - } else if (err > 0) { - // positive error means we are pointing too high, so push the - // servo down - channel_pitch.servo_out = -9000; - } else { - // negative error means we are pointing too low, so push the - // servo up - channel_pitch.servo_out = 9000; - } -} - -/** - update the yaw (azimuth) servo. - */ -static void update_yaw_servo(float yaw) -{ - switch ((enum ServoType)g.servo_type.get()) { - case SERVO_TYPE_ONOFF: - update_yaw_onoff_servo(yaw); - break; - - case SERVO_TYPE_POSITION: - default: - update_yaw_position_servo(yaw); - break; - } - - // convert servo_out to radio_out and send to servo - channel_yaw.calc_pwm(); - channel_yaw.output(); -} - -/** - update the yaw (azimuth) servo. The aim is to drive the boards ahrs - yaw to the requested yaw, so the board (and therefore the antenna) - will be pointing at the target - */ -static void update_yaw_position_servo(float yaw) -{ - int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); - int32_t yaw_cd = wrap_180_cd(yaw*100); - int32_t yaw_limit_cd = g.yaw_range*100/2; - const int16_t margin = 500; // 5 degrees slop - - // 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. - // - // This algorithm accounts for the fact that the antenna mount may not be aligned with North - // (in fact, any alignment is permissable), 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 - // to reverse the servo. Yours may be different - // - // You MUST set RC1_MIN and RC1_MAX so that your servo drives the antenna azimuth from -180 to 180 relative - // to the mount. - // To drive my HS-645MG servos through their full 180 degrees of rotational range and therefore the - // antenna through a full 360 degrees, I have to set: - // param set RC1_MAX 2380 - // param set RC1_MIN 680 - // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html, - // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1 - int32_t angle_err = wrap_180_cd(ahrs_yaw_cd - yaw_cd); - - /* - a positive error means that we need to rotate counter-clockwise - a negative error means that we need to rotate clockwise - - Use our current yawspeed to determine if we are moving in the - right direction - */ - static int8_t slew_dir; - static uint32_t slew_start_ms; - int8_t new_slew_dir = slew_dir; - - // get earth frame z-axis rotation rate in radians - Vector3f earth_rotation = ahrs.get_gyro() * ahrs.get_dcm_matrix(); - - - bool making_progress; - if (slew_dir != 0) { - making_progress = (-slew_dir * earth_rotation.z >= 0); - } else { - making_progress = (angle_err * earth_rotation.z >= 0); - } - - // handle hitting servo limits - if (abs(channel_yaw.servo_out) == 18000 && - abs(angle_err) > margin && - making_progress && - hal.scheduler->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.servo_out / 18000; - slew_start_ms = hal.scheduler->millis(); - } - - /* - stop slewing and revert to normal control when normal control - should move us in the right direction - */ - if (slew_dir * angle_err < -margin) { - new_slew_dir = 0; - } - - if (new_slew_dir != slew_dir) { - gcs_send_text_fmt(PSTR("SLEW: %d/%d err=%ld servo=%ld ez=%.3f"), - (int)slew_dir, (int)new_slew_dir, - (long)angle_err, - (long)channel_yaw.servo_out, - degrees(ahrs.get_gyro().z)); - } - - slew_dir = new_slew_dir; - - // initialise limit flags - servo_limit.yaw_lower = false; - servo_limit.yaw_upper = false; - - int16_t new_servo_out; - if (slew_dir != 0) { - new_servo_out = slew_dir * 18000; - g.pidYaw2Srv.reset_I(); - } else { - float servo_change = g.pidYaw2Srv.get_pid(angle_err); - servo_change = constrain_float(servo_change, -18000, 18000); - new_servo_out = constrain_float(channel_yaw.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.servo_out - max_change) { - new_servo_out = channel_yaw.servo_out - max_change; - servo_limit.yaw_lower = true; - } - if (new_servo_out >= channel_yaw.servo_out + max_change) { - new_servo_out = channel_yaw.servo_out + max_change; - servo_limit.yaw_upper = true; - } - } - channel_yaw.servo_out = new_servo_out; - - // position limit pitch servo - if (channel_yaw.servo_out <= -yaw_limit_cd) { - channel_yaw.servo_out = -yaw_limit_cd; - servo_limit.yaw_lower = true; - } - if (channel_yaw.servo_out >= yaw_limit_cd) { - channel_yaw.servo_out = yaw_limit_cd; - servo_limit.yaw_upper = true; - } -} - - -/** - update the yaw (azimuth) servo. The aim is to drive the boards ahrs - yaw to the requested yaw, so the board (and therefore the antenna) - will be pointing at the target - */ -static void update_yaw_onoff_servo(float yaw) -{ - int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); - int32_t yaw_cd = wrap_180_cd(yaw*100); - int32_t err_cd = wrap_180_cd(ahrs_yaw_cd - yaw_cd); - float err = err_cd * 0.01f; - - float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime; - if (fabsf(err) < acceptable_error) { - channel_yaw.servo_out = 0; - } else if (err > 0) { - // positive error means we are clockwise of the target, so - // move anti-clockwise - channel_yaw.servo_out = -18000; - } else { - // negative error means we are anti-clockwise of the target, so - // move clockwise - channel_yaw.servo_out = 18000; - } -} +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * servos.pde - code to move pitch and yaw servos to attain a target heading or pitch + */ + +// init_servos - initialises the servos +static void init_servos() +{ + // setup antenna control PWM channels + channel_yaw.set_angle(g.yaw_range * 100/2); // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees + channel_pitch.set_angle(g.pitch_range * 100/2); // pitch range is +/- (PITCH_RANGE parameter/2) converted to centi-degrees + + // move servos to mid position + channel_yaw.output_trim(); + channel_pitch.output_trim(); + + // initialise output to servos + channel_yaw.calc_pwm(); + channel_pitch.calc_pwm(); +} + +/** + update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the + requested pitch, so the board (and therefore the antenna) will be pointing at the target + */ +static void update_pitch_servo(float pitch) +{ + switch ((enum ServoType)g.servo_type.get()) { + case SERVO_TYPE_ONOFF: + update_pitch_onoff_servo(pitch); + break; + + case SERVO_TYPE_POSITION: + default: + update_pitch_position_servo(pitch); + break; + } + + // convert servo_out to radio_out and send to servo + channel_pitch.calc_pwm(); + channel_pitch.output(); +} + +/** + update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the + requested pitch, so the board (and therefore the antenna) will be pointing at the target + */ +static void update_pitch_position_servo(float pitch) +{ + // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal + // pitch argument is -90 to 90, where 0 is horizontal + // servo_out is in 100ths of a degree + float ahrs_pitch = ahrs.pitch_sensor*0.01f; + int32_t angle_err = -(ahrs_pitch - pitch) * 100.0; + int32_t pitch_limit_cd = g.pitch_range*100/2; + // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky, + // above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed + // param set RC2_REV -1 + // + // The pitch servo (RC channel 2) is configured for servo_out of -9000-0-9000 servo_out, + // which will drive the servo from RC2_MIN to RC2_MAX usec pulse width. + // Therefore, you must set RC2_MIN and RC2_MAX so that your servo drives the antenna altitude between -90 to 90 exactly + // To drive my HS-645MG servos through their full 180 degrees of rotational range, I have to set: + // param set RC2_MAX 2540 + // param set RC2_MIN 640 + // + // You will also need to tune the pitch PID to suit your antenna and servos. I use: + // PITCH2SRV_P 0.100000 + // PITCH2SRV_I 0.020000 + // PITCH2SRV_D 0.000000 + // PITCH2SRV_IMAX 4000.000000 + + // calculate new servo position + int32_t new_servo_out = channel_pitch.servo_out + g.pidPitch2Srv.get_pid(angle_err); + + // initialise limit flags + servo_limit.pitch_lower = false; + servo_limit.pitch_upper = false; + + // rate limit pitch servo + if (g.pitch_slew_time > 0.02f) { + uint16_t max_change = 0.02f * (pitch_limit_cd) / g.pitch_slew_time; + if (max_change < 1) { + max_change = 1; + } + if (new_servo_out <= channel_pitch.servo_out - max_change) { + new_servo_out = channel_pitch.servo_out - max_change; + servo_limit.pitch_lower = true; + } + if (new_servo_out >= channel_pitch.servo_out + max_change) { + new_servo_out = channel_pitch.servo_out + max_change; + servo_limit.pitch_upper = true; + } + } + channel_pitch.servo_out = new_servo_out; + + // position limit pitch servo + if (channel_pitch.servo_out <= -pitch_limit_cd) { + channel_pitch.servo_out = -pitch_limit_cd; + servo_limit.pitch_lower = true; + } + if (channel_pitch.servo_out >= pitch_limit_cd) { + channel_pitch.servo_out = pitch_limit_cd; + servo_limit.pitch_upper = true; + } +} + + +/** + update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the + requested pitch, so the board (and therefore the antenna) will be pointing at the target + */ +static void update_pitch_onoff_servo(float pitch) +{ + // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal + // pitch argument is -90 to 90, where 0 is horizontal + // servo_out is in 100ths of a degree + float ahrs_pitch = degrees(ahrs.pitch); + float err = ahrs_pitch - pitch; + + float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime; + if (fabsf(err) < acceptable_error) { + channel_pitch.servo_out = 0; + } else if (err > 0) { + // positive error means we are pointing too high, so push the + // servo down + channel_pitch.servo_out = -9000; + } else { + // negative error means we are pointing too low, so push the + // servo up + channel_pitch.servo_out = 9000; + } +} + +/** + update the yaw (azimuth) servo. + */ +static void update_yaw_servo(float yaw) +{ + switch ((enum ServoType)g.servo_type.get()) { + case SERVO_TYPE_ONOFF: + update_yaw_onoff_servo(yaw); + break; + + case SERVO_TYPE_POSITION: + default: + update_yaw_position_servo(yaw); + break; + } + + // convert servo_out to radio_out and send to servo + channel_yaw.calc_pwm(); + channel_yaw.output(); +} + +/** + update the yaw (azimuth) servo. The aim is to drive the boards ahrs + yaw to the requested yaw, so the board (and therefore the antenna) + will be pointing at the target + */ +static void update_yaw_position_servo(float yaw) +{ + int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); + int32_t yaw_cd = wrap_180_cd(yaw*100); + int32_t yaw_limit_cd = g.yaw_range*100/2; + const int16_t margin = 500; // 5 degrees slop + + // 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. + // + // This algorithm accounts for the fact that the antenna mount may not be aligned with North + // (in fact, any alignment is permissable), 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 + // to reverse the servo. Yours may be different + // + // You MUST set RC1_MIN and RC1_MAX so that your servo drives the antenna azimuth from -180 to 180 relative + // to the mount. + // To drive my HS-645MG servos through their full 180 degrees of rotational range and therefore the + // antenna through a full 360 degrees, I have to set: + // param set RC1_MAX 2380 + // param set RC1_MIN 680 + // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html, + // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1 + int32_t angle_err = wrap_180_cd(ahrs_yaw_cd - yaw_cd); + + /* + a positive error means that we need to rotate counter-clockwise + a negative error means that we need to rotate clockwise + + Use our current yawspeed to determine if we are moving in the + right direction + */ + static int8_t slew_dir; + static uint32_t slew_start_ms; + int8_t new_slew_dir = slew_dir; + + // get earth frame z-axis rotation rate in radians + Vector3f earth_rotation = ahrs.get_gyro() * ahrs.get_dcm_matrix(); + + + bool making_progress; + if (slew_dir != 0) { + making_progress = (-slew_dir * earth_rotation.z >= 0); + } else { + making_progress = (angle_err * earth_rotation.z >= 0); + } + + // handle hitting servo limits + if (abs(channel_yaw.servo_out) == 18000 && + abs(angle_err) > margin && + making_progress && + hal.scheduler->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.servo_out / 18000; + slew_start_ms = hal.scheduler->millis(); + } + + /* + stop slewing and revert to normal control when normal control + should move us in the right direction + */ + if (slew_dir * angle_err < -margin) { + new_slew_dir = 0; + } + + if (new_slew_dir != slew_dir) { + gcs_send_text_fmt(PSTR("SLEW: %d/%d err=%ld servo=%ld ez=%.3f"), + (int)slew_dir, (int)new_slew_dir, + (long)angle_err, + (long)channel_yaw.servo_out, + degrees(ahrs.get_gyro().z)); + } + + slew_dir = new_slew_dir; + + // initialise limit flags + servo_limit.yaw_lower = false; + servo_limit.yaw_upper = false; + + int16_t new_servo_out; + if (slew_dir != 0) { + new_servo_out = slew_dir * 18000; + g.pidYaw2Srv.reset_I(); + } else { + float servo_change = g.pidYaw2Srv.get_pid(angle_err); + servo_change = constrain_float(servo_change, -18000, 18000); + new_servo_out = constrain_float(channel_yaw.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.servo_out - max_change) { + new_servo_out = channel_yaw.servo_out - max_change; + servo_limit.yaw_lower = true; + } + if (new_servo_out >= channel_yaw.servo_out + max_change) { + new_servo_out = channel_yaw.servo_out + max_change; + servo_limit.yaw_upper = true; + } + } + channel_yaw.servo_out = new_servo_out; + + // position limit pitch servo + if (channel_yaw.servo_out <= -yaw_limit_cd) { + channel_yaw.servo_out = -yaw_limit_cd; + servo_limit.yaw_lower = true; + } + if (channel_yaw.servo_out >= yaw_limit_cd) { + channel_yaw.servo_out = yaw_limit_cd; + servo_limit.yaw_upper = true; + } +} + + +/** + update the yaw (azimuth) servo. The aim is to drive the boards ahrs + yaw to the requested yaw, so the board (and therefore the antenna) + will be pointing at the target + */ +static void update_yaw_onoff_servo(float yaw) +{ + int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); + int32_t yaw_cd = wrap_180_cd(yaw*100); + int32_t err_cd = wrap_180_cd(ahrs_yaw_cd - yaw_cd); + float err = err_cd * 0.01f; + + float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime; + if (fabsf(err) < acceptable_error) { + channel_yaw.servo_out = 0; + } else if (err > 0) { + // positive error means we are clockwise of the target, so + // move anti-clockwise + channel_yaw.servo_out = -18000; + } else { + // negative error means we are anti-clockwise of the target, so + // move clockwise + channel_yaw.servo_out = 18000; + } +}