예제 #1
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);
    }
}
예제 #2
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);
}
예제 #3
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);
}
예제 #4
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);
}
예제 #5
0
void ModeGuided::update()
{
    switch (_guided_mode) {
        case Guided_WP:
        {
            _distance_to_destination = get_distance(rover.current_loc, _destination);
            const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
            // check if we've reached the destination
            if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
                _reached_destination = true;
                rover.gcs().send_mission_item_reached_message(0);
            }
            // determine if we should keep navigating
            if (!_reached_destination || (rover.is_boat() && !near_wp)) {
                // drive towards destination
                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
            } else {
                stop_vehicle();
            }
            break;
        }

        case Guided_HeadingAndSpeed:
        {
            // stop vehicle if target not updated within 3 seconds
            if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
                gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
                have_attitude_target = false;
            }
            if (have_attitude_target) {
                // run steering and throttle controllers
                calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
            } else {
                stop_vehicle();
            }
            break;
        }

        case Guided_TurnRateAndSpeed:
        {
            // stop vehicle if target not updated within 3 seconds
            if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
                gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
                have_attitude_target = false;
            }
            if (have_attitude_target) {
                // run steering and throttle controllers
                float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f),
                                                                            g2.motors.limit.steer_left,
                                                                            g2.motors.limit.steer_right,
                                                                            rover.G_Dt);
                g2.motors.set_steering(steering_out * 4500.0f);
                calc_throttle(_desired_speed, true, true);
            } else {
                stop_vehicle();
            }
            break;
        }

        default:
            gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
            break;
    }
}