自学教程：C++ wrap_PI函数代码示例

/// update - update circle controllervoid 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 smoothingvoid 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 anglevoid 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 functionvoid 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//voidAP_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
C++ wrap_here函数代码示例
C++ wrap_360_cd函数代码示例