Ardupilot笔记：Rover auto模式下的执行流程

先从mode_auto.cpp的update()开始分析。
流程如图：

进入函数update()后会执行函数navigate_to_waypoint()。

mode_auto.cpp

void ModeAuto::update()
{
    // update navigation controller
    navigate_to_waypoint();
}

mode.cpp

void Mode::navigate_to_waypoint()
{
    // update navigation controller
    g2.wp_nav.update(rover.G_Dt);
	// call turn rate steering controller
    //根据转弯角加速度和速度进行转向
    calc_steering_from_turn_rate(g2.wp_nav.get_turn_rate_rads(), desired_speed, g2.wp_nav.get_reversed());
}

AR_WPNav.cpp

void AR_WPNav::update(float dt)
{
    // calculate the required turn of the wheels
    update_steering(current_loc, speed);

    // calculate desired speed
    update_desired_speed(dt);
}

AR_WPNav.cpp

void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
{
        // run L1 controller
        _nav_controller.set_reverse(_reversed);
        _nav_controller.update_waypoint(_reached_destination ? current_loc : _oa_origin, _oa_destination, _radius);
        _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed);//根据所需向心力和速度计算角速度
}

此处调用了L1航迹控制算法，得到向心力后，通过函数get_turn_rate_from_lat_accel(）得到应有的角速度。
至此，继续执行mode.cpp的calc_steering_from_turn_rate(）函数。
mode.cpp

// calculate steering output given a turn rate and speed
//转向的关键  好像没有直接调用到电机
void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed)
{
	//gcs().send_text(MAV_SEVERITY_INFO, "Run:Mode::calc_steering_from_turn_rate()");自动模式执行
    // calculate and send final steering command to motor library 输出范围-1——1
    //steering_out 含义未搞清
    const float steering_out = attitude_control.get_steering_out_rate(turn_rate,
                                                                      g2.motors.limit.steer_left,
                                                                      g2.motors.limit.steer_right,
                                                                      rover.G_Dt);
    //gcs().send_text(MAV_SEVERITY_NOTICE, "steering_out=%f",steering_out * 4500.0f);
    g2.motors.set_steering(steering_out * 4500.0f);
}

AP_MotorsUGV.cpp

void AP_MotorsUGV::set_steering(float steering, bool apply_scaling)
{
    _steering = steering;
    _scale_steering = apply_scaling;
}

此处会得到一个_steering值，待其他函数调用。
再来看Rover.cpp里面的一个循环任务

SCHED_TASK(set_servos,            400,    200),

void Rover::set_servos(void)
{
    // send output signals to motors
    if (motor_test) {
        motor_test_output();
    } else {
        // get ground speed
        float speed;
        if (!g2.attitude_control.get_forward_speed(speed)) {
            speed = 0.0f;
        }

        g2.motors.output(arming.is_armed(), speed, G_Dt);
    }
}

此处调用AP_MotorsUGV.cpp中的output()函数，使用到了_steering（应该是转弯成度）。

void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
{
	//gcs().send_text(MAV_SEVERITY_NOTICE, "Run:AP_MotorsUGV::output%f  %f",ground_speed,dt);
    // soft-armed overrides passed in armed status
    if (!hal.util->get_soft_armed()) {
        armed = false;
        _throttle = 0.0f;
    }

    // sanity check parameters
    sanity_check_parameters();

    // slew limit throttle
    slew_limit_throttle(dt);

    // output for regular steering/throttle style frames
    output_regular(armed, ground_speed, _steering, _throttle);

    // output for skid steering style frames
    output_skid_steering(armed, _steering, _throttle, dt);

    // output for omni frames
    output_omni(armed, _steering, _throttle, _lateral);

    // output to mainsail
    output_mainsail();

    // send values to the PWM timers for output
    SRV_Channels::calc_pwm();
    SRV_Channels::cork();
    SRV_Channels::output_ch_all();
    SRV_Channels::push();
}

SRV_Channels、SRV_Channel为PWM输出类，此处还不了解，SRV_Channels为多个输出通道，SRV_Channel为单个输出通道。