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

// 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);    }}

// 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}

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 Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd){    // select heading method. Either mission, gps bearing projection or yaw based    // If prev_WP_loc and next_WP_loc are different then an accurate wp based bearing can    // be computed. However, if we had just changed modes before this, such as an aborted landing    // via mode change, the prev and next wps are the same.    float bearing;    if (!locations_are_same(prev_WP_loc, next_WP_loc)) {        // use waypoint based bearing, this is the usual case        steer_state.hold_course_cd = -1;    } else if (ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {        // use gps ground course based bearing hold        steer_state.hold_course_cd = -1;        bearing = ahrs.get_gps().ground_course_cd() * 0.01f;        location_update(next_WP_loc, bearing, 1000); // push it out 1km    } else {        // use yaw based bearing hold        steer_state.hold_course_cd = wrap_360_cd(ahrs.yaw_sensor);        bearing = ahrs.yaw_sensor * 0.01f;        location_update(next_WP_loc, bearing, 1000); // push it out 1km    }    next_WP_loc.alt = cmd.content.location.alt + home.alt;    condition_value = cmd.p1;    reset_offset_altitude();}

/*  copy current data to CHEK message */void Replay::log_check_generate(void){    Vector3f euler;    Vector3f velocity;    Location loc {};    _vehicle.EKF.getEulerAngles(euler);    _vehicle.EKF.getVelNED(velocity);    _vehicle.EKF.getLLH(loc);    struct log_Chek packet = {        LOG_PACKET_HEADER_INIT(LOG_CHEK_MSG),        time_us : AP_HAL::micros64(),        roll    : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)        pitch   : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)        yaw     : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)        lat     : loc.lat,        lng     : loc.lng,        alt     : loc.alt*0.01f,        vnorth  : velocity.x,        veast   : velocity.y,        vdown   : velocity.z    };    _vehicle.dataflash.WriteBlock(&packet, sizeof(packet));}

// 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) {        desired_heading = wrap_360_cd(desired_heading + 18000);        desired_lat_accel *= -1.0f;    }    _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);    if (rover.sailboat_use_indirect_route(desired_heading)) {        // sailboats use heading controller when tacking upwind        desired_heading = rover.sailboat_calc_heading(desired_heading);        calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);    } else if (rover.use_pivot_steering(_yaw_error_cd)) {        // for pivot turns use heading controller        calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);    } else {        // call lateral acceleration to steering controller        calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);    }}

/* report SITL state to DataFlash */void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash){    float yaw;    // convert to same conventions as DCM    yaw = state.yawDeg;    if (yaw > 180) {        yaw -= 360;    }    struct log_AHRS pkt = {        LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),        time_us : AP_HAL::micros64(),        roll    : (int16_t)(state.rollDeg*100),        pitch   : (int16_t)(state.pitchDeg*100),        yaw     : (uint16_t)(wrap_360_cd(yaw*100)),        alt     : (float)state.altitude,        lat     : (int32_t)(state.latitude*1.0e7),        lng     : (int32_t)(state.longitude*1.0e7),        q1      : state.quaternion.q1,        q2      : state.quaternion.q2,        q3      : state.quaternion.q3,        q4      : state.quaternion.q4,    };    DataFlash->WriteBlock(&pkt, sizeof(pkt));}

/*  set HIL (hardware in the loop) status for a GPS instance */void AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,                const Location &_location, const Vector3f &_velocity, uint8_t _num_sats,                uint16_t hdop, bool _have_vertical_velocity){    if (instance >= GPS_MAX_INSTANCES) {        return;    }    uint32_t tnow = AP_HAL::millis();    GPS_State &istate = state[instance];    istate.status = _status;    istate.location = _location;    istate.location.options = 0;    istate.velocity = _velocity;    istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);    istate.ground_course_cd = wrap_360_cd(degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL);    istate.hdop = hdop;    istate.num_sats = _num_sats;    istate.have_vertical_velocity |= _have_vertical_velocity;    istate.last_gps_time_ms = tnow;    uint64_t gps_time_ms = time_epoch_ms - (17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL);    istate.time_week     = gps_time_ms / (86400*7*(uint64_t)1000);    istate.time_week_ms  = gps_time_ms - istate.time_week*(86400*7*(uint64_t)1000);    timing[instance].last_message_time_ms = tnow;    timing[instance].last_fix_time_ms = tnow;    _type[instance].set(GPS_TYPE_HIL);}

/* report SITL state to DataFlash */void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash){    double p, q, r;    float yaw;    // we want the gyro values to be directly comparable to the    // raw_imu message, which is in body frame    convert_body_frame(state.rollDeg, state.pitchDeg,                       state.rollRate, state.pitchRate, state.yawRate,                       &p, &q, &r);    // convert to same conventions as DCM    yaw = state.yawDeg;    if (yaw > 180) {        yaw -= 360;    }    struct log_AHRS pkt = {        LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),        time_ms : hal.scheduler->millis(),        roll    : (int16_t)(state.rollDeg*100),        pitch   : (int16_t)(state.pitchDeg*100),        yaw     : (uint16_t)(wrap_360_cd(yaw*100)),        alt     : (float)state.altitude,        lat     : (int32_t)(state.latitude*1.0e7),        lng     : (int32_t)(state.longitude*1.0e7)    };    DataFlash.WriteBlock(&pkt, sizeof(pkt));}

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;}

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 AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t yaw){    float v_length = v.length();    char arrow = SYM_ARROW_START;    if (v_length > 1.0f) {        int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - yaw);        int32_t interval = 36000 / SYM_ARROW_COUNT;        arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;    }    backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, v_length), u_icon(SPEED));}

// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate requestvoid AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error, float overshoot_max){    // calculate angle error with maximum of +- max angle overshoot    angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);    angle_ef_error.z  = constrain_float(angle_ef_error.z, -overshoot_max, overshoot_max);    // update yaw angle target to be within max angle overshoot of our current heading    _angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor;    // increment the yaw angle target    _angle_ef_target.z += yaw_rate_ef * _dt;    _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z);}

// Write an attitude packetvoid Tracker::Log_Write_Attitude(){    Vector3f targets;    targets.y = nav_status.pitch * 100.0f;    targets.z = wrap_360_cd(nav_status.bearing * 100.0f);    DataFlash.Log_Write_Attitude(ahrs, targets);    DataFlash.Log_Write_EKF(ahrs,false);    DataFlash.Log_Write_AHRS2(ahrs);#if CONFIG_HAL_BOARD == HAL_BOARD_SITL    sitl.Log_Write_SIMSTATE(&DataFlash);#endif    DataFlash.Log_Write_POS(ahrs);}

// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate requestvoid AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error){    // calculate angle error with maximum of +- max angle overshoot    angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);    angle_ef_error.z  = constrain_float(angle_ef_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);    // update yaw angle target to be within max angle overshoot of our current heading    _angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor;    // increment the yaw angle target    _angle_ef_target.z += yaw_rate_ef * _dt;    _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z);}

// decode pilot's input and return heading_out (in cd) and speed_out (in m/s)void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out){    // get steering and throttle in the -1 to +1 range    const float desired_steering = constrain_float(rover.channel_steer->norm_input_dz(), -1.0f, 1.0f);    const float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f);    // calculate angle of input stick vector    heading_out = wrap_360_cd(atan2f(desired_steering, desired_throttle) * DEGX100);    // calculate throttle using magnitude of input stick vector    const float throttle = MIN(safe_sqrt(sq(desired_throttle) + sq(desired_steering)), 1.0f);    speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);}

void Copter::init_simple_bearing(){    // capture current cos_yaw and sin_yaw values    simple_cos_yaw = ahrs.cos_yaw();    simple_sin_yaw = ahrs.sin_yaw();    // initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading    super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);    super_simple_cos_yaw = simple_cos_yaw;    super_simple_sin_yaw = simple_sin_yaw;    // log the simple bearing to dataflash    if (should_log(MASK_LOG_ANY)) {        Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor);    }}

// get_auto_heading - returns target heading depending upon auto_yaw_mode// 100hz update ratefloat Sub::get_auto_heading(void){    switch (auto_yaw_mode) {    case AUTO_YAW_ROI:        // point towards a location held in roi_WP        return get_roi_yaw();        break;    case AUTO_YAW_LOOK_AT_HEADING:        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed        return yaw_look_at_heading;        break;    case AUTO_YAW_LOOK_AHEAD:        // Commanded Yaw to automatically look ahead.        return get_look_ahead_yaw();        break;    case AUTO_YAW_RESETTOARMEDYAW:        // changes yaw to be same as when quad was armed        return initial_armed_bearing;        break;    case AUTO_YAW_CORRECT_XTRACK: {        // TODO return current yaw if not in appropriate mode        // Bearing of current track (centidegrees)        float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());        // Bearing from current position towards intermediate position target (centidegrees)        float desired_angle = wp_nav.get_loiter_bearing_to_target();        float angle_error = wrap_180_cd(desired_angle - track_bearing);        float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);        return wrap_360_cd(track_bearing + angle_limited);    }    break;    case AUTO_YAW_LOOK_AT_NEXT_WP:    default:        // point towards next waypoint.        // we don't use wp_bearing because we don't want the vehicle to turn too much during flight        return wp_nav.get_yaw();        break;    }}

// Write an attitude packetvoid Plane::Log_Write_Attitude(void){    Vector3f targets;       // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude    targets.x = nav_roll_cd;    targets.y = nav_pitch_cd;    if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {        // when VTOL active log the copter target yaw        targets.z = wrap_360_cd(quadplane.attitude_control->get_att_target_euler_cd().z);    } else {        //Plane does not have the concept of navyaw. This is a placeholder.        targets.z = 0;    }        if (quadplane.tailsitter_active()) {        DataFlash.Log_Write_AttitudeView(*quadplane.ahrs_view, targets);    } else {        DataFlash.Log_Write_Attitude(ahrs, targets);    }    if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {        // log quadplane PIDs separately from fixed wing PIDs        DataFlash.Log_Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());        DataFlash.Log_Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());        DataFlash.Log_Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());        DataFlash.Log_Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() );    }    DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());    DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());    DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info());    DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());#if AP_AHRS_NAVEKF_AVAILABLE    DataFlash.Log_Write_EKF(ahrs);    DataFlash.Log_Write_AHRS2(ahrs);#endif#if CONFIG_HAL_BOARD == HAL_BOARD_SITL    sitl.Log_Write_SIMSTATE(&DataFlash);#endif    DataFlash.Log_Write_POS(ahrs);}

void ModeSimple::update(){    float desired_heading_cd, desired_speed;    // get pilot input    get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);    // rotate heading around based on initial heading    if (g2.simple_type == Simple_InitialHeading) {        desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd);    }    // if sticks in middle, use previous desired heading (important when vehicle is slowing down)    if (!is_positive(desired_speed)) {        desired_heading_cd = _desired_heading_cd;    } else {        // record desired heading for next iteration        _desired_heading_cd = desired_heading_cd;    }    // run throttle and steering controllers    calc_steering_to_heading(desired_heading_cd);    calc_throttle(desired_speed, true);}

TEST(MathWrapTest, Angle360){    // Full circle test    for (int32_t i = 0; i <= 36000; i += 100) {        if (i == 0) {            // hit pole position            EXPECT_EQ(i, wrap_360_cd(i));            EXPECT_EQ(i, wrap_360_cd(-i));        } else if (i < 36000) {            // between pole position            EXPECT_EQ(i, wrap_360_cd(i));            EXPECT_EQ(36000 - i, wrap_360_cd(-i));        } else if (i == 36000) {            // hit pole position            EXPECT_EQ(0, wrap_360_cd(i));            EXPECT_EQ(0, wrap_360_cd(-i));        }    }    EXPECT_EQ(4500.f,  wrap_360_cd(4500.f));    EXPECT_EQ(9000.f,  wrap_360_cd(9000.f));    EXPECT_EQ(18000.f, wrap_360_cd(18000.f));    EXPECT_EQ(27000.f, wrap_360_cd(27000.f));    EXPECT_EQ(0.f,     wrap_360_cd(36000.f));    EXPECT_EQ(0.f,     wrap_360_cd(72000.f));    EXPECT_EQ(0.f,     wrap_360_cd(360000.f));    EXPECT_EQ(0.f,     wrap_360_cd(720000.f));    EXPECT_EQ( 0.f,    wrap_360_cd(-3600000000.f));    EXPECT_EQ(31500.f, wrap_360_cd(-4500.f));    EXPECT_EQ(27000.f, wrap_360_cd(-9000.f));    EXPECT_EQ(18000.f, wrap_360_cd(-18000.f));    EXPECT_EQ(9000.f,  wrap_360_cd(-27000.f));    EXPECT_EQ(0.f,     wrap_360_cd(-36000.f));    EXPECT_EQ(0.f,     wrap_360_cd(-72000.f));}

boolAP_GPS_ERB::_parse_gps(void){    switch (_msg_id) {    case MSG_VER:        Debug("Version of ERB protocol %u.%u.%u",              _buffer.ver.ver_high,              _buffer.ver.ver_medium,              _buffer.ver.ver_low);        break;    case MSG_POS:        Debug("Message POS");        _last_pos_time        = _buffer.pos.time;        state.location.lng    = (int32_t)(_buffer.pos.longitude * 1e7);        state.location.lat    = (int32_t)(_buffer.pos.latitude * 1e7);        state.location.alt    = (int32_t)(_buffer.pos.altitude_msl * 1e2);        state.status          = next_fix;        _new_position = true;        state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f;        state.vertical_accuracy = _buffer.pos.vertical_accuracy * 1.0e-3f;        state.have_horizontal_accuracy = true;        state.have_vertical_accuracy = true;        break;    case MSG_STAT:        Debug("Message STAT fix_status=%u fix_type=%u",              _buffer.stat.fix_status,              _buffer.stat.fix_type);        if (_buffer.stat.fix_status & STAT_FIX_VALID) {            if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FIX) {                next_fix = AP_GPS::GPS_OK_FIX_3D_RTK;            } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FLOAT) {                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;            } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_SINGLE) {                next_fix = AP_GPS::GPS_OK_FIX_3D;            } else {                next_fix = AP_GPS::NO_FIX;                state.status = AP_GPS::NO_FIX;            }        } else {            next_fix = AP_GPS::NO_FIX;            state.status = AP_GPS::NO_FIX;        }        state.num_sats = _buffer.stat.satellites;        if (next_fix >= AP_GPS::GPS_OK_FIX_3D) {            state.last_gps_time_ms = AP_HAL::millis();            state.time_week_ms    = _buffer.stat.time;            state.time_week       = _buffer.stat.week;        }        break;    case MSG_DOPS:        Debug("Message DOPS");        state.hdop = _buffer.dops.hDOP;        state.vdop = _buffer.dops.vDOP;        break;    case MSG_VEL:        Debug("Message VEL");        _last_vel_time         = _buffer.vel.time;        state.ground_speed     = _buffer.vel.speed_2d * 0.01f;        // m/s        // Heading 2D deg * 100000 rescaled to deg * 100        state.ground_course_cd = wrap_360_cd(_buffer.vel.heading_2d / 1000);        state.have_vertical_velocity = true;        state.velocity.x = _buffer.vel.vel_north * 0.01f;        state.velocity.y = _buffer.vel.vel_east * 0.01f;        state.velocity.z = _buffer.vel.vel_down * 0.01f;        state.have_speed_accuracy = true;        state.speed_accuracy = _buffer.vel.speed_accuracy * 0.01f;        _new_speed = true;        break;    default:        Debug("Unexpected message 0x%02x", (unsigned)_msg_id);        return false;    }    // we only return true when we get new position and speed data    // this ensures we don't use stale data    if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {        _new_speed = _new_position = false;        _fix_count++;        return true;    }    return false;}

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) {            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 arbitary            // 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(PSTR("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(PSTR("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;#if GEOFENCE_ENABLED == ENABLED        if (g.fence_autoenable > 0) {            if (! geofence_set_enabled(true, AUTO_TOGGLED)) {                gcs_send_text_P(SEVERITY_HIGH, PSTR("Enable fence failed (cannot autoenable"));            } else {                gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence enabled. (autoenabled)"));            }        }#endif        // 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;    }}

int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv){	if (!g.compass_enabled) {        cliSerial->print("Compass: ");		print_enabled(false);		return (0);    }    if (!compass.init()) {        cliSerial->println("Compass initialisation failed!");        return 0;    }    ahrs.init();    ahrs.set_fly_forward(true);    ahrs.set_compass(&compass);    // we need the AHRS initialised for this test    ins.init(scheduler.get_loop_rate_hz());    ahrs.reset();	int counter = 0;    float heading = 0;    print_hit_enter();    uint8_t medium_loopCounter = 0;    while(1) {        ins.wait_for_sample();        ahrs.update();        medium_loopCounter++;        if(medium_loopCounter >= 5){            if (compass.read()) {                // Calculate heading                Matrix3f m = ahrs.get_rotation_body_to_ned();                heading = compass.calculate_heading(m);                compass.learn_offsets();            }            medium_loopCounter = 0;        }                counter++;        if (counter>20) {            if (compass.healthy()) {                const Vector3f mag_ofs = compass.get_offsets();                const Vector3f mag = compass.get_field();                cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,/tXYZoff: %6.2f, %6.2f, %6.2f/n",                                    (double)(wrap_360_cd(ToDeg(heading) * 100)) /100,                                    (double)mag.x, (double)mag.y, (double)mag.z,                                    (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);            } else {                cliSerial->println("compass not healthy");            }            counter=0;        }        if (cliSerial->available() > 0) {            break;        }    }    // save offsets. This allows you to get sane offset values using    // the CLI before you go flying.        cliSerial->println("saving offsets");    compass.save_offsets();    return (0);}

int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv){    if (!g.compass_enabled) {        cliSerial->printf_P(PSTR("Compass: "));        print_enabled(false);        return (0);    }    if (!compass.init()) {        cliSerial->println_P(PSTR("Compass initialisation failed!"));        return 0;    }    ahrs.init();    ahrs.set_fly_forward(true);    ahrs.set_wind_estimation(true);    ahrs.set_compass(&compass);    // we need the AHRS initialised for this test    ins.init(AP_InertialSensor::COLD_START,              ins_sample_rate);    ahrs.reset();    uint16_t counter = 0;    float heading = 0;    print_hit_enter();    while(1) {        hal.scheduler->delay(20);        if (micros() - fast_loopTimer_us > 19000UL) {            fast_loopTimer_us       = micros();            // INS            // ---            ahrs.update();            if(counter % 5 == 0) {                if (compass.read()) {                    // Calculate heading                    const Matrix3f &m = ahrs.get_dcm_matrix();                    heading = compass.calculate_heading(m);                    compass.learn_offsets();                }            }            counter++;            if (counter>20) {                if (compass.healthy()) {                    const Vector3f &mag_ofs = compass.get_offsets_milligauss();                    const Vector3f &mag = compass.get_field_milligauss();                    cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,/tXYZoff: %6.2f, %6.2f, %6.2f/n"),                                        (wrap_360_cd(ToDeg(heading) * 100)) /100,                                        (double)mag.x, (double)mag.y, (double)mag.z,                                        (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);                } else {                    cliSerial->println_P(PSTR("compass not healthy"));                }                counter=0;            }        }        if (cliSerial->available() > 0) {            break;        }    }    // save offsets. This allows you to get sane offset values using    // the CLI before you go flying.    cliSerial->println_P(PSTR("saving offsets"));    compass.save_offsets();    return (0);}

//.........这里部分代码省略.........        }else{            next_fix = AP_GPS::NO_FIX;            state.status = AP_GPS::NO_FIX;        }        if(noReceivedHdop) {            state.hdop = _buffer.solution.position_DOP;        }        state.num_sats    = _buffer.solution.satellites;        if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {            state.last_gps_time_ms = AP_HAL::millis();            if (state.time_week == _buffer.solution.week &&                state.time_week_ms + 200 == _buffer.solution.time) {                // we got a 5Hz update. This relies on the way                // that uBlox gives timestamps that are always                // multiples of 200 for 5Hz                _last_5hz_time = state.last_gps_time_ms;            }            state.time_week_ms    = _buffer.solution.time;            state.time_week       = _buffer.solution.week;        }#if UBLOX_FAKE_3DLOCK        next_fix = state.status;        state.num_sats = 10;        state.time_week = 1721;        state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;        state.last_gps_time_ms = AP_HAL::millis();        state.hdop = 130;#endif        break;    case MSG_VELNED:        Debug("MSG_VELNED");        _last_vel_time         = _buffer.velned.time;        state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s        state.ground_course_cd = wrap_360_cd(_buffer.velned.heading_2d / 1000);       // Heading 2D deg * 100000 rescaled to deg * 100        state.have_vertical_velocity = true;        state.velocity.x = _buffer.velned.ned_north * 0.01f;        state.velocity.y = _buffer.velned.ned_east * 0.01f;        state.velocity.z = _buffer.velned.ned_down * 0.01f;        state.have_speed_accuracy = true;        state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;#if UBLOX_FAKE_3DLOCK        state.speed_accuracy = 0;#endif        _new_speed = true;        break;#if UBLOX_VERSION_AUTODETECTION    case MSG_NAV_SVINFO:        {        Debug("MSG_NAV_SVINFO/n");        static const uint8_t HardwareGenerationMask = 0x07;        uint8_t hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;        switch (hardware_generation) {            case UBLOX_5:            case UBLOX_6:                /*speed already configured */;                break;            case UBLOX_7:            case UBLOX_M8:                port->begin(4000000U);                Debug("Changed speed to 5Mhzfor SPI-driven UBlox/n");                break;            default:                hal.console->printf("Wrong Ublox' Hardware Version%u/n", hardware_generation);                break;        };        /* We don't need that anymore */

//.........这里部分代码省略.........                    inav_pos.z,                    ekf_relpos.x,                    ekf_relpos.y,                    -ekf_relpos.z);            fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f/n",                    AP_HAL::millis() * 0.001f,                    degrees(ekf_euler.x),                    degrees(ekf_euler.y),                    temp,                    velNED.x,                     velNED.y,                     velNED.z,                     posNED.x,                     posNED.y,                     posNED.z,                     60*degrees(gyroBias.x),                     60*degrees(gyroBias.y),                     60*degrees(gyroBias.z),                     windVel.x,                     windVel.y,                     magNED.x,                     magNED.y,                     magNED.z,                     magXYZ.x,                     magXYZ.y,                     magXYZ.z,                    logreader.get_attitude().x,                    logreader.get_attitude().y,                    logreader.get_attitude().z);            // define messages for EKF1 data packet            int16_t     roll  = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg)            int16_t     pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg)            uint16_t    yaw   = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg)            float       velN  = (float)(velNED.x); // velocity North (m/s)            float       velE  = (float)(velNED.y); // velocity East (m/s)            float       velD  = (float)(velNED.z); // velocity Down (m/s)            float       posN  = (float)(posNED.x); // metres North            float       posE  = (float)(posNED.y); // metres East            float       posD  = (float)(posNED.z); // metres Down            float       gyrX  = (float)(6000*degrees(gyroBias.x)); // centi-deg/min            float       gyrY  = (float)(6000*degrees(gyroBias.y)); // centi-deg/min            float       gyrZ  = (float)(6000*degrees(gyroBias.z)); // centi-deg/min            // print EKF1 data packet            fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f/n",                    AP_HAL::millis() * 0.001f,                    AP_HAL::millis(),                    roll,                     pitch,                     yaw,                     velN,                     velE,                     velD,                     posN,                     posE,                     posD,                     gyrX,                    gyrY,                    gyrZ);            // define messages for EKF2 data packet            int8_t  accWeight  = (int8_t)(100*accelWeighting);            int8_t  acc1  = (int8_t)(100*accelZBias1);            int8_t  acc2  = (int8_t)(100*accelZBias2);            int16_t windN = (int16_t)(100*windVel.x);

//.........这里部分代码省略.........        heartbeat.custom_mode = 0;        /*          save and restore sequence number for chan0, as it is used by          generated encode functions         */        mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);        uint8_t saved_seq = chan0_status->current_tx_seq;        chan0_status->current_tx_seq = mavlink.seq;        len = mavlink_msg_heartbeat_encode(vehicle_system_id,                                           vehicle_component_id,                                           &msg, &heartbeat);        chan0_status->current_tx_seq = saved_seq;        mav_socket.send(&msg.magic, len);        last_heartbeat_ms = now;    }    /*      send a ADSB_VEHICLE messages     */    uint32_t now_us = AP_HAL::micros();    if (now_us - last_report_us >= reporting_period_ms*1000UL) {        for (uint8_t i=0; i<num_vehicles; i++) {            ADSB_Vehicle &vehicle = vehicles[i];            Location loc = home;            location_offset(loc, vehicle.position.x, vehicle.position.y);            // re-init when exceeding radius range            if (get_distance(home, loc) > _sitl->adsb_radius_m) {                vehicle.initialised = false;            }                        mavlink_adsb_vehicle_t adsb_vehicle {};            last_report_us = now_us;            adsb_vehicle.ICAO_address = vehicle.ICAO_address;            adsb_vehicle.lat = loc.lat;            adsb_vehicle.lon = loc.lng;            adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;            adsb_vehicle.altitude = -vehicle.position.z * 1000;            adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x)));            adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100;            adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100;            memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign));            adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE;            adsb_vehicle.tslc = 1;            adsb_vehicle.flags =                ADSB_FLAGS_VALID_COORDS |                ADSB_FLAGS_VALID_ALTITUDE |                ADSB_FLAGS_VALID_HEADING |                ADSB_FLAGS_VALID_VELOCITY |                ADSB_FLAGS_VALID_CALLSIGN |                ADSB_FLAGS_SIMULATED;            adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set            mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);            uint8_t saved_seq = chan0_status->current_tx_seq;            chan0_status->current_tx_seq = mavlink.seq;            len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id,                                                  MAV_COMP_ID_ADSB,                                                  &msg, &adsb_vehicle);            chan0_status->current_tx_seq = saved_seq;                        uint8_t msgbuf[len];            len = mavlink_msg_to_send_buffer(msgbuf, &msg);            if (len > 0) {                mav_socket.send(msgbuf, len);            }        }    }        // ADSB_transceiever is enabled, send the status report.    if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) {        last_tx_report_ms = now;        mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);        uint8_t saved_seq = chan0_status->current_tx_seq;        uint8_t saved_flags = chan0_status->flags;        chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;        chan0_status->current_tx_seq = mavlink.seq;        const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK};        len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id,                                              MAV_COMP_ID_ADSB,                                              &msg, &health_report);        chan0_status->current_tx_seq = saved_seq;        chan0_status->flags = saved_flags;        uint8_t msgbuf[len];        len = mavlink_msg_to_send_buffer(msgbuf, &msg);        if (len > 0) {            mav_socket.send(msgbuf, len);            ::printf("ADSBsim send tx health packet/n");        }    }}