APM_ArduCopter源码解析学习（一）——ArduCopter主程序

最新推荐文章于 2024-04-16 16:58:03 发布

zhangfengmei1987

最新推荐文章于 2024-04-16 16:58:03 发布

阅读量3.5k

点赞数 7

分类专栏：无人机

本文链接：https://blog.csdn.net/zhangfengmei1987/article/details/110432871

版权

无人机专栏收录该内容

23 篇文章 16 订阅

订阅专栏

ArduCopter源码解析学习（一）——ArduCopter主程序

前言
一、准备工作
二、Copter.cpp解析
三、总结

前言

由于项目经常需要做功能优化所以开源飞控APM和PX4作为最热门的飞控资源，需要针对性的进行梳理。

一、准备工作

参考https://blog.csdn.net/moumde/article/details/108814986 在网上查了很多资料，在阅读本文内容之前，请仔细阅读Ardupilot的官方手册，建立起一个大概的印象即可，这里便不再讲述一些基础知识。

Github源码戳这里~
Ardusub官方手册
 Ardupilot官方手册
 Ardupilot开发者手册

由于源码内容很多，此次我仅挑选 “ardupilot/Copter/” 路径下的Copter.cpp主文件进行解析，后续如果有时间再慢慢补充其他内容。如果自己下载源码，推荐使用Souce Insight或者understand进行阅读。

二、Copter.cpp解析

2.1 scheduler table

SCHED_TASK_CLASS和SCHED_TASK用来声明产生调度任务表，表中是除了被fast_loop()函数调用任务的其他常规任务。其中_rate_hz表示调用频率，_max_time_micros表示最长等待时间。

#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros)

 
 
 
 1

#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)

 
 
 
 1

注意到SCHED_TASK_CLASS可以指向不同类，而SCHED_TASK固定为Copter类中的函数实现，Copter为无人机类型。

2.2 scheduler

get_scheduler_tasks()

如果是master版本的，程序前面应该是get_scheduler_tasks()

void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                                 uint8_t &task_count,
                                 uint32_t &log_bit)
{
    tasks = &scheduler_tasks[0];
    task_count = ARRAY_SIZE(scheduler_tasks);
    log_bit = MASK_LOG_PM;
}

该函数主要实现获取调度表其实地址，并且计算出任务数量。

2.3 fast_loop()

void Copter::fast_loop()
{
    // update INS immediately to get current gyro data populated
    ins.update();//AP_InertialSensor &ins()

    // run low level rate controllers that only require IMU data
    attitude_control->rate_controller_run();

    // send outputs to the motors library immediately
    motors_output();

    // run EKF state estimator (expensive)
    // --------------------
    read_AHRS();

#if FRAME_CONFIG == HELI_FRAME
    update_heli_control_dynamics();
    #if MODE_AUTOROTATE_ENABLED == ENABLED
        heli_update_autorotation();
    #endif
#endif //HELI_FRAME

    // Inertial Nav
    // --------------------
    read_inertia();

    // check if ekf has reset target heading or position
    check_ekf_reset();

    // run the attitude controllers
    update_flight_mode();

    // update home from EKF if necessary
    update_home_from_EKF();

    // check if we've landed or crashed
    update_land_and_crash_detectors();

#if HAL_MOUNT_ENABLED
    // camera mount's fast update
    camera_mount.update_fast();
#endif

    // log sensor health
    if (should_log(MASK_LOG_ANY)) {
        Log_Sensor_Health();
    }

    AP_Vehicle::fast_loop();
}

该函数为本文件中最重要的函数，以400Hz频率运行。
attitude_control.rate_controller_run()在libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.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();

    _rate_target_ang_vel += _rate_sysid_ang_vel;

    Vector3f gyro_latest = _ahrs.get_gyro_latest();

    _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
    _motors.set_roll_ff(get_rate_roll_pid().get_ff());

    _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
    _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());

    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);

    _rate_sysid_ang_vel.zero();
    _actuator_sysid.zero();

    control_monitor_update();
}

主要是用set_xxx()方法接收roll/pitch/yaw的-1~1的值，这些值表示的是期望尽快往哪个方向运动的意思。具体查看这里。

这里主要讲一下最主要的姿态控制函数update_flight_mode()。

2.3.1 mode.cpp

update_flight_mode()定义于Copter文件路径下的mode.cpp，该函数通常以100Hz及以上频率被调用。内部的 flightmode 定义于mode.h中，用来表示不同的飞行状态，共计27种模式。

在mode.cpp中update_flight_mode()的功能函数如下：

void Copter::update_flight_mode()
{
    surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used

    flightmode->run();
}

在mode.h中定义了各个飞行模式的具体含义内容：

    // Auto Pilot Modes enumeration
    enum class Number : uint8_t {
        STABILIZE =     0,  // manual airframe angle with manual throttle
        ACRO =          1,  // manual body-frame angular rate with manual throttle
        ALT_HOLD =      2,  // manual airframe angle with automatic throttle
        AUTO =          3,  // fully automatic waypoint control using mission commands
        GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
        LOITER =        5,  // automatic horizontal acceleration with automatic throttle
        RTL =           6,  // automatic return to launching point
        CIRCLE =        7,  // automatic circular flight with automatic throttle
        LAND =          9,  // automatic landing with horizontal position control
        DRIFT =        11,  // semi-automous position, yaw and throttle control
        SPORT =        13,  // manual earth-frame angular rate control with manual throttle
        FLIP =         14,  // automatically flip the vehicle on the roll axis
        AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
        POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
        BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
        THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
        AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
        GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
        SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
        FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
        FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
        ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
        SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
        AUTOROTATE =   26,  // Autonomous autorotation
    };

update_flight_mode()中对应的flightmode->run ()函数则是在同路径下的mode_xxx.cpp中定义。下面以ModeStabilize::run()为例进行讲解，因此我们需要进入Mode_stabilize.cpp中。

2.3.2 Mode_stabilize.cpp

注意到内部包含Mode_stabilize.cpp文件只有一个ModeStabilize::run()函数。

/*
 * Init and run calls for stabilize flight mode
 */

// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void ModeStabilize::run()
{
    // apply simple mode transform to pilot inputs
    update_simple_mode();

    // convert pilot input to lean angles
    float target_roll, target_pitch;
    get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);

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

    if (!motors->armed()) {
        // Motors should be Stopped
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
    } else if (copter.ap.throttle_zero) {
        // Attempting to Land
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
    } else {
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
    }

    switch (motors->get_spool_state()) {
    case AP_Motors::SpoolState::SHUT_DOWN:
        // Motors Stopped
        attitude_control->set_yaw_target_to_current_heading();
        attitude_control->reset_rate_controller_I_terms();
        break;

    case AP_Motors::SpoolState::GROUND_IDLE:
        // Landed
        attitude_control->set_yaw_target_to_current_heading();
        attitude_control->reset_rate_controller_I_terms();
        break;

    case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
        // clear landing flag above zero throttle
        if (!motors->limit.throttle_lower) {
            set_land_complete(false);
        }
        break;

    case AP_Motors::SpoolState::SPOOLING_UP:
    case AP_Motors::SpoolState::SPOOLING_DOWN:
        // do nothing
        break;
    }

    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

    // output pilot's throttle
    attitude_control->set_throttle_out(get_pilot_desired_throttle(),
                                       true,
                                       g.throttle_filt);
}

run()函数则是实现了具体的控制器功能，该函数应该以100Hz及以上频率被调用。

run()内部首先调用update_simple_mode()这个函数是进行通道的转换，目前理解不够深刻。

// update_simple_mode - rotates pilot input if we are in simple mode
void Copter::update_simple_mode(void)
{
    float rollx, pitchx;

    // exit immediately if no new radio frame or not in simple mode
    if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
        return;
    }

    // mark radio frame as consumed
    ap.new_radio_frame = false;

    if (simple_mode == SimpleMode::SIMPLE) {
        // rotate roll, pitch input by -initial simple heading (i.e. north facing)
        rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
        pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
    }else{
        // rotate roll, pitch input by -super simple heading (reverse of heading to home)
        rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
        pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
    }

    // rotate roll, pitch input from north facing to vehicle's perspective
    channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
    channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
}

接下来get_pilot_desired_lean_angles()用于将打杆量映射成角度。

// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
{
    // throttle failsafe check
    if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
        roll_out = 0;
        pitch_out = 0;
        return;
    }
    // fetch roll and pitch inputs
    roll_out = channel_roll->get_control_in();
    pitch_out = channel_pitch->get_control_in();

    // limit max lean angle
    angle_limit = constrain_float(angle_limit, 1000.0f, angle_max);

    // scale roll and pitch inputs to ANGLE_MAX parameter range
    float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
    roll_out *= scaler;
    pitch_out *= scaler;

    // do circular limit
    float total_in = norm(pitch_out, roll_out);
    if (total_in > angle_limit) {
        float ratio = angle_limit / total_in;
        roll_out *= ratio;
        pitch_out *= ratio;
    }

    // do lateral tilt to euler roll conversion
    roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));

    // roll_out and pitch_out are returned
}

在执行完上述两部分内容后，首先判断电机是否已经准备就绪，如果还没有，就进入if中的代码段
- 首先是通过set_desired_spool_state()函数设置为期望的轴状态，此处设置为GROUOND_IDLE，即将所有电机闲置（lobraries/AP_Motors/AP_Motors_Class.h中SpoolState声明了3种轴状态：SHUT_DOWN关闭、GROUND_IDLE空闲和THROTTLE_UNLIMITED取消限制）。
如果电机已经准备好了，那么就不进入if()的代码段中，首先设置轴状态为取消限制。get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)函数将获取roll和pitch的输入量然后将其转换为对应的以度单位表示的角度输出传输给在run()最开始设置的target_roll和target_pitch。get_pilot_desired_yaw_rate()函数将获取输入的yaw期望并且将其转换为°/s为单位yaw角速度输出并保存在target_yaw_rate中。
然后进入姿态控制器，更新姿态至期望值。
2.3.3 AC_AttitudeControl.cpp

最后来看一下在libraries库中AC_AttitudeControl下AC_AttitudeControl.cpp中的input_euler_angle_roll_pitch_euler_rate_yaw()方法，其实大家看源码也能看出大概的意思了。

这边也简单说一下，函数内部先把相应的角度转换为弧度制，然后计算出对于的期望欧拉角，并在欧拉角的roll向上增加侧倾调整以补偿推力，然后判断是否开启了前馈。是的话，还需要进行一系列计算，例如将摇杆期望欧拉角与前馈目标欧拉角进行差值计算以得到前馈角速度，还有将目标欧拉角速度转换为机体角速度等等；如果没有开启前馈，那么就直接将欧拉角输入到目标中，并根据目标欧拉角转换得出目标四元数，然后前馈率归零。最后调用四元数姿态控制器。

总的来说，这个函数的功能就是通过角速度前馈和平滑来控制欧拉角和俯仰角以及欧拉偏航率。对于前面mode_stabilize.cpp中所用的input_euler_angle_roll_pitch_euler_rate_yaw则是通过角速度前馈和平滑控制欧拉角，俯仰角和偏航角。如果有兴趣继续研究，详看官方的手册。

void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
    // Convert from centidegrees on public interface to radians
    float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
    float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
    float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);

    // calculate the attitude target euler angles
    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    euler_roll_angle += get_roll_trim_rad();

    if (_rate_bf_ff_enabled) {
        // translate the roll pitch and yaw acceleration limits to the euler axis
        Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));

        // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
        // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
        // and an exponential decay specified by smoothing_gain at the end.
        _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
        _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);

        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        _attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
        euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
        // Limit the angular velocity
        ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
        // Convert body-frame angular velocity into euler angle derivative of desired attitude
        ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
    } else {
        // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
        _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);

        // Set rate feedforward requests to zero
        _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
        _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
    }

    // Call quaternion attitude controller
    attitude_controller_run_quat();
}

2.4 其他

无。

三、总结

以上便是本次的全部内容，下一篇打算讲讲Copter内部的底层电机控制原理。

~~第一次更新：2020/12/1~~

~~第二次更新：2020/12/2~~

zhangfengmei1987

关注

7
点赞
踩
50

收藏

觉得还不错? 一键收藏
3
评论
APM_ArduCopter源码解析学习（一）——ArduCopter主程序

<link rel="stylesheet" href="https://csdnimg.cn/release/blogv2/dist/mdeditor/css/editerView/ck_htmledit_views-b5506197d8.css"> <div id="content_views" class="markdown_views prism-atom-one-dark"> <svg xmlns="ht...
复制链接

扫一扫