APM飞行模式内部结构——以loiter为例

飞行模式的架构我已经在上一篇博文中提到了，本文将lotiter模式为例，详细讲解一下lotier模式的内部控制链。lotier模式即为悬停模式，也是GPS定点模式。在起飞前确保GPS是打开的。飞手控制的是飞机水平方向的加速度。控制链如下：

1 主循环（fast_loop()）

循环是以400Hz调用的。包括以下函数：

ins.update();//更新惯导数据
attitude_control->rate_controller_run();//姿态控制器
motors_output();//电机输出
read_AHRS();//读取飞行器姿态等基础数据
read_inertia();//读取惯性导航数据
check_ekf_reset();//检查ekf是否已重置目标航向或位置
update_flight_mode();//更新飞行模式

2 重要函数

2.1 set_mode()

两个重要的接口函数，update_flight_mode()(在loop中)和set_mode()（不在loop中）,下面是set_mode()部分代码：

bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{

    // return immediately if we are already in the desired mode
    if (mode == control_mode) {
        control_mode_reason = reason;
        return true;
    }

    Copter::Mode *new_flightmode = mode_from_mode_num(mode);
    if (new_flightmode == nullptr) {
        gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
        Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
        return false;
    }

    bool ignore_checks = !motors->armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform

//...安全判断
// update flight mode
    flightmode = new_flightmode;
    control_mode = mode;
    control_mode_reason = reason;
    DataFlash.Log_Write_Mode(control_mode, reason);

2.2 run()

通过mode_from_mode_num(mode)函数返回一个模式对象。最后的flightmode就是经过层层判断最后生成的飞行模式，run（）函数标准写法：

void Copter::ModeLoiter::run()
{
    LoiterModeState loiter_state;

    float target_roll, target_pitch;
    float target_yaw_rate = 0.0f;
    float target_climb_rate = 0.0f;
    float takeoff_climb_rate = 0.0f;

    // initialize vertical speed and acceleration
    pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
    pos_control->set_accel_z(g.pilot_accel_z);

    // process pilot inputs unless we are in radio failsafe
    if (!copter.failsafe.radio) {
        // apply SIMPLE mode transform to pilot inputs
        update_simple_mode();//更新为简单模式

        // convert pilot input to lean angles
        //飞手目标的倾斜角度，根据遥控器输入
        get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());

        // process pilot's roll and pitch input
        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);

        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

        // get pilot desired climb rate
        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
        target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
    } else {
        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
        loiter_nav->clear_pilot_desired_acceleration();
    }

    // relax loiter target if we might be landed
    //...

    // 悬停状态分类
    switch (loiter_state) {
	//电机停转
    case Loiter_MotorStopped:
	   //实现代码
	   break;

	//起飞情况下
    case Loiter_Takeoff:
        //实现代码
        break;
	//着陆情况下
    case Loiter_Landed:
      	//代码实现...
        break;
	//飞行过程中
    case Loiter_Flying:

        // set motors to full range
        //允许油门从0到最大值
        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

#if PRECISION_LANDING == ENABLED
        if (do_precision_loiter()) {
            precision_loiter_xy();
        }
#endif

        // run loiter controller
        //重要的 函数
        //进入位置控制
        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);

        // call attitude controller
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);

        // adjust climb rate using rangefinder
        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);

        // get avoidance adjusted climb rate
        target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

        // update altitude target and call position controller
        pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
        pos_control->update_z_controller();
        break;
    }
}

2.3 位置控制器

进入位置控制后，首先初始化位置控制器的速度和加速度，更新水平位置控制器：

_pos_control.set_speed_xy(_speed_cms);
_pos_control.set_accel_xy(_accel_cmss);
calc_desired_velocity(dt,ekfGndSpdLimit);
_pos_control.update_xy_controller(ekfNavVelGainScaler);

然后就进入了_pos_control.update_xy_controller(位置控制器)，首先限制狗绳的长度，

calc_leash_length_xy()

然后根据飞手目标速度前馈到目标位置：

desired_vel_to_pos(dt);

接下来运行位置控制器，即：

 run_xy_controller();

进入到位置控制器后首先得到位置偏差，根据位置偏差求出目标位置，进一步求出目标速度：

 _pos_error.x = _pos_target.x - curr_pos.x;
 _pos_error.y = _pos_target.y - curr_pos.y;
 
 _distance_to_target = norm(_pos_error.x, _pos_error.y);
 _vel_target = sqrt_controller(_pos_error, kP, _accel_cms);

接下来根据期望速度与已知速度的差值，求出目标加速度：

_vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
_vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;

求pid系数：

// call pi controller
_pid_vel_xy.set_input(_vel_error);

// get p
vel_xy_p = _pid_vel_xy.get_p();

//i
if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
        vel_xy_i = _pid_vel_xy.get_i();
    } else {
        vel_xy_i = _pid_vel_xy.get_i_shrink();
    }
// get d
vel_xy_d = _pid_vel_xy.get_d();

根据速度偏差以及pid原理求出目标加速度：

accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;

根据期望加速度求出目标欧拉角

accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);

接下来将进入姿态控制器。

2.4 姿态控制器

mode_loiter.cpp进入姿态控制器的代码：

attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);

即根据位置控制器解算出的水平方向的欧拉角以及铅垂方向的角速率求出水平方向的目标角速度：
先初始化目标欧拉角：

 _attitude_target_euler_angle.x = euler_roll_angle;
 _attitude_target_euler_angle.y = euler_pitch_angle;
 _attitude_target_euler_angle.z += euler_yaw_rate*_dt;
 // Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

然后进入姿态控制器：

 attitude_controller_run_quat();

具体姿态控制代码，即将期望欧拉角转换为角速度。
void AC_AttitudeControl::attitude_controller_run_quat()
{
    // Retrieve quaternion vehicle attitude
    // TODO add _ahrs.get_quaternion()
    Quaternion attitude_vehicle_quat;
    attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());

    // Compute attitude error
    Vector3f attitude_error_vector;
    thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

    // Compute the angular velocity target from the attitude error
    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);

    // Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
    // todo: this should probably be a matrix that couples yaw as well.
    _rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
    _rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;

    ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));

    // Add the angular velocity feedforward, rotated into vehicle frame
    Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
    Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
    Quaternion desired_ang_vel_quat = to_to_from_quat.inverse()*attitude_target_ang_vel_quat*to_to_from_quat;

    // Correct the thrust vector and smoothly add feedforward and yaw input
    if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
        _rate_target_ang_vel.z = _ahrs.get_gyro().z;
    }else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
        float feedforward_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
        _rate_target_ang_vel.x += desired_ang_vel_quat.q2*feedforward_scalar;
        _rate_target_ang_vel.y += desired_ang_vel_quat.q3*feedforward_scalar;
        _rate_target_ang_vel.z += desired_ang_vel_quat.q4;
        _rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-feedforward_scalar) + _rate_target_ang_vel.z*feedforward_scalar;
    } else {
        _rate_target_ang_vel.x += desired_ang_vel_quat.q2;
        _rate_target_ang_vel.y += desired_ang_vel_quat.q3;
        _rate_target_ang_vel.z += desired_ang_vel_quat.q4;
    }

    if (_rate_bf_ff_enabled) {
        // rotate target and normalize
        Quaternion attitude_target_update_quat;
        attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
        _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
        _attitude_target_quat.normalize();
    }

    // ensure Quaternions stay normalized
    _attitude_target_quat.normalize();

    // Record error to handle EKF resets
    _attitude_ang_error = attitude_vehicle_quat.inverse() * _attitude_target_quat;
}

最后将速率控制转化为电机控制，代码在libraries/AC_AttitudeControlMulti.cpp下：

void AC_AttitudeControl_Multi::rate_controller_run()
{
    // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
    update_throttle_rpy_mix();

    Vector3f gyro_latest = _ahrs.get_gyro_latest();
    _motors.set_roll(rate_target_to_motor_roll(gyro_latest.x, _rate_target_ang_vel.x));
    _motors.set_pitch(rate_target_to_motor_pitch(gyro_latest.y, _rate_target_ang_vel.y));
    _motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z));

    control_monitor_update();
}

接下来就是电机的混控输出，在libiaries/AP_MotorsMatris下：

void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
    // remove existing motors
    for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
        remove_motor(i);
    }

    bool success = false;

    switch (frame_class) {
    case MOTOR_FRAME_TYPE_PLUS:
    	add_motor(AP_MOTORS_MOT_1,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
        add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 
        add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  
        add_motor(AP_MOTORS_MOT_4,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 
        add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 
        add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  
        success = true;
      break;
      //不同MOTOR_FRAME。。。
      }

3 总结

本文主要分析飞行过程中（Loiter_Flying）的控制链：位置控制器通过目标位置->目标速度->目标加速度->目标倾角。接下来便进入姿态控制器，遥控器输入的是两个姿态角（roll以及pith）,以及一个角速度（rate_yaw）,自动控制可以实现输入三个欧拉角，得到目标角速度。最后电机混控输出。