예제 #1
0
void Rover::update_navigation()
{
    switch (control_mode) {
    case MANUAL:
    case HOLD:
    case LEARNING:
    case STEERING:
    case INITIALISING:
        break;

    case AUTO:
        mission.update();
        if (do_auto_rotation) {
            do_yaw_rotation();
        }
        break;

    case RTL:
        // no loitering around the wp with the rover, goes direct to the wp position
        calc_lateral_acceleration();
        calc_nav_steer();
        if (verify_RTL()) {
            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
            set_mode(HOLD);
        }
        break;

    case GUIDED:
        switch (guided_mode) {
        case Guided_Angle:
            nav_set_yaw_speed();
            break;

        case Guided_WP:
            // no loitering around the wp with the rover, goes direct to the wp position
            calc_lateral_acceleration();
            calc_nav_steer();
            if (rtl_complete || verify_RTL()) {
                // we have reached destination so stop where we are
                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
                lateral_acceleration = 0;
            }
            break;

        default:
            gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
            break;
        }
        break;
    }
}
예제 #2
0
void Rover::update_navigation()
{
    switch (control_mode) {
    case MANUAL:
    case HOLD:
    case LEARNING:
    case STEERING:
    case INITIALISING:
        break;

    case AUTO:
        mission.update();
        break;

    case RTL:
        // no loitering around the wp with the rover, goes direct to the wp position
        calc_lateral_acceleration();
        calc_nav_steer();
        if (verify_RTL()) {
            channel_throttle->servo_out = g.throttle_min.get();
            set_mode(HOLD);
        }
        break;

    case GUIDED:
        // no loitering around the wp with the rover, goes direct to the wp position
        calc_lateral_acceleration();
        calc_nav_steer();
        if (!rtl_complete) {
            if (verify_RTL()) {
                // we have reached destination so stop where we are
                channel_throttle->servo_out = g.throttle_min.get();
                channel_steer->servo_out = 0;
                lateral_acceleration = 0;
            }
        }
        break;
    }
}
예제 #3
0
void Rover::update_current_mode(void)
{
    switch (control_mode){
    case AUTO:
    case RTL:
        set_reverse(false);
        calc_lateral_acceleration();
        calc_nav_steer();
        calc_throttle(g.speed_cruise);
        break;

    case GUIDED:
        set_reverse(false);
        if (rtl_complete || verify_RTL()) {
            // we have reached destination so stop where we are
            if (channel_throttle->servo_out != g.throttle_min.get()) {
                gcs_send_mission_item_reached_message(0);
            }
            channel_throttle->servo_out = g.throttle_min.get();
            channel_steer->servo_out = 0;
            lateral_acceleration = 0;
        } else {
            calc_lateral_acceleration();
            calc_nav_steer();
            calc_throttle(g.speed_cruise);
        }
        break;

    case STEERING: {
        /*
          in steering mode we control lateral acceleration
          directly. We first calculate the maximum lateral
          acceleration at full steering lock for this speed. That is
          V^2/R where R is the radius of turn. We get the radius of
          turn from half the STEER2SRV_P.
         */
        float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();

        // constrain to user set TURN_MAX_G
        max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);

        lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f);
        calc_nav_steer();

        // and throttle gives speed in proportion to cruise speed, up
        // to 50% throttle, then uses nudging above that.
        float target_speed = channel_throttle->pwm_to_angle() * 0.01f * 2 * g.speed_cruise;
        set_reverse(target_speed < 0);
        if (in_reverse) {
            target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
        } else {
            target_speed = constrain_float(target_speed, 0, g.speed_cruise);
        }
        calc_throttle(target_speed);
        break;
    }

    case LEARNING:
    case MANUAL:
        /*
          in both MANUAL and LEARNING we pass through the
          controls. Setting servo_out here actually doesn't matter, as
          we set the exact value in set_servos(), but it helps for
          logging
         */
        channel_throttle->servo_out = channel_throttle->control_in;
        channel_steer->servo_out = channel_steer->pwm_to_angle();

        // mark us as in_reverse when using a negative throttle to
        // stop AHRS getting off
        set_reverse(channel_throttle->servo_out < 0);
        break;

    case HOLD:
        // hold position - stop motors and center steering
        channel_throttle->servo_out = 0;
        channel_steer->servo_out = 0;
        set_reverse(false);
        break;

    case INITIALISING:
        break;
    }
}
예제 #4
0
void Rover::update_current_mode(void)
{
    switch (control_mode) {
    case AUTO:
    case RTL:
        if (!in_auto_reverse) {
            set_reverse(false);
        }
        if (!do_auto_rotation) {
            calc_lateral_acceleration();
            calc_nav_steer();
            calc_throttle(g.speed_cruise);
        } else {
            do_yaw_rotation();
        }
        break;

    case GUIDED: {
        if (!in_auto_reverse) {
            set_reverse(false);
        }
        switch (guided_mode) {
        case Guided_Angle:
            nav_set_yaw_speed();
            break;

        case Guided_WP:
            if (rtl_complete || verify_RTL()) {
                // we have reached destination so stop where we are
                if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) {
                    gcs_send_mission_item_reached_message(0);
                }
                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
                lateral_acceleration = 0;
            } else {
                calc_lateral_acceleration();
                calc_nav_steer();
                calc_throttle(g.speed_cruise);
                Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt),
                                       Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
            }
            break;

        default:
            gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
            break;
        }
        break;
    }

    case STEERING: {
        /*
          in steering mode we control lateral acceleration
          directly. We first calculate the maximum lateral
          acceleration at full steering lock for this speed. That is
          V^2/R where R is the radius of turn. We get the radius of
          turn from half the STEER2SRV_P.
         */
        float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();

        // constrain to user set TURN_MAX_G
        max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);

        lateral_acceleration = max_g_force * (channel_steer->get_control_in()/4500.0f);
        calc_nav_steer();

        // and throttle gives speed in proportion to cruise speed, up
        // to 50% throttle, then uses nudging above that.
        float target_speed = channel_throttle->get_control_in() * 0.01f * 2 * g.speed_cruise;
        set_reverse(target_speed < 0);
        if (in_reverse) {
            target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
        } else {
            target_speed = constrain_float(target_speed, 0, g.speed_cruise);
        }
        calc_throttle(target_speed);
        break;
    }

    case LEARNING:
    case MANUAL:
        /*
          in both MANUAL and LEARNING we pass through the
          controls. Setting servo_out here actually doesn't matter, as
          we set the exact value in set_servos(), but it helps for
          logging
         */
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in());

        // mark us as in_reverse when using a negative throttle to
        // stop AHRS getting off
        set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0);
        break;

    case HOLD:
        // hold position - stop motors and center steering
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
        if (!in_auto_reverse) {
            set_reverse(false);
        }
        break;

    case INITIALISING:
        break;
    }
}