ArduSub程序学习（2）--ArduSub主程序讲解

仗剑走代码

于 2024-07-29 15:18:02 发布

阅读量427

点赞数 21

分类专栏： ardusub ardupilot 文章标签：学习 autopilot c++

本文链接：https://blog.csdn.net/weixin_49839886/article/details/140725282

版权

ardusub 同时被 2 个专栏收录

5 篇文章 0 订阅

订阅专栏

ardupilot

5 篇文章 0 订阅

订阅专栏

前言

本次对于ardusub主程序的讲解以sub4.1版本为主，每个版本的程序会有些差距，在讲解的过程中我会对比不同版本的进行学习，大家可以在github里面直接观看程序学习。ardupilot/ArduSub at Sub-4.1 · ArduPilot/ardupilot (github.com)

ardusub.cpp

首先打开ArduPilot/ArduSub/ArduSub.cpp，开头是#include "Sub.h"一个包含头文件的处理，

然后#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)是一个关于任务调度函数的宏定义。

1.头文件包含和宏定义

#include "Sub.h"//包含Sub.h文件

#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)
//定义了一个名为 SCHED_TASK 的宏。这个宏接受五个参数：func（函数名）、rate_hz（调用频率）、max_time_micros（最大执行时间）它使用了另一个名为 SCHED_TASK_CLASS 的宏来展开，后面会解释。

2.调度任务表格：

接下来开始进行任务调度，这是一个 scheduler_tasks 数组的定义，它是 AP_Scheduler::Task 类型的数组。数组中的每个元素都是一个调度任务，每个任务由 SCHED_TASK 宏展开成相应的参数。关于任务调度方面的学习，大家可以去了解一下freertos，能快速的学习关于一些嵌入式的知识。

const AP_Scheduler::Task Sub::scheduler_tasks[] = {
    SCHED_TASK(fifty_hz_loop,         50,     75),
    SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200),
    // 更多任务项...
    SCHED_TASK(read_airspeed,          10,    100),
};

3.SCHED_TASK_CLASS 宏：

SCHED_TASK 宏将任务的函数名、调用频率和最大执行时间传递给 SCHED_TASK_CLASS 宏。根据代码的上下文推测，SCHED_TASK_CLASS 宏可能是一个用于将任务与特定类的成员函数绑定的宏。例如，可能的实现是类似这样的：

#define SCHED_TASK_CLASS(cls, obj, func, rate_hz, max_time_micros) \
    { &cls::func, obj, rate_hz, max_time_micros }

这里的 SCHED_TASK_CLASS 宏会返回一个结构体或类的实例，表示一个调度任务，包括函数指针、对象指针、调用频率和最大执行时间。

4.任务定义：

在 scheduler_tasks 数组中，每个 SCHED_TASK 宏都对应一个具体的任务。例如：

SCHED_TASK(fifty_hz_loop, 50, 75)

表示一个名为 fifty_hz_loop 的函数，调用频率为 50Hz，最大执行时间为 75微秒。

5.条件编译

代码中还包含了一些条件编译的部分（例如 #if 和 #ifdef），这些部分根据预定义的宏（如 OPTFLOW, AC_FENCE, CAMERA 等）来决定是否包含某些特定的调度任务。

6.获取任务调度信息

void Sub::get_scheduler_tasks这段代码不一样的版本会有所不同，有的(sub3.5)上来直接是void Sub::setup()一个初始化函数。

void Sub::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,//tasks 是一个指向 AP_Scheduler::Task 类型数组的常量指针引用。该函数通过这个参数返回调度任务数组的首地址。
                              uint8_t &task_count,//task_count 是一个 uint8_t 类型的引用，用来返回调度任务数组中的任务数量
                              uint32_t &log_bit)//log_bit 是一个 uint32_t 类型的引用，用来返回日志位掩码的值。
{
    tasks = &scheduler_tasks[0];//tasks = &scheduler_tasks[0]; 将 tasks 设置为指向 scheduler_tasks 数组的第一个元素的地址。
    task_count = ARRAY_SIZE(scheduler_tasks);//将 task_count 设置为 scheduler_tasks 数组的元素个数。
    log_bit = MASK_LOG_PM;//log_bit = MASK_LOG_PM; 将 log_bit 设置为 MASK_LOG_PM 的值。
}

7.fast_loop()

这段代码的作用是在主循环中以400Hz的频率执行各种任务和控制器，包括更新惯性导航系统、执行姿态控制、发送电机输出、运行状态估计器（EKF）、处理惯性导航数据、更新航向控制等。

里面很多函数是比较重要的，建议大家去跳转到函数定义去阅读以下函数定义。

// Main loop - 400hz
void Sub::fast_loop()
{
   //用于更新惯性导航系统（INS），以获取当前陀螺仪数据。
    ins.update();

   //如果 control_mode 不是手动模式（MANUAL）或电机检测模式（MOTOR_DETECT），则调用姿态控制对象 attitude_control 的 rate_controller_run() 方法来运行低级速率控制器，这些控制器只需使用IMU数据。
    if (control_mode != MANUAL && control_mode != MOTOR_DETECT) {
      
        attitude_control.rate_controller_run();
    }

     //调用 motors_output() 函数，将输出发送到电机库，这可能是根据控制器计算得出的电机输出。
    motors_output();

    //调用 read_AHRS() 函数，用于运行扩展卡尔曼滤波（EKF）状态估计器，这是一个计算量较大的操作，用于估计飞行器的状态。
    read_AHRS();

    //调用 read_inertia() 函数，执行惯性导航相关的处理，可能包括获取加速度计和陀螺仪的数据来计算位置和速度等信息。
    read_inertia();

     //调用 check_ekf_yaw_reset() 函数，检查 EKF 是否重置了目标航向。
    check_ekf_yaw_reset();

    //执行姿态控制器相关的更新。
    update_flight_mode();

    // 根据 EKF 的信息更新家庭位置。
    update_home_from_EKF();

    // 检查是否到达水面或底部。
    update_surface_and_bottom_detector();

#if HAL_MOUNT_ENABLED
    //用于快速更新相机挂载状态。
    camera_mount.update_fast();
#endif

    // 如果需要记录任何传感器日志，调用 Log_Sensor_Health() 函数，记录传感器的健康状态。
    if (should_log(MASK_LOG_ANY)) {
        Log_Sensor_Health();
    }
    //最后，调用基类 AP_Vehicle 的 fast_loop() 方法，执行可能与飞行器或车辆通用的其他处理。
    AP_Vehicle::fast_loop();
}

里面需要注意的是这段代码

    //don't run rate controller in manual or motordetection modes
    if (control_mode != MANUAL && control_mode != MOTOR_DETECT) {
        // run low level rate controllers that only require IMU data
        attitude_control.rate_controller_run();
    }

我们转到函数定义的地方去观察一下libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

void AC_AttitudeControl_Sub::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();

    // 使用角速率PID控制器更新每个轴的控制输出
    _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll));
    _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch));
    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));

    // 更新控制监视器
    control_monitor_update();
}

对于每个轴（横滚、俯仰、偏航），使用相应的角速率PID控制器更新控制输出：
- get_rate_roll_pid().update_all()：使用横滚角速率PID控制器，传入目标角速率、当前陀螺仪测量值和限制范围，更新横滚轴的输出。
- get_rate_pitch_pid().update_all()：使用俯仰角速率PID控制器，传入目标角速率、当前陀螺仪测量值和限制范围，更新俯仰轴的输出。
- get_rate_yaw_pid().update_all()：使用偏航角速率PID控制器，传入目标角速率、当前陀螺仪测量值和限制范围，更新偏航轴的输出。
_motors 可能是控制电机或执行器的对象，通过 set_roll()、set_pitch() 和 set_yaw() 方法设置每个轴的控制输出。
set_roll（）、set_pitch（）、set_yaw（）：接受 -4500 范围内的横滚、俯仰和偏航值 ~ 4500. 这些不是理想的角度或甚至速率，而只是一个值。例如，set_roll（-4500）表示向左滚动为越快越好。

ArduSub.cpp其余函数解释

fifty_hz_loop()
- 作用： 按照50Hz的频率执行一系列任务，主要涉及故障检查和RC输入输出的更新。
- 具体步骤：
  - failsafe_pilot_input_check()：检查飞行员输入的故障。
  - failsafe_crash_check()：检查是否发生了碰撞。
  - failsafe_ekf_check()：检查扩展卡尔曼滤波器（EKF）的状态。
  - failsafe_sensors_check()：检查传感器的状态。
  - rc().read_input()：更新遥控器输入。
  - SRV_Channels::output_ch_all()：更新所有输出通道。
update_batt_compass()
- 作用： 按照10Hz的频率读取电池和罗盘数据。
- 具体步骤：
  - battery.read()：读取电池状态。
  - 如果罗盘被启用 (AP::compass().enabled())，则执行以下操作：
    - compass.set_throttle(motors.get_throttle())：设置罗盘的油门值，用于罗盘电机干扰补偿。
    - compass.read()：读取罗盘数据。
ten_hz_logging_loop()
- 作用： 按照10Hz的频率记录日志数据。
- 具体步骤：
  - 根据日志需求记录姿态数据、PID控制器信息、电机电池数据、遥控器输入输出、位置控制器相关数据以及IMU的振动信息。
twentyfive_hz_logging()
- 作用： 按照25Hz的频率记录日志数据。
- 具体步骤：
  - 根据日志需求记录姿态数据、PID控制器信息以及IMU的数据。
three_hz_loop()
- 作用： 按照3.3Hz的频率执行一系列故障检查和事件更新。
- 具体步骤：
  - 执行泄漏检测、内部压力和温度检查、地面站连接检查、地形数据丢失检查，以及如果启用了电子围栏则执行围栏检查。
one_hz_loop()
- 作用： 按照1Hz的频率执行一系列初始化和状态更新操作。
- 具体步骤：
  - 执行预启动检查、更新姿态传感器方向、更新电机油门范围、启用辅助舵机通道、更新位置控制器的高度限制、记录地形数据等。
read_AHRS()
- 作用： 执行IMU计算并获取姿态信息。
- 具体步骤：
  - 调用 ahrs.update(true) 和 ahrs_view.update(true) 方法来更新姿态信息，参数 true 表示跳过INS更新，因为在 fast_loop() 方法中已经执行了。
update_altitude()
- 作用： 按照10Hz的频率更新高度相关数据。
- 具体步骤：
  - 调用 read_barometer() 读取气压计数据，并根据日志需求记录控制调谐和陀螺仪数据。
control_check_barometer()
- 作用： 检查气压计的状态。
- 具体步骤：
  - 如果没有深度传感器或者传感器健康状态失败，则发送警告信息。
get_wp_distance_m()、get_wp_bearing_deg()、get_wp_crosstrack_error_m()
- 作用： 提供特定于车辆的航点信息的帮助方法。
- 具体步骤：
  - 提供到目标航点的距离、方位角和横跨误差。
AP_HAL_MAIN_CALLBACKS(&sub);
- 作用： 注册主回调函数。
- 具体步骤：
  - 将 &sub 注册为主回调函数，这表明 sub 对象的方法将作为主回调在整个程序执行期间被调用。

flight_mode.cpp

对于fast_loop里面的update_flight_mode();我们转到函数定义去看看ArduSub/flight_mode.cpp

void Sub::update_flight_mode()
{
    switch (control_mode) {
    case ACRO:
        acro_run();
        break;

    case STABILIZE:
        stabilize_run();
        break;

    case ALT_HOLD:
        althold_run();
        break;

    case AUTO:
        auto_run();
        break;

    case CIRCLE:
        circle_run();
        break;

    case GUIDED:
        guided_run();
        break;

    case SURFACE:
        surface_run();
        break;

#if POSHOLD_ENABLED == ENABLED
    case POSHOLD:
        poshold_run();
        break;
#endif

    case MANUAL:
        manual_run();
        break;

    case MOTOR_DETECT:
        motordetect_run();
        break;

    default:
        break;
    }
}

switch-case 结构：
- switch (control_mode) 开始了一个根据 control_mode 变量值的多分支选择结构。
不同的飞行模式：
- 每个 case 分支代表了一个可能的飞行模式，例如 ACRO、STABILIZE、ALT_HOLD 等。
- 每个分支中调用对应的 *_run() 方法来执行特定飞行模式的逻辑。
- 下面是每个飞行模式对应的处理函数示例：
  - acro_run()：执行Acro（特技）模式的逻辑。
  - stabilize_run()：执行Stabilize（稳定）模式的逻辑。
  - althold_run()：执行Altitude Hold（高度保持）模式的逻辑。
  - auto_run()：执行Auto（自动）模式的逻辑。
  - circle_run()：执行Circle（环绕）模式的逻辑。
  - guided_run()：执行Guided（引导）模式的逻辑。
  - surface_run()：执行Surface（上升）模式的逻辑。
  - poshold_run()：执行PosHold（位置保持）模式的逻辑（如果启用）。
  - manual_run()：执行Manual（手动）模式的逻辑。
  - motordetect_run()：执行Motor Detect（电机检测）模式的逻辑。
default 分支：
- 如果 control_mode 的值不在上述列举的任何一个飞行模式中，则执行 default: 分支。
- 默认情况下，update_flight_mode() 方法不执行任何操作，直接跳出 switch-case 结构。

各个模式宏定义于defines.h中

// Auto Pilot Modes enumeration
enum control_mode_t : uint8_t {
    STABILIZE =     0,  // manual angle with manual depth/throttle
    ACRO =          1,  // manual body-frame angular rate with manual depth/throttle
    ALT_HOLD =      2,  // manual angle with automatic depth/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
    CIRCLE =        7,  // automatic circular flight with automatic throttle
    SURFACE =       9,  // automatically return to surface, pilot maintains horizontal control
    POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
    MANUAL =       19,  // Pass-through input with no stabilization
    MOTOR_DETECT = 20   // Automatically detect motors orientation
};