C++ wrap_PI函数代码示例

/// update - update circle controller
void AC_Circle::update()
{
    // calculate dt
    float dt = _pos_control.time_since_last_xy_update();

    // update circle position at poscontrol update rate
    if (dt >= _pos_control.get_dt_xy()) {

        // double check dt is reasonable
        if (dt >= 0.2f) {
            dt = 0.0f;
        }

        // ramp angular velocity to maximum
        if (_angular_vel < _angular_vel_max) {
            _angular_vel += fabsf(_angular_accel) * dt;
            _angular_vel = min(_angular_vel, _angular_vel_max);
        }
        if (_angular_vel > _angular_vel_max) {
            _angular_vel -= fabsf(_angular_accel) * dt;
            _angular_vel = max(_angular_vel, _angular_vel_max);
        }

        // update the target angle and total angle traveled
        float angle_change = _angular_vel * dt;
        _angle += angle_change;
        _angle = wrap_PI(_angle);
        _angle_total += angle_change;

        // if the circle_radius is zero we are doing panorama so no need to update loiter target
        if (!is_zero(_radius)) {
            // calculate target position
            Vector3f target;
            target.x = _center.x + _radius * cosf(-_angle);
            target.y = _center.y - _radius * sinf(-_angle);
            target.z = _pos_control.get_alt_target();

            // update position controller target
            _pos_control.set_xy_target(target.x, target.y);

            // heading is 180 deg from vehicles target position around circle
            _yaw = wrap_PI(_angle-PI) * AC_CIRCLE_DEGX100;
        }else{
            // set target position to center
            Vector3f target;
            target.x = _center.x;
            target.y = _center.y;
            target.z = _pos_control.get_alt_target();

            // update position controller target
            _pos_control.set_xy_target(target.x, target.y);

            // heading is same as _angle but converted to centi-degrees
            _yaw = _angle * AC_CIRCLE_DEGX100;
        }

        // update position controller
        _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);
    }
}

// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain)
{
    // calculate the attitude target euler angles
    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

    // ensure smoothing gain can not cause overshoot
    smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);

    Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
    Vector3f attitude_error_angle;
    attitude_error_quat.to_axis_angle(attitude_error_angle);

    if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
        // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
        // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
        // and an exponential decay specified by smoothing_gain at the end.
        _attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt);
        _attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt);
        _attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt);

        // Convert body-frame angular velocity into euler angle derivative of desired attitude
        ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
    } else {
        _attitude_target_quat = attitude_desired_quat;

        // Set rate feedforward requests to zero
        _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
        _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
    }

    // Call quaternion attitude controller
    attitude_controller_run_quat();
}

// init_start_angle - sets the starting angle around the circle and initialises the angle_total
//  if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
//  if use_heading is false the vehicle's position from the center will be used to initialise the angle
void AC_Circle::init_start_angle(bool use_heading)
{
    // initialise angle total
    _angle_total = 0;

    // if the radius is zero we are doing panorama so init angle to the current heading
    if (_radius <= 0) {
        _angle = _ahrs.yaw;
        return;
    }

    // if use_heading is true
    if (use_heading) {
        _angle = wrap_PI(_ahrs.yaw-PI);
    } else {
        // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
        const Vector3f &curr_pos = _inav.get_position();
        if (is_equal(curr_pos.x,_center.x) && is_equal(curr_pos.y,_center.y)) {
            _angle = wrap_PI(_ahrs.yaw-PI);
        } else {
            // get bearing from circle center to vehicle in radians
            float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
            _angle = wrap_PI(bearing_rad);
        }
    }
}

/// set pilot desired acceleration in centi-degrees
//   dt should be the time (in seconds) since the last call to this function
void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
{
    // Convert from centidegrees on public interface to radians
    const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
    const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);

    // convert our desired attitude to an acceleration vector assuming we are hovering
    const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f);
    const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
    const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);

    // rotate acceleration vectors input to lat/lon frame
    _desired_accel.x = (pilot_accel_fwd_cms*_ahrs.cos_yaw() - pilot_accel_rgt_cms*_ahrs.sin_yaw());
    _desired_accel.y = (pilot_accel_fwd_cms*_ahrs.sin_yaw() + pilot_accel_rgt_cms*_ahrs.cos_yaw());

    // difference between where we think we should be and where we want to be
    Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y));

    // calculate the angular velocity that we would expect given our desired and predicted attitude
    _attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt);

    // update our predicted attitude based on our predicted angular velocity
    _predicted_euler_angle += _predicted_euler_rate * dt;

    // convert our predicted attitude to an acceleration vector assuming we are hovering
    const float pilot_predicted_cos_pitch_target = cosf(_predicted_euler_angle.y);
    const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
    const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.y);

    // rotate acceleration vectors input to lat/lon frame
    _predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
    _predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());
}

TEST(MathWrapTest, AnglePI)
{
    const float accuracy = 1.0e-5;

    EXPECT_NEAR(M_PI,    wrap_PI(M_PI),      accuracy);
    EXPECT_NEAR(0.f,     wrap_PI(M_2PI),     accuracy);
    EXPECT_NEAR(0,       wrap_PI(M_PI * 10), accuracy);
}

//
// drift_correction_yaw - yaw drift correction using the compass
//
void
AP_AHRS_MPU6000::drift_correction_yaw(void)
{
    static float yaw_last_uncorrected = HEADING_UNKNOWN;
    static float yaw_corrected = HEADING_UNKNOWN;
    float yaw_delta = 0;
    bool new_value = false;
    float yaw_error;
    static float heading;

    // we assume the DCM matrix is completely uncorrect for yaw
    // retrieve how much heading has changed according to non-corrected dcm
    if( yaw_last_uncorrected != HEADING_UNKNOWN ) {
        yaw_delta = wrap_PI(yaw - yaw_last_uncorrected);                // the change in heading according to the gyros only since the last interation
        yaw_last_uncorrected = yaw;
    }

    // initialise yaw_corrected (if necessary)
    if( yaw_corrected != HEADING_UNKNOWN ) {
        yaw_corrected = yaw;
    }else{
        yaw_corrected = wrap_PI(yaw_corrected + yaw_delta);             // best guess of current yaw is previous iterations corrected yaw + yaw change from gyro
        _dcm_matrix.from_euler(roll, pitch, yaw_corrected);                     // rebuild dcm matrix with best guess at current yaw
    }

    // if we have new compass data
    if( _compass && _compass->use_for_yaw() ) {
        if (_compass->last_update != _compass_last_update) {
            _compass_last_update = _compass->last_update;
            heading = _compass->calculate_heading(_dcm_matrix);
            if( !_have_initial_yaw ) {
                yaw_corrected = heading;
                _have_initial_yaw = true;
                _dcm_matrix.from_euler(roll, pitch, yaw_corrected);                             // rebuild dcm matrix with best guess at current yaw
            }
            new_value = true;
        }
    }

    // perform the yaw correction
    if( new_value ) {
        yaw_error = yaw_error_compass();
        // the yaw error is a vector in earth frame
        //Vector3f error = Vector3f(0,0, yaw_error);

        // convert the error vector to body frame
        //error = _dcm_matrix.mul_transpose(error);

        // Update the differential component to reduce the error (it´s like a P control)
        yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get());                    // probably completely wrong

        // rebuild the dcm matrix yet again
        _dcm_matrix.from_euler(roll, pitch, yaw_corrected);
    }
}

// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain)
{
    // Convert from centidegrees on public interface to radians
    float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
    float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
    float euler_yaw_angle = radians(euler_yaw_angle_cd*0.01f);

    // calculate the attitude target euler angles
    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

    // ensure smoothing gain can not cause overshoot
    smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    euler_roll_angle += get_roll_trim_rad();

    if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
        // translate the roll pitch and yaw acceleration limits to the euler axis
        Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));

        // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
        // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
        // and an exponential decay specified by smoothing_gain at the end.
        _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
        _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);
        _attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z, _dt);
        if (slew_yaw) {
            _attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
        }

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
        euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
    } else {
        // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
        _attitude_target_euler_angle.x = euler_roll_angle;
        _attitude_target_euler_angle.y = euler_pitch_angle;
        if (slew_yaw) {
            // Compute constrained angle error
            float angle_error = constrain_float(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), -get_slew_yaw_rads()*_dt, get_slew_yaw_rads()*_dt);
            // Update attitude target from constrained angle error
            _attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
        } else {
            _attitude_target_euler_angle.z = euler_yaw_angle;
        }
        // Compute quaternion target attitude
        _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

        // Set rate feedforward requests to zero
        _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
        _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
    }

    // Call quaternion attitude controller
    attitude_controller_run_quat();
}

//
// drift_correction_yaw - yaw drift correction using the compass
//    we have no way to update the dmp with it's actual heading from our
//    compass instead we use the yaw_corrected variable to hold what we think
//    is the real heading we also record what the dmp said it's last heading
//    was in the yaw_last_uncorrected variable so that on the next iteration we
//    can add the change in yaw to our estimate
//
void
AP_AHRS_MPU6000::drift_correction_yaw(void)
{
    static float yaw_corrected = HEADING_UNKNOWN;
    static float last_dmp_yaw = HEADING_UNKNOWN;
    // roll pitch and yaw values from dmp
    float dmp_roll, dmp_pitch, dmp_yaw;
    // change in yaw according to dmp
    float yaw_delta;
    // difference between heading and corrected yaw
    float yaw_error;
    static float heading;

    // get uncorrected yaw values from dmp
    _mpu6000->quaternion.to_euler(&dmp_roll, &dmp_pitch, &dmp_yaw);

    // initialise headings on first iteration
    if( yaw_corrected == HEADING_UNKNOWN ) {
        yaw_corrected = dmp_yaw;
        last_dmp_yaw = dmp_yaw;
    }

    // change in yaw according to dmp
    yaw_delta = wrap_PI(dmp_yaw - last_dmp_yaw);
    yaw_corrected = wrap_PI(yaw_corrected + yaw_delta);
    last_dmp_yaw = dmp_yaw;

    // rebuild dcm matrix
    _dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected);

    // if we have new compass data
    if(_compass && _compass->use_for_yaw() ) {
        if(_compass->last_update != _compass_last_update) {
            _compass_last_update = _compass->last_update;
            heading = _compass->calculate_heading(_dcm_matrix);

            // if this is the first good compass reading then set yaw to this heading
            if( !_have_initial_yaw ) {
                _have_initial_yaw = true;
                yaw_corrected = wrap_PI(heading);
            }

            // yaw correction based on compass
            //yaw_error = yaw_error_compass();
            yaw_error = wrap_PI(heading - yaw_corrected);

            // shift the corrected yaw towards the compass heading a bit
            yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get() * 0.1);

            // rebuild the dcm matrix yet again
            _dcm_matrix.from_euler(dmp_roll, dmp_pitch, yaw_corrected);
        }
    }
}

void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
    // Convert from centidegrees on public interface to radians
    float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
    float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
    float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);

    Vector3f    att_error_euler_rad;

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    euler_roll_angle_rad += get_roll_trim_rad();

    // Set roll/pitch attitude targets from input.
    _att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
    _att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());

    // Update roll/pitch attitude error.
    att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
    att_error_euler_rad.y = wrap_PI(_att_target_euler_rad.y - _ahrs.pitch);

    // Zero the roll and pitch feed-forward rate.
    _att_target_euler_rate_rads.x = 0;
    _att_target_euler_rate_rads.y = 0;

    if (get_accel_yaw_max_radss() > 0.0f) {
        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
        _att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);

        // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
        update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
    } else {
        // When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
        // is fed forward into the rate controller.
        _att_target_euler_rate_rads.z = euler_yaw_rate_rads;
        update_att_target_and_error_yaw(_att_target_euler_rate_rads.z, att_error_euler_rad, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
    }

    // Convert 321-intrinsic euler angle errors to a body-frame rotation vector
    // NOTE: This results in an approximation of the attitude error based on a linearization about the current attitude
    euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_error_euler_rad, _att_error_rot_vec_rad);

    // Compute the angular velocity target from the attitude error
    update_ang_vel_target_from_att_error();

    // Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
    // NOTE: This should be done about the desired attitude instead of about the vehicle attitude
    euler_rate_to_ang_vel(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_target_euler_rate_rads, _att_target_ang_vel_rads);
    // NOTE: A rotation of _att_target_ang_vel_rads from desired body frame to estimated body frame is possibly omitted here

    // Add the angular velocity feedforward
    _ang_vel_target_rads += _att_target_ang_vel_rads;
}

bool Plane::verify_takeoff()
{
    if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
        const float min_gps_speed = 5;
        if (auto_state.takeoff_speed_time_ms == 0 && 
            gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
            gps.ground_speed() > min_gps_speed &&
            hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
            auto_state.takeoff_speed_time_ms = millis();
        }
        if (auto_state.takeoff_speed_time_ms != 0 &&
            millis() - auto_state.takeoff_speed_time_ms >= 2000) {
            // once we reach sufficient speed for good GPS course
            // estimation we save our current GPS ground course
            // corrected for summed yaw to set the take off
            // course. This keeps wings level until we are ready to
            // rotate, and also allows us to cope with arbitrary
            // compass errors for auto takeoff
            float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err;
            takeoff_course = wrap_PI(takeoff_course);
            steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)",
                              steer_state.hold_course_cd,
                              (double)gps.ground_speed(),
                              (double)degrees(steer_state.locked_course_err));
        }
    }

    if (steer_state.hold_course_cd != -1) {
        // call navigation controller for heading hold
        nav_controller->update_heading_hold(steer_state.hold_course_cd);
    } else {
        nav_controller->update_level_flight();        
    }

    // see if we have reached takeoff altitude
    int32_t relative_alt_cm = adjusted_relative_altitude_cm();
    if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
                          (double)(relative_alt_cm*0.01f));
        steer_state.hold_course_cd = -1;
        auto_state.takeoff_complete = true;
        next_WP_loc = prev_WP_loc = current_loc;

        plane.complete_auto_takeoff();

        // don't cross-track on completion of takeoff, as otherwise we
        // can end up doing too sharp a turn
        auto_state.next_wp_no_crosstrack = true;
        return true;
    } else {
        return false;
    }
}

void AC_AttitudeControl::update_att_target_roll(float euler_roll_rate_rads, float overshoot_max_rad)
{
    // Compute constrained angle error
    float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.x - _ahrs.roll), -overshoot_max_rad, overshoot_max_rad);

    // Update attitude target from constrained angle error
    _att_target_euler_rad.x = angle_error + _ahrs.roll;

    // Increment the attitude target
    _att_target_euler_rad.x += euler_roll_rate_rads * _dt;
    _att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x);
}

float AP_Landing_Deepstall::update_steering()
{
    Location current_loc;
    if ((!landing.ahrs.get_position(current_loc) || !landing.ahrs.healthy()) && !hold_level) {
        // panic if no position source is available
        // continue the stall but target just holding the wings held level as deepstall should be a minimal
        // energy configuration on the aircraft, and if a position isn't available aborting would be worse
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
        hold_level = true;
    }

    float desired_change = 0.0f;

    if (!hold_level) {
        uint32_t time = AP_HAL::millis();
        float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3;
        last_time = time;

        Vector2f ab = location_diff(arc_exit, extended_approach);
        ab.normalize();
        Vector2f a_air = location_diff(arc_exit, current_loc);

        crosstrack_error = a_air % ab;
        float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
        float nu1 = asinf(sine_nu1);

        if (L1_i > 0) {
            L1_xtrack_i += nu1 * L1_i / dt;
            L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
            nu1 += L1_xtrack_i;
        }
        desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw) / time_constant;
    }

    float yaw_rate = landing.ahrs.get_gyro().z;
    float yaw_rate_limit_rps = radians(yaw_rate_limit);
    float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);

#ifdef DEBUG_PRINTS
    gcs().send_text(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
                                    (double)crosstrack_error,
                                    (double)error,
                                    (double)degrees(yaw_rate),
                                    (double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS

    return ds_PID.get_pid(error);
}

/*
  Wrap AHRS yaw if in reverse - radians
 */
float AP_L1_Control::get_yaw()
{
    if (_reverse) {
        return wrap_PI(M_PI + _ahrs.yaw);
    }
    return _ahrs.yaw;
}

// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
//      this should be called whenever the radius or rate are changed
//      initialises the yaw and current position around the circle
void AC_Circle::calc_velocities()
{
    // if we are doing a panorama set the circle_angle to the current heading
    if (_radius <= 0) {
        _angular_vel_max = ToRad(_rate);
        _angular_accel = _angular_vel_max;  // reach maximum yaw velocity in 1 second
    } else {
        // set starting angle to current heading - 180 degrees
        _angle = wrap_PI(_ahrs.yaw-PI);

        // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
        float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));

        // angular_velocity in radians per second
        _angular_vel_max = velocity_max/_radius;
        _angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);

        // angular_velocity in radians per second
        _angular_accel = _pos_control.get_accel_xy()/_radius;
        if (_rate < 0.0f) {
            _angular_accel = -_angular_accel;
        }
    }

    // initialise angular velocity
    _angular_vel = 0;
}

/*
 * Adjusts the desired velocity based on output from the proximity sensor
 */
void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel)
{
    // exit immediately if proximity sensor is not present
    if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
        return;
    }

    // exit immediately if no desired velocity
    if (desired_vel.is_zero()) {
        return;
    }

    // normalise desired velocity vector
    Vector2f vel_dir = desired_vel.normalized();

    // get angle of desired velocity
    float heading_rad = atan2f(vel_dir.y, vel_dir.x);

    // rotate desired velocity angle into body-frame angle
    float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw);

    // get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame)
    float distance_m;
    if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) {
        limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f));
    }
}