Exemplo n.º 1
0
static void test_one_offset(const struct Location &loc,
                            float ofs_north, float ofs_east,
                            float dist, float bearing)
{
    struct Location loc2;
    float dist2, bearing2;

    loc2 = loc;
    uint32_t t1 = AP_HAL::micros();
    location_offset(loc2, ofs_north, ofs_east);
    hal.console->printf("location_offset took %u usec\n",
                        (unsigned)(AP_HAL::micros() - t1));
    dist2 = get_distance(loc, loc2);
    bearing2 = get_bearing_cd(loc, loc2) * 0.01f;
    float brg_error = bearing2-bearing;
    if (brg_error > 180) {
        brg_error -= 360;
    } else if (brg_error < -180) {
        brg_error += 360;
    }

    if (fabsf(dist - dist2) > 1.0f ||
        brg_error > 1.0f) {
        hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n",
                      brg_error, dist-dist2);
    }
}
Exemplo n.º 2
0
/*
  start a VTOL landing
 */
bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
{
    if (!setup()) {
        return false;
    }
    motors->slow_start(true);
    pid_rate_roll.reset_I();
    pid_rate_pitch.reset_I();
    pid_rate_yaw.reset_I();
    pid_accel_z.reset_I();
    pi_vel_xy.reset_I();
    
    plane.set_next_WP(cmd.content.location);
    // initially aim for current altitude
    plane.next_WP_loc.alt = plane.current_loc.alt;
    land_state = QLAND_POSITION;
    throttle_wait = false;
    land_yaw_cd = get_bearing_cd(plane.prev_WP_loc, plane.next_WP_loc);
    land_wp_proportion = 0;
    motors_lower_limit_start_ms = 0;
    Location origin = inertial_nav.get_origin();
    Vector2f diff2d;
    Vector3f target;
    diff2d = location_diff(origin, plane.next_WP_loc);
    target.x = diff2d.x * 100;
    target.y = diff2d.y * 100;
    target.z = plane.next_WP_loc.alt - origin.alt;
    wp_nav->set_wp_origin_and_destination(inertial_nav.get_position(), target);
    pos_control->set_alt_target(inertial_nav.get_altitude());
    
    // also update nav_controller for status output
    plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
    return true;
}
Exemplo n.º 3
0
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
    // record targets
    _origin = rover.current_loc;
    _destination = destination;

    // initialise distance
    _distance_to_destination = get_distance(_origin, _destination);
    _reached_destination = false;

    // set final desired speed
    _desired_speed_final = 0.0f;
    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
        // if not turning can continue at full speed
        if (is_zero(next_leg_bearing_cd)) {
            _desired_speed_final = _desired_speed;
        } else {
            // calculate maximum speed that keeps overshoot within bounds
            const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
            const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
        }
    }
}
Exemplo n.º 4
0
bool Plane::verify_continue_and_change_alt()
{
    //climbing?
    if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) {
        return true;
    }
    //descending?
    else if (condition_value == 2 &&
             adjusted_altitude_cm() <= next_WP_loc.alt) {
        return true;
    }    
    //don't care if we're climbing or descending
    else if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
        return true;
    }
   
    // Is the next_WP less than 200 m away?
    if (get_distance(current_loc, next_WP_loc) < 200.0f) {
        //push another 300 m down the line
        int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
        location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.0f);
    }

    //keep flying the same course
    nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);

    return false;
}
Exemplo n.º 5
0
/**
  update_bearing_and_distance - updates bearing and distance to the vehicle
  should be called at 50hz
 */
void Tracker::update_bearing_and_distance()
{
    // exit immediately if we do not have a valid vehicle position
    if (!vehicle.location_valid) {
        return;
    }

    // calculate bearing to vehicle
    // To-Do: remove need for check of control_mode
    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
        nav_status.bearing  = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
    }

    // calculate distance to vehicle
    nav_status.distance = get_distance(current_loc, vehicle.location_estimate);

    // calculate altitude difference to vehicle using gps
    if (g.alt_source == ALT_SOURCE_GPS) {
        nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
    } else {
        // g.alt_source == ALT_SOURCE_GPS_VEH_ONLY
        nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f;
    }

    // calculate pitch to vehicle
    // To-Do: remove need for check of control_mode
    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
        if (g.alt_source == ALT_SOURCE_BARO) {
            nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
        } else {
            nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
        }
    }
}
Exemplo n.º 6
0
void ModeLoiter::update()
{
    // get distance (in meters) to destination
    _distance_to_destination = get_distance(rover.current_loc, _destination);

    // if within waypoint radius slew desired speed towards zero and use existing desired heading
    if (_distance_to_destination <= g.waypoint_radius) {
        _desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
        _yaw_error_cd = 0.0f;
    } else {
        // P controller with hard-coded gain to convert distance to desired speed
        // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
        _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);

        // calculate bearing to destination
        _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
        _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
        // if destination is behind vehicle, reverse towards it
        if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
            _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
            _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
            _desired_speed = -_desired_speed;
        }

        // reduce desired speed if yaw_error is large
        // 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
        float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
        _desired_speed *= yaw_error_ratio;
    }

    // run steering and throttle controllers
    calc_steering_to_heading(_desired_yaw_cd);
    calc_throttle(_desired_speed, false, true);
}
Exemplo n.º 7
0
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
    // set origin to last destination if waypoint controller active
    if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
        _origin = _destination;
    } else {
        // otherwise use reasonable stopping point
        calc_stopping_location(_origin);
    }
    _destination = destination;

    // initialise distance
    _distance_to_destination = get_distance(_origin, _destination);
    _reached_destination = false;

    // set final desired speed
    _desired_speed_final = 0.0f;
    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
        const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
        const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
        if (is_zero(turn_angle_cd)) {
            // if not turning can continue at full speed
            _desired_speed_final = _desired_speed;
        } else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
            // pivoting so we will stop
            _desired_speed_final = 0.0f;
        } else {
            // calculate maximum speed that keeps overshoot within bounds
            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
        }
    }
}
// currently identical to the slope aborts
void AP_Landing_Deepstall::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
{
    // when aborting a landing, mimic the verify_takeoff with steering hold. Once
    // the altitude has been reached, restart the landing sequence
    throttle_suppressed = false;
    landing.nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
}
Exemplo n.º 9
0
/// get the ground course of the next navigation leg in centidegrees
/// from 0 36000. Return default_angle if next navigation
/// leg cannot be determined
int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
{
    Mission_Command cmd;
    if (!get_next_nav_cmd(_nav_cmd.index+1, cmd)) {
        return default_angle;
    }
    return get_bearing_cd(_nav_cmd.content.location, cmd.content.location);
}
Exemplo n.º 10
0
/*
  verify a LOITER_TO_ALT command. This involves checking we have
  reached both the desired altitude and desired heading. The desired
  altitude only needs to be reached once.
 */
bool Plane::verify_loiter_to_alt() 
{
    update_loiter();

    //has target altitude been reached?
    if (condition_value != 0) {
        if (labs(condition_value - current_loc.alt) < 500) {
            //Only have to reach the altitude once -- that's why I need
            //this global condition variable.
            //
            //This is in case of altitude oscillation when still trying
            //to reach the target heading.
            condition_value = 0;
        } else {
            return false;
        }
    }
    
    //has target heading been reached?
    if (condition_value2 != 0) {
        //Get the lat/lon of next Nav waypoint after this one:
        AP_Mission::Mission_Command next_nav_cmd;
        if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
                                       next_nav_cmd)) {
            //no next waypoint to shoot for -- go ahead and break out of loiter
            return true;        
        } 

        // Bearing in radians
        int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);

        // get current heading. We should probably be using the ground
        // course instead to improve the accuracy in wind
        int32_t heading_cd = ahrs.yaw_sensor;

        int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
 
        /*
          Check to see if the the plane is heading toward the land
          waypoint
          We use 10 degrees of slop so that we can handle 100
          degrees/second of yaw
        */
        if (abs(heading_err_cd) <= 1000) {
            //Want to head in a straight line from _here_ to the next waypoint.
            //DON'T want to head in a line from the center of the loiter to 
            //the next waypoint.
            //Therefore: mark the "last" (next_wp_loc is about to be updated)
            //wp lat/lon as the current location.
            next_WP_loc = current_loc;
            return true;
        } else {
            return false;
        }
    } 

    return true;
}
Exemplo n.º 11
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;
}
Exemplo n.º 12
0
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
///     terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin)
///     returns false on failure (likely caused by missing terrain data)
bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt)
{
    // store origin and destination locations
    _origin = origin;
    _destination = destination;
    _terrain_alt = terrain_alt;
    Vector3f pos_delta = _destination - _origin;

    _track_length = pos_delta.length(); // get track length

    // calculate each axis' percentage of the total distance to the destination
    if (is_zero(_track_length)) {
        // avoid possible divide by zero
        _pos_delta_unit.x = 0;
        _pos_delta_unit.y = 0;
        _pos_delta_unit.z = 0;
    }else{
        _pos_delta_unit = pos_delta/_track_length;
    }

    // calculate leash lengths
    calculate_wp_leash_length();

    // initialise yaw heading
    if (_track_length >= WPNAV_YAW_DIST_MIN) {
        _yaw = get_bearing_cd(_origin, _destination);
    } else {
        // set target yaw to current heading target
        _yaw = _attitude_control.get_att_target_euler_cd().z;
    }

    // get origin's alt-above-terrain
    float origin_terr_offset = 0.0f;
    if (terrain_alt) {
        if (!get_terrain_offset(origin_terr_offset)) {
            return false;
        }
    }

    // initialise intermediate point to the origin
    _pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset));
    _track_desired = 0;             // target is at beginning of track
    _flags.reached_destination = false;
    _flags.fast_waypoint = false;   // default waypoint back to slow
    _flags.slowing_down = false;    // target is not slowing down yet
    _flags.segment_type = SEGMENT_STRAIGHT;
    _flags.new_wp_destination = true;   // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition

    // initialise the limited speed to current speed along the track
    const Vector3f &curr_vel = _inav.get_velocity();
    // get speed along track (note: we convert vertical speed into horizontal speed equivalent)
    float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
    _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);

    return true;
}
Exemplo n.º 13
0
/// update - update circle controller
void AC_Circle::update()
{
    // calculate dt
    float dt = _pos_control.time_since_last_xy_update();
    if (dt >= 0.2f) {
        dt = 0.0f;
    }

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

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

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

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

        // heading is from vehicle to center of circle
        _yaw = get_bearing_cd(_inav.get_position(), _center);
    } else {
        // set target position to center
        Vector3f target;
        target.x = _center.x;
        target.y = _center.y;
        target.z = _pos_control.get_alt_target();

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

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

    // update position controller
    _pos_control.update_xy_controller();
}
Exemplo n.º 14
0
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
float Copter::get_roi_yaw()
{
    static uint8_t roi_yaw_counter = 0;     // used to reduce update rate to 100hz

    roi_yaw_counter++;
    if (roi_yaw_counter >= 4) {
        roi_yaw_counter = 0;
        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
    }

    return yaw_look_at_WP_bearing;
}
Exemplo n.º 15
0
/*
  calculate the turn angle for the next leg of the mission
 */
void Plane::setup_turn_angle(void)
{
    int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
    if (next_ground_course_cd == -1) {
        // the mission library can't determine a turn angle, assume 90 degrees
        auto_state.next_turn_angle = 90.0f;
    } else {
        // get the heading of the current leg
        int32_t ground_course_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);

        // work out the angle we need to turn through
        auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
    }
}    
Exemplo n.º 16
0
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
    // store origin and destination locations
    _origin = origin;
    _destination = destination;
    Vector3f pos_delta = _destination - _origin;

    _track_length = pos_delta.length(); // get track length

    // calculate each axis' percentage of the total distance to the destination
    if (_track_length == 0.0f) {
        // avoid possible divide by zero
        _pos_delta_unit.x = 0;
        _pos_delta_unit.y = 0;
        _pos_delta_unit.z = 0;
    }else{
        _pos_delta_unit = pos_delta/_track_length;
    }

    // check _wp_accel_cms is reasonable
    if (_wp_accel_cms <= 0) {
        _wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
    }

    // initialise position controller speed and acceleration
    _pos_control.set_speed_xy(_wp_speed_cms);
    _pos_control.set_accel_xy(_wp_accel_cms);
    _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
    _pos_control.calc_leash_length_xy();
    _pos_control.calc_leash_length_z();

    // calculate leash lengths
    calculate_wp_leash_length();

    // initialise yaw heading
    _yaw = get_bearing_cd(_origin, _destination);

    // initialise intermediate point to the origin
    _pos_control.set_pos_target(origin);
    _track_desired = 0;             // target is at beginning of track
    _flags.reached_destination = false;
    _flags.fast_waypoint = false;   // default waypoint back to slow
    _flags.segment_type = SEGMENT_STRAIGHT;

    // initialise the limited speed to current speed along the track
    const Vector3f &curr_vel = _inav->get_velocity();
    // get speed along track (note: we convert vertical speed into horizontal speed equivalent)
    float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
    _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
}
Exemplo n.º 17
0
void ModeLoiter::update()
{
    // get distance (in meters) to destination
    _distance_to_destination = get_distance(rover.current_loc, _destination);

    // if within waypoint radius slew desired speed towards zero and use existing desired heading
    if (_distance_to_destination <= g.waypoint_radius) {
        if (is_negative(_desired_speed)) {
            _desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
        } else {
            _desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
        }
        _yaw_error_cd = 0.0f;
    } else {
        // P controller with hard-coded gain to convert distance to desired speed
        // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
        _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);

        // calculate bearing to destination
        _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
        _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
        // if destination is behind vehicle, reverse towards it
        if (fabsf(_yaw_error_cd) > 9000) {
            _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
            _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
            _desired_speed = -_desired_speed;
        }

        // reduce desired speed if yaw_error is large
        // note: copied from calc_reduced_speed_for_turn_or_distance
        float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
        _desired_speed *= yaw_error_ratio;
    }

    // run steering and throttle controllers
    calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
    calc_throttle(_desired_speed, false, true);

    // mark us as in_reverse when using a negative throttle
    // To-Do: only in reverse if vehicle is actually travelling backwards?
    rover.set_reverse(_desired_speed < 0);
}
Exemplo n.º 18
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 = get_distance(home_loc, loc);
        int32_t angle = wrap_360_cd(get_bearing_cd(loc, 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);
    }
}
Exemplo n.º 19
0
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_lateral_acceleration and before calc_throttle
// relies on these internal members being updated: _yaw_error_cd, _distance_to_destination
float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
{
    // reduce speed to zero during pivot turns
    if (rover.use_pivot_steering(_yaw_error_cd)) {
        return 0.0f;
    }

    // reduce speed to limit overshoot from line between origin and destination
    // calculate number of degrees vehicle must turn to face waypoint
    const float heading_cd = is_negative(desired_speed) ? wrap_180_cd(ahrs.yaw_sensor + 18000) : ahrs.yaw_sensor;
    const float wp_yaw_diff = wrap_180_cd(rover.nav_controller->target_bearing_cd() - heading_cd);
    const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f));

    // calculate distance from vehicle to line + wp_overshoot
    const float line_yaw_diff = wrap_180_cd(get_bearing_cd(_origin, _destination) - heading_cd);
    const float crosstrack_error = rover.nav_controller->crosstrack_error();
    const float dist_from_line = fabsf(crosstrack_error);
    const bool heading_away = is_positive(line_yaw_diff) == is_positive(crosstrack_error);
    const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;

    // calculate radius of circle that touches vehicle's current position and heading and target position and heading
    float radius_m = 999.0f;
    float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
    if (!is_zero(radius_calc_denom)) {
        radius_m = MAX(0.0f, rover.g.waypoint_overshoot + wp_overshoot_adj) / radius_calc_denom;
    }

    // calculate and limit speed to allow vehicle to stay on circle
    float overshoot_speed_max = safe_sqrt(g.turn_max_g * GRAVITY_MSS * MAX(g2.turn_radius, radius_m));
    float speed_max = constrain_float(desired_speed, -overshoot_speed_max, overshoot_speed_max);

    // limit speed based on distance to waypoint and max acceleration/deceleration
    if (is_positive(_distance_to_destination) && is_positive(attitude_control.get_decel_max())) {
        const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * attitude_control.get_decel_max() + sq(_desired_speed_final));
        speed_max = constrain_float(speed_max, -dist_speed_max, dist_speed_max);
    }

    // return minimum speed
    return speed_max;
}
Exemplo n.º 20
0
/**
  update_bearing_and_distance - updates bearing and distance to the vehicle
  should be called at 50hz
 */
void Tracker::update_bearing_and_distance()
{
    // exit immediately if we do not have a valid vehicle position
    if (!vehicle.location_valid) {
        return;
    }

    // calculate bearing to vehicle
    // To-Do: remove need for check of control_mode
    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
        nav_status.bearing  = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
    }

    // calculate distance to vehicle
    nav_status.distance = get_distance(current_loc, vehicle.location_estimate);

    // calculate pitch to vehicle
    // To-Do: remove need for check of control_mode
    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
        nav_status.pitch    = degrees(atan2f(nav_status.altitude_difference, nav_status.distance));
    }
}
Exemplo n.º 21
0
bool Plane::verify_continue_and_change_alt()
{
    // is waypoint info not available and heading hold is?
    if (locations_are_same(prev_WP_loc, next_WP_loc) &&
        steer_state.hold_course_cd != -1) {
        //keep flying the same course with fixed steering heading computed at start if cmd
        nav_controller->update_heading_hold(steer_state.hold_course_cd);
    }
    else {
        // Is the next_WP less than 200 m away?
        if (get_distance(current_loc, next_WP_loc) < 200.0f) {
            //push another 300 m down the line
            int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
            location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.0f);
        }

        //keep flying the same course
        nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
    }

    //climbing?
    if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) {
        return true;
    }
    //descending?
    else if (condition_value == 2 &&
             adjusted_altitude_cm() <= next_WP_loc.alt) {
        return true;
    }    
    //don't care if we're climbing or descending
    else if (labs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
        return true;
    }

    return false;
}
Exemplo n.º 22
0
/// get_bearing_to_target - get bearing to target position in centi-degrees
int32_t AC_PosControl::get_bearing_to_target() const
{
    return get_bearing_cd(_inav.get_position(), _pos_target);
}
Exemplo n.º 23
0
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_bearing_to_destination()
{
    return get_bearing_cd(_inav->get_position(), _destination);
}
Exemplo n.º 24
0
	void pf_field::update(local_member* p_ac, AP_AHRS_DCM* ahrs, AP_Airspeed* airspeed)
{
#endif
#if HIL_MODE == HIL_MODE_ATTITUDE
	void pf_field::update(local_member* p_ac, AP_AHRS_HIL* ahrs, AP_Airspeed* airspeed)
{
#endif
				const Relative* tmp_p_rel = p_ac->get_rel(); //Gets the current relative information from the ac object
				Vector3i tmp_gV_L =  *p_ac->get_gV_L();
				uint16_t tmp_psi_ac = *p_ac->get_hdg();	//Get our heading (deg)
				//Calculate total repulsive function from i flock members
				Vector3f tmp_r_phi; //initialize vector (NED components)
				//Sum repulsive functions from i flock members
				for(int i=0;i<tmp_p_rel->Num_members;i++)
				{
					//indexing convention to organize member pointers
					int j = tmp_p_rel->Member_ids[i]-1; 
					//Relative distance to flock members (NED components)
					Vector3f tmp_dX;	//initialize vector each iteration
					tmp_dX.x=tmp_p_rel->dX[j]/100.0;	//[m]
					tmp_dX.y=tmp_p_rel->dY[j]/100.0;	//[m]
					tmp_dX.z=tmp_p_rel->dZ[j]/100.0;	//[m]

					//Calculate repulsive parameters based on relative states

					//Gaussian repulsive function accumulation
					tmp_r_phi.x += (-2*(tmp_dX.x)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
					tmp_r_phi.y += (-2*(tmp_dX.y)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
					tmp_r_phi.z += (-2*(tmp_dX.z)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
				}
				_phi_r = tmp_r_phi;		//Repulsive potential gradient vector (NED component)

				//Relative distance vector to leader (NED components)
				Vector3i tmp_dL;	//initialize vector	(NED components)
				tmp_dL.x=tmp_p_rel->dXL;		//[m*100]
				tmp_dL.y=tmp_p_rel->dYL;		//[m*100]
				tmp_dL.z=tmp_p_rel->dZL;		//[m*100]
				//Relative distance magnitude to leader
				int32_t tmp_d2L=tmp_p_rel->d2L;	//[m*100]
				//Relative velocity to leader (NED components)
				//Vector3i tmp_dV;	//initialize vector (NED components)
				Vector3i tmp_gV_LNAV;	//initialize dVaspd (Local Navigation frame)
				//tmp_dV.x=tmp_p_rel->dVXL;		//[m/s*100]
				//tmp_dV.y=tmp_p_rel->dVYL;		//[m/s*100]
				//tmp_dV.z=tmp_p_rel->dVZL;		//[m/s*100]
				float Cpsi = cos(ToRad(tmp_psi_ac/100.0));
				float Spsi = sin(ToRad(tmp_psi_ac/100.0));

				tmp_gV_LNAV.x=int16_t((tmp_gV_L.x)*Cpsi+(tmp_gV_L.y)*Spsi);		//[m/s*100]
				tmp_gV_LNAV.y=int16_t(-(tmp_gV_L.x)*Spsi+(tmp_gV_L.y)*Cpsi);		//[m/s*100]
				tmp_gV_LNAV.z=tmp_p_rel->dVZL;		//[m/s*100]


				//Calculate the attractive potential gradient (NED components)
					/*Note:
					Attractive potential is comprised of two types of potential functions
					-In the far-field regime we use a linear potential field
					-In the near-field regime we use a quadratic potential field
					These fields must be matched at _chi, the near-field, far-field regime cut-off such that
					-Gradient of quadratic at _chi = gradient of linear at _chi

					In far-field, we are guided towards the leader's position (no offset) - this should improve convergence
					In near-field, we are guided towards the offset position, the side of which is dictated by swarm algorithm
				*/
					Vector3i tmp_pf_offset_NED;						//Initialize vector
					uint16_t tmp_psi_L = tmp_p_rel->hdgL;		//Get the heading of the Leader
					float CpsiL = cos(ToRad(tmp_p_rel->hdgL/100.0));
					float SpsiL = sin(ToRad(tmp_p_rel->hdgL/100.0));
					//set offset (Leader's Local frame)
					Vector3i tmp_pf_offset = _pf_offset_l;
					//update offset with side correction
					tmp_pf_offset.y = tmp_pf_offset.y*_side;
					//Rotate the offset distances (Leader's Local Navigation Frame) to NED frame
					tmp_pf_offset_NED.x = tmp_pf_offset.x*CpsiL - tmp_pf_offset.y*SpsiL;
					tmp_pf_offset_NED.y = tmp_pf_offset.x*SpsiL + tmp_pf_offset.y*CpsiL;
					tmp_pf_offset_NED.z = tmp_pf_offset.z;

					//Extrapolate Goal for NF case
					Location tmp_goal_loc = *p_ac->get_loc();
					Location tmp_current_loc = *p_ac->get_loc();

					//A bunch of new stuff for this extrapolation technique
					location_offset(&tmp_goal_loc,tmp_dL.x/100.0, tmp_dL.y/100.0);
					location_offset(&tmp_goal_loc,-tmp_pf_offset_NED.x/100.0, -tmp_pf_offset_NED.y/100.0);
					float tmp_N_extrap = 10*CpsiL;
					float tmp_E_extrap = 10*SpsiL;
					location_offset(&tmp_goal_loc,tmp_N_extrap, tmp_E_extrap);
					int32_t tmp_B_extrap = get_bearing_cd(&tmp_current_loc,&tmp_goal_loc);

					Vector3f extrap_uvec_NED;
					extrap_uvec_NED.x = cos(ToRad(tmp_B_extrap/100.0));
					extrap_uvec_NED.y = sin(ToRad(tmp_B_extrap/100.0));
					extrap_uvec_NED.z = 0;

					tmp_dL -=tmp_pf_offset_NED;
					tmp_d2L = int32_t(100*safe_sqrt(square(tmp_dL.x/100.0)+square(tmp_dL.y/100.0)+square(tmp_dL.z/100.0)));

				//Subtract offset distance vector from leader distance vector to get modified relative distance vector to leader
				if(tmp_d2L<(_chi*100))	//Near-field regime condition
				{
					_phi_a.x = 2*(_lambda.x/100.0)*(tmp_dL.x/100.0);			//Calculate Quad PF X gradient with weighting parameter (NED frame)
					_phi_a.y = 2*(_lambda.y/100.0)*(tmp_dL.y/100.0);			//Calculate Quad PF Y gradient with weighting parameter (NED frame)
					_phi_a.z = 2*(_lambda.z/100.0)*(tmp_dL.z/100.0);			//Calculate Quad PF Z gradient with weighting parameter (NED frame)
					float tmp_phi_a_mag = _phi_a.length();
					_phi_a_c.x = tmp_phi_a_mag*extrap_uvec_NED.x;
					_phi_a_c.y = tmp_phi_a_mag*extrap_uvec_NED.y;
					_phi_a_c.z = 0;
				}
				else
				{
					//float tmp_M =2*safe_sqrt(square(_lambda.x/100.0)+square(_lambda.y/100.0)+square(_lambda.z/100.0))*(_chi);		//Slope of linear potential field (Magnitude of gradient) = nominal magnitude of quadratic gradient at chi
					float tmp_M = 2*_chi;
					float tmp_R = safe_sqrt(square((_lambda.x/100.0)*(tmp_dL.x/100.0))+square((_lambda.y/100.0)*(tmp_dL.y/100.0))+square((_lambda.z/100.0)*(tmp_dL.z/100)));
					_phi_a.x = tmp_M*square(_lambda.x/100.0)*(tmp_dL.x/100.0)/tmp_R;			//Calculate Lin PF X gradient with weighting parameter (NED frame)
					_phi_a.y = tmp_M*square(_lambda.y/100.0)*(tmp_dL.y/100.0)/tmp_R;			//Calculate Lin PF Y gradient with weighting parameter (NED frame)
					_phi_a.z = tmp_M*square(_lambda.z/100.0)*(tmp_dL.z/100.0)/tmp_R;			//Calculate Lin PF Z gradient with weighting parameter (NED frame)
				}
				//Calculate the total potential gradient components
				_phi_NED = _phi_a + _phi_r;
				_phi_c_NED = _phi_a_c + _phi_r;
				

				//Calculate the magnitude of the total potential function gradient
				if(_phi_NED.length() > 0) //divide-by-zero check (safety first)
				{
					//Calculate the normalized potential gradient components
					_Nphi_NED = _phi_NED.normalized();
					_Nphi_c_NED = _phi_c_NED.normalized();
					//Find X component in Local Navigation frame by rotating NED vector about heading
					_Nphi_l.x = _Nphi_NED.x*Cpsi + _Nphi_NED.y*Spsi;
					if(tmp_d2L<_chi*100) //near-field consideration- We don't want the VWP behind the body
					{
						_regime_mask = 0x01;	//Make first bit in regime mask 1 to reflect that we're in near-field regime
						
					
/*						//Correct normalized vector for VWP in near-field
						if(_Nphi_l.x<0)		//Vector is pointing behind ac in Local Navigation frame
						{
							//Rotate NED pf gradiend components into Local Navigation components
							_phi_l.x = _phi_NED.x*cos(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*sin(ToRad(tmp_psi_ac/100.0));
							_phi_l.y = - _phi_NED.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*cos(ToRad(tmp_psi_ac/100.0));
							//Correct potential function to point forward
							/*	Note:
								Corrected by using airspeed instead of relative distance... almost a predictor of where the goal "will" be in a second
							/
							float airspeed_est;
							if(ahrs->airspeed_estimate(&airspeed_est))
							{
								_phi_l.x = 2*(_lambda.x/100.0)*(airspeed_est);
							}
							else
							{
								_phi_l.x = 2*(_lambda.x/100.0)*10; //Not the most elegant way to do this... 10 is an average airspeed for the skysurfer
							}
							//Corrected potential rotated back into NED frame from Local Navigation frame
							_phi_c_NED.x= _phi_l.x*cos(ToRad(tmp_psi_ac/100.0)) - _phi_l.y*sin(ToRad(tmp_psi_ac/100.0));
							_phi_c_NED.y= _phi_l.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_l.y*cos(ToRad(tmp_psi_ac/100.0));
							//Normalize the corrected potential gradient vector
							_Nphi_c_NED = _phi_c_NED.normalized();
							//Modify regime mask to reflect that both the first bit and the second bit are 1 (near-field regime, and using a corrected 
							_regime_mask = 0x03;
						}
						//Make sure regime mask reflects that only 
					else _regime_mask = 0x01;					*/	
					}
					else
					{
						//Regime mask should reflect that we are not in the near-field, and we therefore aren't using a corrected potential field
						_regime_mask = 0x00;
					}
				}
				else
				{
					//If the magnitude of the gradient is not greater than 0, make the norm 0 to avoid divide by 0 problems 
					_Nphi_NED.zero();
					_Nphi_c_NED.zero();
				}

				float tmp_dNorth_com;
				float tmp_dEast_com;
				float tmp_dalt_com;
				float tmp_dspd_com;


				if(tmp_d2L<_chi*100)
				{
					//Calculate the North and East Offset [m]
					tmp_dNorth_com	=	_VWP_offset*_Nphi_c_NED.x;
					tmp_dEast_com	=	_VWP_offset*_Nphi_c_NED.y;
					//Calculate the change in altitude [m]
					//k_alt_V is the equivalent of a derivative gain- set to 0 for simplicity.
					//tmp_dalt_com	=	_VWP_Z_offset*_Nphi_NED.z;  
					tmp_dalt_com = tmp_dL.z/100.0;
					_phi_norm = _Nphi_c_NED;
				}
				else
				{								//Far-Field Case
					tmp_dNorth_com	=	_VWP_offset*_Nphi_NED.x;
					tmp_dEast_com	=	_VWP_offset*_Nphi_NED.y;
					//tmp_dalt_com	=	_VWP_Z_offset*_Nphi_NED.z;
					tmp_dalt_com = tmp_dL.z/100.0;
					_phi_norm = _Nphi_NED;
				}
				
					const Location* p_current_location = p_ac->get_loc();
					_next_VWP = *p_current_location;
					location_offset(&_next_VWP, tmp_dNorth_com, tmp_dEast_com);
					_next_VWP.alt+=(int32_t)(tmp_dalt_com*100); //double-check sign here.
					constrain(_next_VWP.alt,PFG_MIN_ALT,PFG_MAX_ALT);

				if(tmp_d2L<_chi*100)
				{							//Near-Field Case
					
					//New speed matching/ Speed from PFG command algorithm
					_next_airspeed_com = int32_t(100*((tmp_gV_LNAV.x/100.0*(PFG_K_P_dV/100.0)) +(_Nphi_l.x*(PFG_K_I_dV/100.0))));
					constrain(_next_airspeed_com,PFG_MIN_AIRSPEED_CM,PFG_MAX_AIRSPEED_CM);
				}
				else
				{							//Far-Field Case
					_next_airspeed_com = PFG_MAX_AIRSPEED_CM; //In far-field, set airspeed to maximum allowable (to catch up)
				}
} 

const Location* pf_field::get_VWP(){
	return (const Location*)&_next_VWP;
}

const int32_t* pf_field::get_new_speed(){
	return (const int32_t*)&_next_airspeed_com;
}

const Vector3f* pf_field::get_pfg_att(){
	return (const Vector3f*)&_phi_a;
}

const Vector3f* pf_field::get_pfg_rep(){
	return (const Vector3f*)&_phi_r;
}

const Vector3f* pf_field::get_pfg_norm(){
	return (const Vector3f*)&_phi_norm;
}

uint8_t pf_field::get_regime_mask(){
	return _regime_mask;
}

pf_field PF_FIELD;
Exemplo n.º 25
0
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_bearing_to_target() const
{
    return get_bearing_cd(_inav->get_position(), _target);
}
Exemplo n.º 26
0
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_wp_bearing_to_destination() const
{
    return get_bearing_cd(_inav.get_position(), _destination);
}
Exemplo n.º 27
0
void Copter::ModeFollow::run()
{
    // if not armed set throttle to zero and exit immediately
    if (is_disarmed_or_landed()) {
        make_safe_spool_down();
        return;
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // re-use guided mode's velocity controller
    // Note: this is safe from interference from GCSs and companion computer's whose guided mode
    //       position and velocity requests will be ignored while the vehicle is not in guided mode

    // variables to be sent to velocity controller
    Vector3f desired_velocity_neu_cms;
    bool use_yaw = false;
    float yaw_cd = 0.0f;

    Vector3f dist_vec;  // vector to lead vehicle
    Vector3f dist_vec_offs;  // vector to lead vehicle + offset
    Vector3f vel_of_target;  // velocity of lead vehicle
    if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
        // convert dist_vec_offs to cm in NEU
        const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);

        // calculate desired velocity vector in cm/s in NEU
        const float kp = g2.follow.get_pos_p().kP();
        desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
        desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
        desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);

        // scale desired velocity to stay within horizontal speed limit
        float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
        if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy())) {
            const float scalar_xy = pos_control->get_max_speed_xy() / desired_speed_xy;
            desired_velocity_neu_cms.x *= scalar_xy;
            desired_velocity_neu_cms.y *= scalar_xy;
            desired_speed_xy = pos_control->get_max_speed_xy();
        }

        // limit desired velocity to be between maximum climb and descent rates
        desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down()), pos_control->get_max_speed_up());

        // unit vector towards target position (i.e. vector to lead vehicle + offset)
        Vector3f dir_to_target_neu = dist_vec_offs_neu;
        const float dir_to_target_neu_len = dir_to_target_neu.length();
        if (!is_zero(dir_to_target_neu_len)) {
            dir_to_target_neu /= dir_to_target_neu_len;
        }

        // create horizontal desired velocity vector (required for slow down calculations)
        Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);

        // create horizontal unit vector towards target (required for slow down calculations)
        Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
        if (!dir_to_target_xy.is_zero()) {
            dir_to_target_xy.normalize();
        }

        // slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
        const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
        copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);

        // limit the horizontal velocity to prevent fence violations
        copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), desired_velocity_xy_cms, G_Dt);

        // copy horizontal velocity limits back to 3d vector
        desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
        desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;

        // limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
        const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
        desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);

        // get avoidance adjusted climb rate
        desired_velocity_neu_cms.z = get_avoidance_adjusted_climbrate(desired_velocity_neu_cms.z);

        // calculate vehicle heading
        switch (g2.follow.get_yaw_behave()) {
            case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
                const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
                if (dist_vec_xy.length() > 1.0f) {
                    yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
                    use_yaw = true;
                }
                break;
            }

            case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
                float target_hdg = 0.0f;
                if (g2.follow.get_target_heading_deg(target_hdg)) {
                    yaw_cd = target_hdg * 100.0f;
                    use_yaw = true;
                }
                break;
            }

            case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
                const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
                if (vel_vec.length() > 100.0f) {
                    yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
                    use_yaw = true;
                }
                break;
            }

            case AP_Follow::YAW_BEHAVE_NONE:
            default:
                // do nothing
               break;

        }
    }

    // log output at 10hz
    uint32_t now = AP_HAL::millis();
    bool log_request = false;
    if ((now - last_log_ms >= 100) || (last_log_ms == 0)) {
        log_request = true;
        last_log_ms = now;
    }
    // re-use guided mode's velocity controller (takes NEU)
    Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);

    Copter::ModeGuided::run();
}
Exemplo n.º 28
0
// update L1 control for loitering
void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction)
{
	struct Location _current_loc;

	// Calculate guidance gains used by PD loop (used during circle tracking)
	float omega = (6.2832f / _L1_period);
	float Kx = omega * omega;
	float Kv = 2.0f * _L1_damping * omega;

	// Calculate L1 gain required for specified damping (used during waypoint capture)
	float K_L1 = 4.0f * _L1_damping * _L1_damping;

	//Get current position and velocity
    _ahrs->get_position(&_current_loc);

	// update _target_bearing_cd
	_target_bearing_cd = get_bearing_cd(&_current_loc, &center_WP);

	Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();

	//Calculate groundspeed
	float groundSpeed = _groundspeed_vector.length();

	// Calculate time varying control parameters
	// Calculate the L1 length required for specified period
	// 0.3183099 = 1/pi
	_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;

	//Convert current location and WP positionsto 2D vectors in lat and long
    Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f));
    Vector2f A_v((center_WP.lat*1.0e-7f), (center_WP.lng*1.0e-7f));

	//Calculate the NE position of the aircraft relative to WP A
    A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH;
	
    //Calculate the unit vector from WP A to aircraft
    Vector2f A_air_unit = (A_air).normalized();

	//Calculate Nu to capture center_WP
	float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
	float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
	float Nu = atan2f(xtrackVelCap,ltrackVelCap);
	Nu = constrain_float(Nu, -1.5708f, +1.5708f); //Limit Nu to +- Pi/2

	//Calculate lat accln demand to capture center_WP (use L1 guidance law)
	float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
	
	//Calculate radial position and velocity errors
	float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
	float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle

	// keep crosstrack error for reporting
	_crosstrack_error = xtrackErrCirc;
	
	//Calculate PD control correction to circle waypoint
	float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
	
	//Calculate tangential velocity
	float velTangent = xtrackVelCap * float(loiter_direction);
	
    //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
    if ( velTangent < 0.0f ) {
        latAccDemCircPD =  _maxf(latAccDemCircPD , 0.0f);
	}
	
	// Calculate centripetal acceleration demand
	float latAccDemCircCtr = velTangent * velTangent / _maxf((0.5f * radius) , (radius + xtrackErrCirc));

	//Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
	float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
	
	// Perform switchover between 'capture' and 'circle' modes at the point where the commands cross over to achieve a seamless transfer
	// Only fly 'capture' mode if outside the circle
	if ((latAccDemCap < latAccDemCirc && loiter_direction > 0 && xtrackErrCirc > 0.0f) | (latAccDemCap > latAccDemCirc && loiter_direction < 0 && xtrackErrCirc > 0.0f)) {
		_latAccDem = latAccDemCap;
		_WPcircle = false;
		_bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
	} else {
		_latAccDem = latAccDemCirc;
		_WPcircle = true;
		_bearing_error = 0.0f; // bearing error (radians), +ve to left of track
		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
	}
}
Exemplo n.º 29
0
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_loiter_bearing_to_target() const
{
    return get_bearing_cd(_inav->get_position(), _pos_control.get_pos_target());
}
Exemplo n.º 30
0
// update L1 control for waypoint navigation
// this function costs about 3.5 milliseconds on a AVR2560
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP)
{

	struct Location _current_loc;
	float Nu;
	float xtrackVel;
	float ltrackVel;
	
	// Calculate L1 gain required for specified damping
	float K_L1 = 4.0f * _L1_damping * _L1_damping;

	// Get current position and velocity
    _ahrs->get_position(&_current_loc);

	// update _target_bearing_cd
	_target_bearing_cd = get_bearing_cd(&_current_loc, &next_WP);

	Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
	
	//Calculate groundspeed
	float groundSpeed = _groundspeed_vector.length();

	// Calculate time varying control parameters
	// Calculate the L1 length required for specified period
	// 0.3183099 = 1/1/pipi
	_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
	
	//Convert current location and WP positions to 2D vectors in lat and long
    Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f));
    Vector2f A_v((prev_WP.lat*1.0e-7f), (prev_WP.lng*1.0e-7f));
    Vector2f B_v((next_WP.lat*1.0e-7f), (next_WP.lng*1.0e-7f));

	//Calculate the NE position of the aircraft and WP B relative to WP A
    A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH;
    Vector2f AB = _geo2planar(A_v, B_v)*RADIUS_OF_EARTH;
	
    //Calculate the unit vector from WP A to WP B
    Vector2f AB_unit = (AB).normalized();

	// calculate distance to target track, for reporting
	_crosstrack_error = AB_unit % A_air;

    //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
	//and further than L1 distance from WP A. Then use WP A as the L1 reference point
	//Otherwise do normal L1 guidance
	float WP_A_dist = A_air.length();
	float alongTrackDist = A_air * AB_unit;
	if (WP_A_dist > _L1_dist && alongTrackDist/(WP_A_dist + 1.0f) < -0.7071f) {

        //Calc Nu to fly To WP A
		Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
		xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
		ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
		Nu = atan2f(xtrackVel,ltrackVel);
		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
		
    } else { //Calc Nu to fly along AB line
		
		//Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
		xtrackVel = _groundspeed_vector % AB_unit; // Velocity cross track
		ltrackVel = _groundspeed_vector * AB_unit; // Velocity along track
		float Nu2 = atan2f(xtrackVel,ltrackVel);
		//Calculate Nu1 angle (Angle to L1 reference point)
		float xtrackErr = A_air % AB_unit;
		float sine_Nu1 = xtrackErr/_maxf(_L1_dist , 0.1f);
		//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
		sine_Nu1 = constrain_float(sine_Nu1, -0.7854f, 0.7854f);
		float Nu1 = asinf(sine_Nu1);
		Nu = Nu1 + Nu2;
		_nav_bearing = atan2f(AB_unit.y, AB_unit.x) + Nu1; // bearing (radians) from AC to L1 point		
	}	
		
	//Limit Nu to +-pi
	Nu = constrain_float(Nu, -1.5708f, +1.5708f);
	_latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
	
	// Waypoint capture status is always false during waypoint following
	_WPcircle = false;
	
	_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
}