示例#1
0
// 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 updated
void 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);
    }
}
示例#2
0
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
void 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
}
示例#3
0
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");
}
示例#4
0
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();
}
示例#5
0
/*
  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));
}
示例#6
0
// 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 value
void 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);
    }
}
示例#7
0
/* 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));
}
示例#8
0
/*
  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);
}
示例#9
0
/* 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));
}
示例#10
0
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;
}
示例#11
0
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;
    }
}
示例#12
0
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 request
void 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);
}
示例#14
0
文件: Log.cpp 项目: Flandoe/ardupilot
// Write an attitude packet
void 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);
}
示例#15
0
void AP_OSD_Screen::draw_waypoint(uint8_t x, uint8_t y)
{
    AP_AHRS &ahrs = AP::ahrs();
    int32_t angle = wrap_360_cd(osd->nav_info.wp_bearing - ahrs.yaw_sensor);
    int32_t interval = 36000 / SYM_ARROW_COUNT;
    if (osd->nav_info.wp_distance < 2.0f) {
        //avoid fast rotating arrow at small distances
        angle = 0;
    }
    char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
    backend->write(x,y, false, "%c%2u%c",SYM_WPNO, osd->nav_info.wp_number, arrow);
    draw_distance(x+4, y, osd->nav_info.wp_distance);
}
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
void 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);
}
示例#17
0
// 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);
}
示例#18
0
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);
    }
}
示例#19
0
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float 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;
    }
}
示例#20
0
void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
{
    AP_AHRS &ahrs = AP::ahrs();
    Location loc;
    if (ahrs.get_position(loc) && ahrs.home_is_set()) {
        const Location &home_loc = ahrs.get_home();
        float distance = home_loc.get_distance(loc);
        int32_t angle = wrap_360_cd(loc.get_bearing_to(home_loc) - ahrs.yaw_sensor);
        int32_t interval = 36000 / SYM_ARROW_COUNT;
        if (distance < 2.0f) {
            //avoid fast rotating arrow at small distances
            angle = 0;
        }
        char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
        backend->write(x, y, false, "%c%c", SYM_HOME, arrow);
        draw_distance(x+2, y, distance);
    } else {
        backend->write(x, y, true, "%c", SYM_HOME);
    }
}
示例#21
0
// Write an attitude packet
void 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);
}
示例#22
0
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);
}
示例#23
0
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));
}
示例#24
0
bool
AP_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;
}
示例#25
0
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;
    }
}
示例#26
0
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
{
	if (!g.compass_enabled) {
        cliSerial->print("Compass: "******"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);
}
示例#27
0
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
{
    if (!g.compass_enabled) {
        cliSerial->printf_P(PSTR("Compass: "******"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);
}
bool
AP_GPS_UBLOX::_parse_gps(void)
{
    if (_class == CLASS_ACK) {
        Debug("ACK %u", (unsigned)_msg_id);

        if(_msg_id == MSG_ACK_ACK && _buffer.ack.clsID == CLASS_CFG && _buffer.ack.msgID == MSG_CFG_CFG) {
            _cfg_saved = true;
        }
        return false;
    }

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
		Debug("Got settings %u min_elev %d drLimit %u\n", 
              (unsigned)_buffer.nav_settings.dynModel,
              (int)_buffer.nav_settings.minElev,
              (unsigned)_buffer.nav_settings.drLimit);
        _buffer.nav_settings.mask = 0;
        if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
            _buffer.nav_settings.dynModel != gps._navfilter) {
            // we've received the current nav settings, change the engine
            // settings and send them back
            Debug("Changing engine setting from %u to %u\n",
                  (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
            _buffer.nav_settings.dynModel = gps._navfilter;
            _buffer.nav_settings.mask |= 1;
        }
        if (gps._min_elevation != -100 &&
            _buffer.nav_settings.minElev != gps._min_elevation) {
            Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
            _buffer.nav_settings.minElev = gps._min_elevation;
            _buffer.nav_settings.mask |= 2;
        }
        if (_buffer.nav_settings.mask != 0) {
            _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                          &_buffer.nav_settings,
                          sizeof(_buffer.nav_settings));
            _cfg_saved = false;     //save configuration
        }
        return false;
    }

#if UBLOX_GNSS_SETTINGS
    if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
        uint8_t gnssCount = 0;
        Debug("Got GNSS Settings %u %u %u %u:\n",
            (unsigned)_buffer.gnss.msgVer,
            (unsigned)_buffer.gnss.numTrkChHw,
            (unsigned)_buffer.gnss.numTrkChUse,
            (unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUG
        for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
            Debug("  %u %u %u 0x%08x\n",
            (unsigned)_buffer.gnss.configBlock[i].gnssId,
            (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
            (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
            (unsigned)_buffer.gnss.configBlock[i].flags);
        }
#endif

        for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
            if((gps._gnss_mode & (1 << i)) && i != GNSS_SBAS) {
                gnssCount++;
            }
        }

        for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
            // Reserve an equal portion of channels for all enabled systems
            if(gps._gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
                if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
                    _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
                    _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
                } else {
                    _buffer.gnss.configBlock[i].resTrkCh = 1;
                    _buffer.gnss.configBlock[i].maxTrkCh = 3;
                }
                _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
            } else {
                _buffer.gnss.configBlock[i].resTrkCh = 0;
                _buffer.gnss.configBlock[i].maxTrkCh = 0;
                _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
            }
        }
        _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
        return false;
    }
#endif

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) {
		Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
              (unsigned)_buffer.sbas.mode,
              (unsigned)_buffer.sbas.usage,
              (unsigned)_buffer.sbas.maxSBAS,
              (unsigned)_buffer.sbas.scanmode2,
              (unsigned)_buffer.sbas.scanmode1);
        if (_buffer.sbas.mode != gps._sbas_mode) {
            _buffer.sbas.mode = gps._sbas_mode;
            _send_message(CLASS_CFG, MSG_CFG_SBAS,
                          &_buffer.sbas,
                          sizeof(_buffer.sbas));
            _cfg_saved = false;
        }
    }

#if UBLOX_HW_LOGGING
    if (_class == CLASS_MON) {
        if (_msg_id == MSG_MON_HW) {
            if (_payload_length == 60 || _payload_length == 68) {
                log_mon_hw();
            }
        } else if (_msg_id == MSG_MON_HW2) {
            if (_payload_length == 28) {
                log_mon_hw2();  
            }
        } else {
            unexpected_message();
        }
        return false;
    }
#endif // UBLOX_HW_LOGGING

#if UBLOX_RXM_RAW_LOGGING
    if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
        log_rxm_raw(_buffer.rxm_raw);
        return false;
    } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
        log_rxm_rawx(_buffer.rxm_rawx);
        return false;
    }
#endif // UBLOX_RXM_RAW_LOGGING

    if (_class != CLASS_NAV) {
        unexpected_message();
        return false;
    }

    switch (_msg_id) {
    case MSG_POSLLH:
        Debug("MSG_POSLLH next_fix=%u", next_fix);
        _last_pos_time        = _buffer.posllh.time;
        state.location.lng    = _buffer.posllh.longitude;
        state.location.lat    = _buffer.posllh.latitude;
        state.location.alt    = _buffer.posllh.altitude_msl / 10;
        state.status          = next_fix;
        _new_position = true;
        state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
        state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
        state.have_horizontal_accuracy = true;
        state.have_vertical_accuracy = true;
#if UBLOX_FAKE_3DLOCK
        state.location.lng = 1491652300L;
        state.location.lat = -353632610L;
        state.location.alt = 58400;
        state.vertical_accuracy = 0;
        state.horizontal_accuracy = 0;
#endif
        break;
    case MSG_STATUS:
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
              _buffer.status.fix_status,
              _buffer.status.fix_type);
        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
            if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
            }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }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;
        }
#if UBLOX_FAKE_3DLOCK
        state.status = AP_GPS::GPS_OK_FIX_3D;
        next_fix = state.status;
#endif
        break;
    case MSG_DOP:
        Debug("MSG_DOP");
        noReceivedHdop = false;
        state.hdop        = _buffer.dop.hDOP;
        state.vdop        = _buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
        state.hdop = 130;
        state.hdop = 170;
#endif
        break;
    case MSG_SOL:
        Debug("MSG_SOL fix_status=%u fix_type=%u",
              _buffer.solution.fix_status,
              _buffer.solution.fix_type);
        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
            if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
            }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }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;
        }
        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 */
        _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
        break;
        }
#endif
    default:
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
        }
        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++;
        if ((AP_HAL::millis() - _last_5hz_time) > 15000U && !need_rate_update) {
            // the GPS is running slow. It possibly browned out and
            // restarted with incorrect parameters. We will slowly
            // send out new parameters to fix it
            need_rate_update = true;
            rate_update_step = 0;
            _last_5hz_time = AP_HAL::millis();
        }

		if (_fix_count == 50 && gps._sbas_mode != 2) {
			// ask for SBAS settings every 20 seconds
			Debug("Asking for SBAS setting\n");
			_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
		}
		if (_fix_count == 100) {
			// ask for nav settings every 20 seconds
			Debug("Asking for engine setting\n");
			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
            _fix_count = 0;
		}
        return true;
    }
    return false;
}
示例#29
0
void Replay::loop()
{
    while (true) {
        char type[5];

        if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
            if (!hal.util->get_soft_armed()) {
                hal.util->set_soft_armed(true);
                ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis());
            }
        }

        if (!logreader.update(type)) {
            ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
            fclose(plotf);
            break;
        }
        read_sensors(type);

        if (streq(type,"ATT")) {
            Vector3f ekf_euler;
            Vector3f velNED;
            Vector3f posNED;
            Vector3f gyroBias;
            float accelWeighting;
            float accelZBias1;
            float accelZBias2;
            Vector3f windVel;
            Vector3f magNED;
            Vector3f magXYZ;
            Vector3f DCM_attitude;
            Vector3f ekf_relpos;
            Vector3f velInnov;
            Vector3f posInnov;
            Vector3f magInnov;
            float    tasInnov;
            float velVar;
            float posVar;
            float hgtVar;
            Vector3f magVar;
            float tasVar;
            Vector2f offset;
            uint16_t faultStatus;

            const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_rotation_body_to_ned();
            dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z);
            _vehicle.EKF.getEulerAngles(ekf_euler);
            _vehicle.EKF.getVelNED(velNED);
            _vehicle.EKF.getPosNED(posNED);
            _vehicle.EKF.getGyroBias(gyroBias);
            _vehicle.EKF.getIMU1Weighting(accelWeighting);
            _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2);
            _vehicle.EKF.getWind(windVel);
            _vehicle.EKF.getMagNED(magNED);
            _vehicle.EKF.getMagXYZ(magXYZ);
            _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov);
            _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
            _vehicle.EKF.getFilterFaults(faultStatus);
            _vehicle.EKF.getPosNED(ekf_relpos);
            Vector3f inav_pos = _vehicle.inertial_nav.get_position() * 0.01f;
            float temp = degrees(ekf_euler.z);

            if (temp < 0.0f) temp = temp + 360.0f;
            fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n",
                    AP_HAL::millis() * 0.001f,
                    logreader.get_sim_attitude().x,
                    logreader.get_sim_attitude().y,
                    logreader.get_sim_attitude().z,
                    _vehicle.barometer.get_altitude(),
                    logreader.get_attitude().x,
                    logreader.get_attitude().y,
                    wrap_180_cd(logreader.get_attitude().z*100)*0.01f,
                    logreader.get_inavpos().x,
                    logreader.get_inavpos().y,
                    logreader.get_ahr2_attitude().x,
                    logreader.get_ahr2_attitude().y,
                    wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f,
                    degrees(DCM_attitude.x),
                    degrees(DCM_attitude.y),
                    degrees(DCM_attitude.z),
                    degrees(ekf_euler.x),
                    degrees(ekf_euler.y),
                    degrees(ekf_euler.z),
                    inav_pos.x,
                    inav_pos.y,
                    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);
            int16_t windE = (int16_t)(100*windVel.y);
            int16_t magN  = (int16_t)(magNED.x);
            int16_t magE  = (int16_t)(magNED.y);
            int16_t magD  = (int16_t)(magNED.z);
            int16_t magX  = (int16_t)(magXYZ.x);
            int16_t magY  = (int16_t)(magXYZ.y);
            int16_t magZ  = (int16_t)(magXYZ.z);

            // print EKF2 data packet
            fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n",
                    AP_HAL::millis() * 0.001f,
                    AP_HAL::millis(),
                    accWeight, 
                    acc1, 
                    acc2, 
                    windN, 
                    windE, 
                    magN, 
                    magE, 
                    magD, 
                    magX,
                    magY,
                    magZ);

            // define messages for EKF3 data packet
            int16_t innovVN = (int16_t)(100*velInnov.x);
            int16_t innovVE = (int16_t)(100*velInnov.y);
            int16_t innovVD = (int16_t)(100*velInnov.z);
            int16_t innovPN = (int16_t)(100*posInnov.x);
            int16_t innovPE = (int16_t)(100*posInnov.y);
            int16_t innovPD = (int16_t)(100*posInnov.z);
            int16_t innovMX = (int16_t)(magInnov.x);
            int16_t innovMY = (int16_t)(magInnov.y);
            int16_t innovMZ = (int16_t)(magInnov.z);
            int16_t innovVT = (int16_t)(100*tasInnov);

            // print EKF3 data packet
            fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n",
                    AP_HAL::millis() * 0.001f,
                    AP_HAL::millis(),
                    innovVN, 
                    innovVE, 
                    innovVD, 
                    innovPN, 
                    innovPE, 
                    innovPD, 
                    innovMX, 
                    innovMY, 
                    innovMZ, 
                    innovVT);

            // define messages for EKF4 data packet
            int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX));
            int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX));
            int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX));
            int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX));
            int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX));
            int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX));
            int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX));
            int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX));
            int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX));

            // print EKF4 data packet
            fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n",
                    AP_HAL::millis() * 0.001f,
                    (unsigned)AP_HAL::millis(),
                    (int)sqrtvarV,
                    (int)sqrtvarP,
                    (int)sqrtvarH,
                    (int)sqrtvarMX, 
                    (int)sqrtvarMY, 
                    (int)sqrtvarMZ,
                    (int)sqrtvarVT,
                    (int)offsetNorth,
                    (int)offsetEast,
                    (int)faultStatus);
        }
    }

    flush_dataflash();

    if (check_solution) {
        report_checks();
    }
    exit(0);
}
示例#30
0
/*
  send a report to the vehicle control code over MAVLink
*/
void ADSB::send_report(void)
{
    if (AP_HAL::millis() < 10000) {
        // simulated aircraft don't appear until 10s after startup. This avoids a windows
        // threading issue with non-blocking sockets and the initial wait on uartA
        return;
    }
    if (!mavlink.connected && mav_socket.connect(target_address, target_port)) {
        ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port);
        mavlink.connected = true;
    }
    if (!mavlink.connected) {
        return;
    }

    // check for incoming MAVLink messages
    uint8_t buf[100];
    ssize_t ret;

    while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) {
        for (uint8_t i=0; i<ret; i++) {
            mavlink_message_t msg;
            mavlink_status_t status;
            if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
                                          buf[i],
                                          &msg, &status) == MAVLINK_FRAMING_OK) {
                switch (msg.msgid) {
                case MAVLINK_MSG_ID_HEARTBEAT: {
                    if (!seen_heartbeat) {
                        seen_heartbeat = true;
                        vehicle_component_id = msg.compid;
                        vehicle_system_id = msg.sysid;
                        ::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id);
                    }
                    break;
                }
                }
            }
        }
    }

    if (!seen_heartbeat) {
        return;
    }

    uint32_t now = AP_HAL::millis();
    mavlink_message_t msg;
    uint16_t len;

    if (now - last_heartbeat_ms >= 1000) {
        mavlink_heartbeat_t heartbeat;
        heartbeat.type = MAV_TYPE_ADSB;
        heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
        heartbeat.base_mode = 0;
        heartbeat.system_status = 0;
        heartbeat.mavlink_version = 0;
        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");
        }
    }

}