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

// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination// this function updates the _yaw_error_cd valuevoid Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed){    // record system time of call    last_steer_to_wp_ms = AP_HAL::millis();    // Calculate the required turn of the wheels    // negative error = left turn    // positive error = right turn    rover.nav_controller->set_reverse(reversed);    rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius);    float desired_lat_accel = rover.nav_controller->lateral_acceleration();    float desired_heading = rover.nav_controller->target_bearing_cd();    if (reversed) {        _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor + 18000);    } else {        _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);    }    if (rover.use_pivot_steering(_yaw_error_cd)) {        // for pivot turns use heading controller        calc_steering_to_heading(desired_heading, reversed);    } else {        // call lateral acceleration to steering controller        calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);    }}

void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed){    // Pitch angle error in centidegrees    // Positive error means the target is above current pitch    // Negative error means the target is below current pitch    float ahrs_pitch = ahrs.pitch_sensor;    int32_t ef_pitch_angle_error = pitch - ahrs_pitch;    // Yaw angle error in centidegrees    // Positive error means the target is right of current yaw    // Negative error means the target is left of current yaw    int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);    int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);    if (direction_reversed) {        if (ef_yaw_angle_error > 0) {            ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;        } else {            ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd);        }    }    // earth frame to body frame angle error conversion    float bf_pitch_err;    float bf_yaw_err;    convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);    nav_status.angle_error_pitch = bf_pitch_err;    nav_status.angle_error_yaw = bf_yaw_err;}

void ModeLoiter::update(){    // get distance (in meters) to destination    _distance_to_destination = get_distance(rover.current_loc, _destination);    // if within waypoint radius slew desired speed towards zero and use existing desired heading    if (_distance_to_destination <= g.waypoint_radius) {        _desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);        _yaw_error_cd = 0.0f;    } else {        // P controller with hard-coded gain to convert distance to desired speed        // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation        _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);        // calculate bearing to destination        _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);        _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);        // if destination is behind vehicle, reverse towards it        if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {            _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);            _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);            _desired_speed = -_desired_speed;        }        // reduce desired speed if yaw_error is large        // 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%        float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;        _desired_speed *= yaw_error_ratio;    }    // run steering and throttle controllers    calc_steering_to_heading(_desired_yaw_cd);    calc_throttle(_desired_speed, false, true);}

// guided_angle_control_run - runs the guided angle controller// called from guided_runvoid Copter::guided_angle_control_run(){    // if not auto armed or motors not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.set_yaw_target_to_current_heading();        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());        attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);#else        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);#endif        pos_control.relax_alt_hold_controllers(0.0f);        return;    }    // constrain desired lean angles    float roll_in = guided_angle_state.roll_cd;    float pitch_in = guided_angle_state.pitch_cd;    float total_in = norm(roll_in, pitch_in);    float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max);    if (total_in > angle_max) {        float ratio = angle_max / total_in;        roll_in *= ratio;        pitch_in *= ratio;    }    // wrap yaw request    float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);    float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);    // constrain climb rate    float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());    // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds    uint32_t tnow = millis();    if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {        roll_in = 0.0f;        pitch_in = 0.0f;        climb_rate_cms = 0.0f;        yaw_rate_in = 0.0f;    }    // set motors to full range    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // call attitude controller    if (guided_angle_state.use_yaw_rate) {        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());    } else {        attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());    }    // call position controller    pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);    pos_control.update_z_controller();}

/**   update the yaw continuous rotation servo */void Tracker::update_yaw_cr_servo(float yaw){    int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);    float yaw_cd = wrap_180_cd(yaw*100.0f);    float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);    g.pidYaw2Srv.set_input_filter_all(err_cd);    channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid());}

// guided_angle_control_run - runs the guided angle controller// called from guided_runvoid Copter::ModeGuided::angle_control_run(){    // if not auto armed or motors not enabled set throttle to zero and exit immediately    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {#if FRAME_CONFIG == HELI_FRAME        attitude_control->set_yaw_target_to_current_heading();#endif        zero_throttle_and_relax_ac();        pos_control->relax_alt_hold_controllers(0.0f);        return;    }    // constrain desired lean angles    float roll_in = guided_angle_state.roll_cd;    float pitch_in = guided_angle_state.pitch_cd;    float total_in = norm(roll_in, pitch_in);    float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);    if (total_in > angle_max) {        float ratio = angle_max / total_in;        roll_in *= ratio;        pitch_in *= ratio;    }    // wrap yaw request    float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);    float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);    // constrain climb rate    float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());    // get avoidance adjusted climb rate    climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);    // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds    uint32_t tnow = millis();    if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {        roll_in = 0.0f;        pitch_in = 0.0f;        climb_rate_cms = 0.0f;        yaw_rate_in = 0.0f;    }    // set motors to full range    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // call attitude controller    if (guided_angle_state.use_yaw_rate) {        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);    } else {        attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);    }    // call position controller    pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);    pos_control->update_z_controller();}

// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate requestvoid AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max){    // calculate angle error with maximum of +- max angle overshoot    angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);    angle_ef_error.x  = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max);    // update roll angle target to be within max angle overshoot of our roll angle    _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;    // increment the roll angle target    _angle_ef_target.x += roll_rate_ef * _dt;    _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);}

// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate requestvoid AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error){    // calculate angle error with maximum of +- max angle overshoot    angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);    angle_ef_error.x  = constrain_float(angle_ef_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);    // update roll angle target to be within max angle overshoot of our roll angle    _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;    // increment the roll angle target    _angle_ef_target.x += roll_rate_ef * _dt;    _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);}

// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate requestvoid AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max){    // calculate angle error with maximum of +- max angle overshoot    angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);//把误差转化到180度之内，此误差为上一个目标点与实际角度的误差    angle_ef_error.x  = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max);    // update roll angle target to be within max angle overshoot of our roll angle误差加传感器的值为目标值    _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;//修正当前目标点（猜测在修正传感器误差）    // increment the roll angle target，把roll角目标更新到下一时刻    _angle_ef_target.x += roll_rate_ef * _dt;//更新下一个目标点    _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);}

// return value is true if taking the long road to the target, false if normal, shortest direction should be usedbool Tracker::get_ef_yaw_direction(){    // calculating distances from current pitch/yaw to lower and upper limits in centi-degrees    float yaw_angle_limit_lower =   (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();    float yaw_angle_limit_upper =   (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();    float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get();    float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get();    // distances to earthframe angle limits in centi-degrees    float ef_yaw_limit_lower = yaw_angle_limit_lower;    float ef_yaw_limit_upper = yaw_angle_limit_upper;    float ef_pitch_limit_lower = pitch_angle_limit_lower;    float ef_pitch_limit_upper = pitch_angle_limit_upper;    convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);    convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);    float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);    float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);    float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);    // calculate angle error to target in both directions (i.e. moving up/right or lower/left)    float yaw_angle_error_upper;    float yaw_angle_error_lower;    if (ef_yaw_angle_error >= 0) {        yaw_angle_error_upper = ef_yaw_angle_error;        yaw_angle_error_lower = ef_yaw_angle_error - 36000;    } else {        yaw_angle_error_upper = 36000 + ef_yaw_angle_error;        yaw_angle_error_lower = ef_yaw_angle_error;    }    // checks that the vehicle is outside the tracker's range    if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {        // if the tracker is trying to move clockwise to reach the vehicle,        // but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true        if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {            return true;        }        // if the tracker is trying to move counter-clockwise to reach the vehicle,        // but the tracker could get closer to the vehicle by moving then set direction_reversed to true        if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {            return true;        }    }    // if we get this far we can use the regular, shortest path to the target    return false;}

// set guided mode angle targetvoid Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads){    // check we are in velocity control mode    if (guided_mode != Guided_Angle) {        angle_control_start();    }    // convert quaternion to euler angles    q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);    guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;    guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;    guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);    guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;    guided_angle_state.use_yaw_rate = use_yaw_rate;    guided_angle_state.climb_rate_cms = climb_rate_cms;    guided_angle_state.update_time_ms = millis();    // interpret positive climb rate as triggering take-off    if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {        copter.set_auto_armed(true);    }    // log target    copter.Log_Write_GuidedTarget(guided_mode,                           Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),                           Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));}

// set desired locationvoid Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd){    // set origin to last destination if waypoint controller active    if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {        _origin = _destination;    } else {        // otherwise use reasonable stopping point        calc_stopping_location(_origin);    }    _destination = destination;    // initialise distance    _distance_to_destination = get_distance(_origin, _destination);    _reached_destination = false;    // set final desired speed    _desired_speed_final = 0.0f;    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {        const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);        const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);        if (is_zero(turn_angle_cd)) {            // if not turning can continue at full speed            _desired_speed_final = _desired_speed;        } else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {            // pivoting so we will stop            _desired_speed_final = 0.0f;        } else {            // calculate maximum speed that keeps overshoot within bounds            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));        }    }}

/*  update the total angle we have covered in a loiter. Used to support  commands to do N circles of loiter */void Plane::loiter_angle_update(void){    static const int32_t lap_check_interval_cd = 3*36000;    const int32_t target_bearing_cd = nav_controller->target_bearing_cd();    int32_t loiter_delta_cd;    if (loiter.sum_cd == 0 && !reached_loiter_target()) {        // we don't start summing until we are doing the real loiter        loiter_delta_cd = 0;    } else if (loiter.sum_cd == 0) {        // use 1 cd for initial delta        loiter_delta_cd = 1;        loiter.start_lap_alt_cm = current_loc.alt;        loiter.next_sum_lap_cd = lap_check_interval_cd;    } else {        loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;    }    loiter.old_target_bearing_cd = target_bearing_cd;    loiter_delta_cd = wrap_180_cd(loiter_delta_cd);    loiter.sum_cd += loiter_delta_cd * loiter.direction;    if (labs(current_loc.alt - next_WP_loc.alt) < 500) {        loiter.reached_target_alt = true;        loiter.unable_to_acheive_target_alt = false;        loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;    } else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {        // check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long        loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;        loiter.start_lap_alt_cm = current_loc.alt;        loiter.next_sum_lap_cd += lap_check_interval_cd;    }}

static void test_wrap_cd(void){    for (uint8_t i=0; i < ARRAY_SIZE(wrap_180_tests); i++) {        int32_t r = wrap_180_cd(wrap_180_tests[i].v);        if (r != wrap_180_tests[i].wv) {            hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld/n",                                (long)wrap_180_tests[i].v,                                (long)wrap_180_tests[i].wv,                                (long)r);        }    }    for (uint8_t i=0; i < ARRAY_SIZE(wrap_360_tests); i++) {        int32_t r = wrap_360_cd(wrap_360_tests[i].v);        if (r != wrap_360_tests[i].wv) {            hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld/n",                                (long)wrap_360_tests[i].v,                                (long)wrap_360_tests[i].wv,                                (long)r);        }    }    for (uint8_t i=0; i < ARRAY_SIZE(wrap_PI_tests); i++) {        float r = wrap_PI(wrap_PI_tests[i].v);        if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) {            hal.console->printf("wrap_PI: v=%f wv=%f r=%f/n",                                wrap_PI_tests[i].v,                                wrap_PI_tests[i].wv,                                r);        }    }    hal.console->printf("wrap_cd tests done/n");}

void Rover::calc_lateral_acceleration() {    switch (control_mode) {    case AUTO:        // If we have reached the waypoint previously navigate        // back to it from our current position        if (previously_reached_wp && (loiter_duration > 0)) {            nav_controller->update_waypoint(current_loc, next_WP);        } else {            nav_controller->update_waypoint(prev_WP, next_WP);        }        break;    case RTL:    case GUIDED:    case STEERING:        nav_controller->update_waypoint(current_loc, next_WP);        break;    default:        return;    }    // Calculate the required turn of the wheels    // negative error = left turn    // positive error = right turn    lateral_acceleration = nav_controller->lateral_acceleration();    if (use_pivot_steering()) {        const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;        if (bearing_error > 0) {            lateral_acceleration = g.turn_max_g * GRAVITY_MSS;        } else {            lateral_acceleration = -g.turn_max_g * GRAVITY_MSS;        }    }}

// set desired locationvoid Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd){    // record targets    _origin = rover.current_loc;    _destination = destination;    // initialise distance    _distance_to_destination = get_distance(_origin, _destination);    _reached_destination = false;    // set final desired speed    _desired_speed_final = 0.0f;    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {        // if not turning can continue at full speed        if (is_zero(next_leg_bearing_cd)) {            _desired_speed_final = _desired_speed;        } else {            // calculate maximum speed that keeps overshoot within bounds            const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);            const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));        }    }}

/*  Wrap AHRS yaw sensor if in reverse - centi-degress */float AP_L1_Control::get_yaw_sensor(){    if (_reverse) {        return wrap_180_cd(18000 + _ahrs.yaw_sensor);    }    return _ahrs.yaw_sensor;}

// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto modevoid Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle){    // get current yaw target    int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;    // get final angle, 1 = Relative, 0 = Absolute    if (relative_angle == 0) {        // absolute angle        yaw_look_at_heading = wrap_360_cd(angle_deg * 100);    } else {        // relative angle        if (direction < 0) {            angle_deg = -angle_deg;        }        yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));    }    // get turn speed    if (is_zero(turn_rate_dps)) {        // default to regular auto slew rate        yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;    }else{        int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;        yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360);    // deg / sec    }    // set yaw mode    set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);    // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction.  1 = clockwise, -1 = counterclockwise}

// return true if we should use the compass for yaw correctionbool AP_AHRS_DCM::use_compass(void){    if (!_compass || !_compass->use_for_yaw()) {        // no compass available        return false;    }    if (!_flags.fly_forward || !have_gps()) {        // we don't have any alterative to the compass        return true;    }    if (_gps.ground_speed() < GPS_SPEED_MIN) {        // we are not going fast enough to use the GPS        return true;    }    // if the current yaw differs from the GPS yaw by more than 45    // degrees and the estimated wind speed is less than 80% of the    // ground speed, then switch to GPS navigation. This will help    // prevent flyaways with very bad compass offsets    int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));    if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {        if (AP_HAL::millis() - _last_consistent_heading > 2000) {            // start using the GPS for heading if the compass has been            // inconsistent with the GPS for 2 seconds            return false;        }    } else {        _last_consistent_heading = AP_HAL::millis();    }    // use the compass    return true;}

// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothingvoid AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw){    if (_inverted_flight) {        euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);    }    AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);}

// calculate steering output to drive along line from origin to destination waypoint// relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updatedvoid AR_WPNav::update_steering(const Location& current_loc, float current_speed){    // calculate yaw error for determining if vehicle should pivot towards waypoint    float yaw_cd = _reversed ? wrap_360_cd(_wp_bearing_cd + 18000) : _wp_bearing_cd;    float yaw_error_cd = wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor);    // calculate desired turn rate and update desired heading    if (use_pivot_steering(yaw_error_cd)) {        _cross_track_error = 0.0f;        _desired_heading_cd = yaw_cd;        _desired_lat_accel = 0.0f;        _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(yaw_cd * 0.01f), radians(_pivot_rate));    } else {        // run L1 controller        _nav_controller.set_reverse(_reversed);        _nav_controller.update_waypoint((_reached_destination ? current_loc : _origin), _destination, _radius);        // retrieve lateral acceleration, heading back towards line and crosstrack error        _desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss);        _desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd());        if (_reversed) {            _desired_lat_accel *= -1.0f;            _desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000);        }        _cross_track_error = _nav_controller.crosstrack_error();        _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed);    }}

// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller//      called by rtl_run at 100hz or morevoid Copter::rtl_loiterathome_run(){    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        // reset attitude control targets        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        // To-Do: re-initialise wpnav targets        return;    }    // process pilot's yaw input    float target_yaw_rate = 0;    if (!failsafe.radio) {        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());        if (!is_zero(target_yaw_rate)) {            set_auto_yaw_mode(AUTO_YAW_HOLD);        }    }    // set motors to full range    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // run waypoint controller    failsafe_terrain_set_status(wp_nav.update_wpnav());    // call z-axis position controller (wpnav should have already updated it's alt target)    pos_control.update_z_controller();    // call attitude controller    if (auto_yaw_mode == AUTO_YAW_HOLD) {        // roll & pitch from waypoint controller, yaw rate from pilot        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());    }else{        // roll, pitch from waypoint controller, yaw heading from auto_heading()        attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());    }    // check if we've completed this stage of RTL    if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {        if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {            // check if heading is within 2 degrees of heading when vehicle was armed            if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {                rtl_state_complete = true;            }        } else {            // we have loitered long enough            rtl_state_complete = true;        }    }}

/*  verify a LOITER_TO_ALT command. This involves checking we have  reached both the desired altitude and desired heading. The desired  altitude only needs to be reached once. */bool Plane::verify_loiter_to_alt() {    update_loiter();    //has target altitude been reached?    if (condition_value != 0) {        if (labs(condition_value - current_loc.alt) < 500) {            //Only have to reach the altitude once -- that's why I need            //this global condition variable.            //            //This is in case of altitude oscillation when still trying            //to reach the target heading.            condition_value = 0;        } else {            return false;        }    }        //has target heading been reached?    if (condition_value2 != 0) {        //Get the lat/lon of next Nav waypoint after this one:        AP_Mission::Mission_Command next_nav_cmd;        if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,                                       next_nav_cmd)) {            //no next waypoint to shoot for -- go ahead and break out of loiter            return true;                }         // Bearing in radians        int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);        // get current heading. We should probably be using the ground        // course instead to improve the accuracy in wind        int32_t heading_cd = ahrs.yaw_sensor;        int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);         /*          Check to see if the the plane is heading toward the land          waypoint          We use 10 degrees of slop so that we can handle 100          degrees/second of yaw        */        if (abs(heading_err_cd) <= 1000) {            //Want to head in a straight line from _here_ to the next waypoint.            //DON'T want to head in a line from the center of the loiter to             //the next waypoint.            //Therefore: mark the "last" (next_wp_loc is about to be updated)            //wp lat/lon as the current location.            next_WP_loc = current_loc;            return true;        } else {            return false;        }    }     return true;}

void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed){    // handle initialisation    if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) {        _guided_mode = ModeGuided::Guided_HeadingAndSpeed;        _desired_yaw_cd = ahrs.yaw_sensor;    }    set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed);}

/*    work out if we are going to use pivot steering*/bool Rover::use_pivot_steering(void) {    if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {        const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;        if (abs(bearing_error) > g.pivot_turn_angle) {            return true;        }    }    return false;}

// calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint// relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members// have been updated: _wp_bearing_cd, _cross_track_error, _distance_to_destinationvoid AR_WPNav::update_desired_speed(float dt){    // reduce speed to zero during pivot turns    if (_pivot_active) {        // decelerate to zero        _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);        return;    }    // accelerate desired speed towards max    float des_speed_lim = _atc.get_desired_speed_accel_limited(_reversed ? -_desired_speed : _desired_speed, dt);    // reduce speed to limit overshoot from line between origin and destination    // calculate number of degrees vehicle must turn to face waypoint    float ahrs_yaw_sensor = AP::ahrs().yaw_sensor;    const float heading_cd = is_negative(des_speed_lim) ? wrap_180_cd(ahrs_yaw_sensor + 18000) : ahrs_yaw_sensor;    const float wp_yaw_diff_cd = wrap_180_cd(_wp_bearing_cd - heading_cd);    const float turn_angle_rad = fabsf(radians(wp_yaw_diff_cd * 0.01f));    // calculate distance from vehicle to line + wp_overshoot    const float line_yaw_diff = wrap_180_cd(_origin.get_bearing_to(_destination) - heading_cd);    const float dist_from_line = fabsf(_cross_track_error);    const bool heading_away = is_positive(line_yaw_diff) == is_positive(_cross_track_error);    const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;    // calculate radius of circle that touches vehicle's current position and heading and target position and heading    float radius_m = 999.0f;    const float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));    if (!is_zero(radius_calc_denom)) {        radius_m = MAX(0.0f, _overshoot + wp_overshoot_adj) / radius_calc_denom;    }    // calculate and limit speed to allow vehicle to stay on circle    const float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m));    des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max);    // limit speed based on distance to waypoint and max acceleration/deceleration    if (is_positive(_distance_to_destination) && is_positive(_atc.get_decel_max())) {        const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final));        des_speed_lim = constrain_float(des_speed_lim, -dist_speed_max, dist_speed_max);    }    _desired_speed_limited = des_speed_lim;}

bool Plane::verify_loiter_heading(bool init){    if (quadplane.in_vtol_auto()) {        // skip heading verify if in VTOL auto        return true;    }    //Get the lat/lon of next Nav waypoint after this one:    AP_Mission::Mission_Command next_nav_cmd;    if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,                                   next_nav_cmd)) {        //no next waypoint to shoot for -- go ahead and break out of loiter        return true;    }    if (get_distance(next_WP_loc, next_nav_cmd.content.location) < abs(aparm.loiter_radius)) {        /* Whenever next waypoint is within the loiter radius,           maintaining loiter would prevent us from ever pointing toward the next waypoint.           Hence break out of loiter immediately         */        return true;    }    // Bearing in degrees    int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);    // get current heading.    int32_t heading_cd = gps.ground_course_cd();    int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);    if (init) {        loiter.total_cd =  wrap_360_cd(bearing_cd - heading_cd);        loiter.sum_cd = 0;    }    /*      Check to see if the the plane is heading toward the land      waypoint. We use 20 degrees (+/-10 deg) of margin so that      we can handle 200 degrees/second of yaw. Allow turn count      to stop it too to ensure we don't loop around forever in      case high winds are forcing us beyond 200 deg/sec at this      particular moment.    */    if (labs(heading_err_cd) <= 1000  ||            loiter.sum_cd > loiter.total_cd) {        // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp        // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location        if (next_WP_loc.flags.loiter_xtrack) {            next_WP_loc = current_loc;        }        return true;    }    return false;}

// verify_yaw - return true if we have reached the desired headingbool Sub::verify_yaw(){    // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)    if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {        set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);    }    // check if we are within 2 degrees of the target heading    return (fabsf(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200);}

void ModeLoiter::update(){    // get distance (in meters) to destination    _distance_to_destination = get_distance(rover.current_loc, _destination);    // if within waypoint radius slew desired speed towards zero and use existing desired heading    if (_distance_to_destination <= g.waypoint_radius) {        if (is_negative(_desired_speed)) {            _desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);        } else {            _desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);        }        _yaw_error_cd = 0.0f;    } else {        // P controller with hard-coded gain to convert distance to desired speed        // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation        _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);        // calculate bearing to destination        _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);        _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);        // if destination is behind vehicle, reverse towards it        if (fabsf(_yaw_error_cd) > 9000) {            _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);            _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);            _desired_speed = -_desired_speed;        }        // reduce desired speed if yaw_error is large        // note: copied from calc_reduced_speed_for_turn_or_distance        float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);        _desired_speed *= yaw_error_ratio;    }    // run steering and throttle controllers    calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);    calc_throttle(_desired_speed, false, true);    // mark us as in_reverse when using a negative throttle    // To-Do: only in reverse if vehicle is actually travelling backwards?    rover.set_reverse(_desired_speed < 0);}