网站导航
> 技术文档 > ArduSub.cpp 水下机器人的核心任务调度与主循环实现

ArduSub.cpp 水下机器人的核心任务调度与主循环实现

网友: 技术文档 2025-07-26 19:45:32 
/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */// ArduSub scheduling, originally copied from ArduCopter#include \"Sub.h\"#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)/* scheduler table for fast CPUs - all regular tasks apart from the fast_loop() should be listed here, along with how often they should be called (in hz) and the maximum time they are expected to take (in microseconds) */const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(fifty_hz_loop, 50, 75), SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200),#if OPTFLOW == ENABLED SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160),#endif SCHED_TASK(update_batt_compass, 10, 120), SCHED_TASK(read_rangefinder, 20, 100), SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(update_turn_counter, 10, 50), SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90), SCHED_TASK_CLASS(AP_Notify,  &sub.notify, update,  50, 90), SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK_CLASS(GCS,  (GCS*)&sub._gcs, update_receive, 400, 180), SCHED_TASK_CLASS(GCS,  (GCS*)&sub._gcs, update_send, 400, 550),#if AC_FENCE == ENABLED SCHED_TASK_CLASS(AC_Fence, &sub.fence, update,  10, 100),#endif#if HAL_MOUNT_ENABLED SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update,  50, 75),#endif#if CAMERA == ENABLED SCHED_TASK_CLASS(AP_Camera,  &sub.camera, update,  50, 75),#endif SCHED_TASK(ten_hz_logging_loop, 10, 350), SCHED_TASK(twentyfive_hz_logging, 25, 110), SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300), SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic,  400, 50), SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75),#if RPM_ENABLED == ENABLED SCHED_TASK(rpm_update, 10, 200),#endif SCHED_TASK_CLASS(Compass, &sub.compass,  cal_update, 100, 100), SCHED_TASK(accel_cal_update, 10, 100), SCHED_TASK(terrain_update, 10, 100),#if GRIPPER_ENABLED == ENABLED SCHED_TASK_CLASS(AP_Gripper, &sub.g2.gripper, update,  10, 75),#endif#ifdef USERHOOK_FASTLOOP SCHED_TASK(userhook_FastLoop, 100, 75),#endif#ifdef USERHOOK_50HZLOOP SCHED_TASK(userhook_50Hz, 50, 75),#endif#ifdef USERHOOK_MEDIUMLOOP SCHED_TASK(userhook_MediumLoop, 10, 75),#endif#ifdef USERHOOK_SLOWLOOP SCHED_TASK(userhook_SlowLoop, 3.3, 75),#endif#ifdef USERHOOK_SUPERSLOWLOOP SCHED_TASK(userhook_SuperSlowLoop, 1, 75),#endif SCHED_TASK(read_airspeed, 10, 100),};void Sub::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;}constexpr int8_t Sub::_failsafe_priorities[5];// Main loop - 400hzvoid Sub::fast_loop(){ // update INS immediately to get current gyro data populated ins.update(); //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(); } // send outputs to the motors library motors_output(); // run EKF state estimator (expensive) // -------------------- read_AHRS(); // Inertial Nav // -------------------- read_inertia(); // check if ekf has reset target heading check_ekf_yaw_reset(); // run the attitude controllers update_flight_mode(); // update home from EKF if necessary update_home_from_EKF(); // check if we\'ve reached the surface or bottom update_surface_and_bottom_detector();#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();}// 50 Hz tasksvoid Sub::fifty_hz_loop(){ // check pilot input failsafe failsafe_pilot_input_check(); failsafe_crash_check(); failsafe_ekf_check(); failsafe_sensors_check(); // Update rc input/output rc().read_input(); SRV_Channels::output_ch_all();}// update_batt_compass - read battery and compass// should be called at 10hzvoid Sub::update_batt_compass(){ // AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); velocity_dvl_raw = ahrs.velocity_dvl_raw; // read battery before compass because it may be used for motor interference compensation battery.read(); if (AP::compass().enabled()) { // update compass with throttle value - used for compassmot compass.set_throttle(motors.get_throttle()); compass.read(); }}// ten_hz_logging_loop// should be run at 10hzvoid Sub::ten_hz_logging_loop(){ // log attitude data if we\'re not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); ahrs_view.Write_Rate(motors, attitude_control, pos_control); if (should_log(MASK_LOG_PID)) { logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info()); } } if (should_log(MASK_LOG_MOTBATT)) { Log_Write_MotBatt(); } if (should_log(MASK_LOG_RCIN)) { logger.Write_RCIN(); } if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); } if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || !mode_has_manual_throttle(control_mode))) { pos_control.write_log(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { AP::ins().Write_Vibration(); } if (should_log(MASK_LOG_CTUN)) { attitude_control.control_monitor_log(); }}// twentyfive_hz_logging_loop// should be run at 25hzvoid Sub::twentyfive_hz_logging(){ if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); ahrs_view.Write_Rate(motors, attitude_control, pos_control); if (should_log(MASK_LOG_PID)) { logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info()); } } // log IMU data if we\'re not already logging at the higher rate if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) { AP::ins().Write_IMU(); }}// three_hz_loop - 3.3hz loopvoid Sub::three_hz_loop(){ leak_detector.update(); failsafe_leak_check(); failsafe_internal_pressure_check(); failsafe_internal_temperature_check(); // check if we\'ve lost contact with the ground station failsafe_gcs_check(); // check if we\'ve lost terrain data failsafe_terrain_check();#if AC_FENCE == ENABLED // check if we have breached a fence fence_check();#endif // AC_FENCE_ENABLED ServoRelayEvents.update_events();}// one_hz_loop - runs at 1Hzvoid Sub::one_hz_loop(){  if(send_cnt==10) { send_cnt = 0; float vx_cm = inertial_nav.get_velocity().x; float vy_cm = inertial_nav.get_velocity().y; gcs().send_text(MAV_SEVERITY_INFO,\"vx cm in world is %f, vy %f\",vx_cm,vy_cm); gcs().send_text(MAV_SEVERITY_INFO,\"DVL raw %f, y %f, z %f\",velocity_dvl_raw.x,velocity_dvl_raw.y,velocity_dvl_raw.z); gcs().send_text(MAV_SEVERITY_INFO,\"X %f Y %f Z %f R %f P %f Y %f \",motors.get_forward(),motors.get_lateral(),motors.get_throttle(),motors.get_roll(),motors.get_pitch(),motors.get_yaw()); } send_cnt++; bool arm_check = arming.pre_arm_checks(false); ap.pre_arm_check = arm_check; AP_Notify::flags.pre_arm_check = arm_check; AP_Notify::flags.pre_arm_gps_check = position_ok(); AP_Notify::flags.flying = motors.armed(); if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::AP_STATE, ap.value); } if (!motors.armed()) { // make it possible to change ahrs orientation at runtime during initial config ahrs.update_orientation(); motors.update_throttle_range(); } // update assigned functions and enable auxiliary servos SRV_Channels::enable_aux_servos(); // update position controller alt limits update_poscon_alt_max(); // log terrain data terrain_logging(); // need to set \"likely flying\" when armed to allow for compass // learning to run set_likely_flying(hal.util->get_soft_armed());}void Sub::read_AHRS(){ // Perform IMU calculations and get attitude info //----------------------------------------------- //  tells AHRS to skip INS update as we have already done it in fast_loop() ahrs.update(true); ahrs_view.update(true);}// read baro and rangefinder altitude at 10hzvoid Sub::update_altitude(){ // read in baro altitude read_barometer(); if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning();#if HAL_GYROFFT_ENABLED gyro_fft.write_log_messages();#else write_notch_log_messages();#endif }}bool Sub::control_check_barometer(){#if CONFIG_HAL_BOARD != HAL_BOARD_SITL if (!ap.depth_sensor_present) { // can\'t hold depth without a depth sensor gcs().send_text(MAV_SEVERITY_WARNING, \"Depth sensor is not connected.\"); return false; } else if (failsafe.sensor_health) { gcs().send_text(MAV_SEVERITY_WARNING, \"Depth sensor error.\"); return false; }#endif return true;}// vehicle specific waypoint info helpersbool Sub::get_wp_distance_m(float &distance) const{ // see GCS_MAVLINK_Sub::send_nav_controller_output() distance = sub.wp_nav.get_wp_distance_to_destination() * 0.01; return true;}// vehicle specific waypoint info helpersbool Sub::get_wp_bearing_deg(float &bearing) const{ // see GCS_MAVLINK_Sub::send_nav_controller_output() bearing = sub.wp_nav.get_wp_bearing_to_destination() * 0.01; return true;}// vehicle specific waypoint info helpersbool Sub::get_wp_crosstrack_error_m(float &xtrack_error) const{ // no crosstrack error reported, see GCS_MAVLINK_Sub::send_nav_controller_output() xtrack_error = 0; return true;}AP_HAL_MAIN_CALLBACKS(&sub);

这段代码是ArduSub水下机器人的核心任务调度与主循环实现，负责管理系统中所有周期性任务的执行频率、优先级和逻辑，是连接传感器数据处理、控制算法和硬件输出的关键环节。以下是详细解释：

一、宏定义

#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)
  • 宏SCHED_TASK用于简化任务注册，通过封装SCHED_TASK_CLASS，将Sub类的成员函数注册为调度任务，参数包括：任务函数、执行频率（Hz）、最大允许执行时间（微秒）。
  • 作用：统一任务注册格式，降低代码冗余，便于维护。

二、任务调度表（scheduler_tasks）

这是系统的“任务清单”，定义了所有周期性任务的执行参数，是调度器的核心配置。每个任务通过SCHED_TASK或SCHED_TASK_CLASS注册，格式为：
SCHED_TASK(函数名, 频率Hz, 最大耗时微秒) 或
SCHED_TASK_CLASS(类名, 实例指针, 成员函数, 频率Hz, 最大耗时微秒)

  1. SCHED_TASK(fifty_hz_loop, 50, 75)

    • 任务：50Hz主循环（fifty_hz_loop具体内容后续有实现函数），包含故障检测、RC遥控器输入处理、电机输出控制等核心逻辑。
    • 频率：50Hz（每秒50次），兼顾实时性与资源消耗。
    • 最大耗时：75微秒，确保任务快速完成，不阻塞高频任务。

  2. SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200)

    • 任务：GPS模块数据更新（AP_GPS类的update方法），读取卫星定位信息（经纬度、速度等）。
    • 频率：50Hz，高频更新以保证水下导航精度。
    • 最大耗时：200微秒，GPS数据解析需一定时间（含串口通信延迟）。

  3. #if OPTFLOW == ENABLED 条件块

    • 仅当光流传感器功能启用（OPTFLOW == ENABLED）时编译。
    • 任务：OpticalFlow类的update方法，更新光流传感器数据（用于水下视觉定位）。
    • 频率：200Hz，光流依赖高频数据实现精准定位，需更高频率。
    • 最大耗时：160微秒，视觉数据处理较耗时但需严格控制。

  4. SCHED_TASK(update_batt_compass, 10, 120)

    • 任务：update_batt_compass函数，更新电池状态（电压、电流）和罗盘数据（磁场强度）。
    • 频率：10Hz，电池和罗盘数据变化较慢，无需高频更新。
    • 最大耗时：120微秒，包含传感器通信和数据校准。

  5. SCHED_TASK(read_rangefinder, 20, 100)

    • 任务：读取测距仪数据（如水下障碍物距离）。
    • 频率：20Hz，适配多数测距仪（如超声波、激光）的响应速度。
    • 最大耗时：100微秒，测距数据解析较简单。

  6. SCHED_TASK(update_altitude, 10, 100)

    • 任务：计算当前深度/高度（基于气压计或深度传感器数据）。
    • 频率：10Hz，深度控制对实时性要求中等，10Hz足够稳定。
    • 最大耗时：100微秒，含压力数据到深度的转换计算。

  7. SCHED_TASK(three_hz_loop, 3, 75)

    • 任务：3Hz低优先级循环（three_hz_loop），处理漏水检测、地面站通信超时检查、电子围栏状态更新等非紧急逻辑。
    • 频率：3Hz（约每333毫秒一次），低优先级任务无需高频。
    • 最大耗时：75微秒，轻量化设计。

  8. SCHED_TASK(update_turn_counter, 10, 50)

    • 任务：更新转向计数器（记录航行器累计转向角度），用于航点导航中的转向控制。
    • 频率：10Hz，转向角度变化较慢，10Hz足够记录。
    • 最大耗时：50微秒，仅简单累加计算。

  9. SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90)

    • 任务：气压计（或深度传感器）数据积累（AP_Baro类的accumulate方法），收集原始压力值用于后续滤波和深度计算。
    • 频率：50Hz，压力数据需高频积累以降低噪声。
    • 最大耗时：90微秒，仅数据缓存，不涉及复杂计算。

  10. SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90)

    • 任务：通知系统更新（AP_Notify类的update方法），控制指示灯、蜂鸣器等硬件（如“低电量报警”“预臂检查通过”）。
    • 频率：50Hz，确保状态提示及时刷新。
    • 最大耗时：90微秒，硬件控制逻辑简单。

  11. SCHED_TASK(one_hz_loop, 1, 100)

    • 任务：1Hz主循环（one_hz_loop），处理预臂检查（如传感器是否正常）、系统状态日志、低优先级调试信息发送（如代码中“每10秒发送速度数据到地面站”）。
    • 频率：1Hz（每秒1次），低优先级任务，不占用高频资源。
    • 最大耗时：100微秒，避免影响核心任务。

  12. SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180)

    • 任务：地面站（GCS）数据接收（GCS类的update_receive方法），解析地面站发送的指令（如“定深”“返航”）。
    • 频率：400Hz，高频接收确保指令无延迟（如紧急停车指令需立即响应）。
    • 最大耗时：180微秒，串口数据解析需快速完成。

  13. SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550)

    • 任务：地面站数据发送（GCS类的update_send方法），向地面站上传航行器状态（位置、深度、电池等）。
    • 频率：400Hz，高频上传确保地面站实时监控。
    • 最大耗时：550微秒，数据打包（如MAVLink协议封装）较耗时，允许更长时间。

  14. #if AC_FENCE == ENABLED 条件块

    • 仅当电子围栏功能启用（AC_FENCE == ENABLED）时编译。
    • 任务：AC_Fence类的update方法，检查航行器是否超出预设安全区域（如“禁止进入某区域”）。
    • 频率：10Hz，围栏检查无需高频，10Hz足够及时响应。
    • 最大耗时：100微秒，区域判断逻辑简单。

  15. #if HAL_MOUNT_ENABLED 条件块

    • 仅当相机挂载功能启用（HAL_MOUNT_ENABLED）时编译。
    • 任务：AP_Mount类的update方法，控制相机云台角度（如“追踪目标”“固定朝向”）。
    • 频率：50Hz，云台控制需中等频率保证平滑转动。
    • 最大耗时：75微秒，云台电机控制逻辑简单。

  16. #if CAMERA == ENABLED 条件块

    • 仅当相机控制功能启用（CAMERA == ENABLED）时编译。
    • 任务：AP_Camera类的update方法，处理相机拍照、录像指令（如“定时拍照”）。
    • 频率：50Hz，相机指令响应无需过高频率。
    • 最大耗时：75微秒，指令执行逻辑简单。

  17. SCHED_TASK(ten_hz_logging_loop, 10, 350)

    • 任务：10Hz日志记录（ten_hz_logging_loop），记录姿态、PID参数、电机输出等中等频率数据。
    • 频率：10Hz，平衡日志完整性与存储消耗。
    • 最大耗时：350微秒，日志写入需一定时间（含SD卡IO操作）。

  18. SCHED_TASK(twentyfive_hz_logging, 25, 110)

    • 任务：25Hz高频日志记录，记录IMU原始数据、光流数据等高频变化量。
    • 频率：25Hz，适应高频传感器数据的记录需求。
    • 最大耗时：110微秒，轻量化日志（仅关键数据）。

  19. SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300)

    • 任务：日志系统周期任务（AP_Logger类的periodic_tasks），处理日志文件管理（如“新建日志文件”“刷新缓存到SD卡”）。
    • 频率：400Hz，确保日志缓存及时写入，避免数据丢失。
    • 最大耗时：300微秒，文件操作需一定时间。

  20. SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50)

    • 任务：惯性测量单元（IMU）周期更新（AP_InertialSensor类的periodic），读取加速度计、陀螺仪原始数据（用于姿态解算）。
    • 频率：400Hz，IMU是姿态控制的核心传感器，需最高频更新。
    • 最大耗时：50微秒，传感器数据读取速度快（直接从硬件寄存器获取）。

  21. SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75)

    • 任务：调度器自身的日志更新（AP_Scheduler类的update_logging），记录任务执行时间、CPU负载等调度器状态。
    • 频率：0.1Hz（每10秒1次），低优先级，无需高频。
    • 最大耗时：75微秒，仅简单统计数据。

  22. #if RPM_ENABLED == ENABLED 条件块

    • 仅当电机转速检测功能启用（RPM_ENABLED == ENABLED）时编译。
    • 任务：rpm_update函数，读取电机转速传感器数据（用于监控推进器状态）。
    • 频率：10Hz，转速变化较慢，10Hz足够。
    • 最大耗时：200微秒，含传感器通信延迟。

  23. SCHED_TASK_CLASS(Compass, &sub.compass, cal_update, 100, 100)

    • 任务：罗盘校准更新（Compass类的cal_update），动态优化罗盘测量值（如“消除磁场干扰”）。
    • 频率：100Hz，校准需高频数据以快速收敛。
    • 最大耗时：100微秒，校准算法（如最小二乘法）需一定计算量。

  24. SCHED_TASK(accel_cal_update, 10, 100)

    • 任务：加速度计校准更新（accel_cal_update），优化加速度计测量值（如“零偏校准”）。
    • 频率：10Hz，校准无需高频，10Hz足够稳定。
    • 最大耗时：100微秒，校准逻辑较简单。

  25. SCHED_TASK(terrain_update, 10, 100)

    • 任务：地形数据更新（terrain_update），读取/缓存水下地形高度数据（用于避障或路径规划）。
    • 频率：10Hz，地形变化缓慢，10Hz足够。
    • 最大耗时：100微秒，地形数据多为预加载，更新逻辑简单。

  26. #if GRIPPER_ENABLED == ENABLED 条件块

    • 仅当机械爪功能启用（GRIPPER_ENABLED == ENABLED）时编译。
    • 任务：AP_Gripper类的update方法，控制机械爪开合（如“抓取物体”“释放”）。
    • 频率：10Hz，机械爪动作缓慢，无需高频。
    • 最大耗时：75微秒，电机控制逻辑简单。

  27. #ifdef USERHOOK_FASTLOOP 条件块

    • 仅当用户定义了USERHOOK_FASTLOOP宏时编译，允许用户添加自定义高频任务（如“自定义传感器数据处理”）。
    • 任务：userhook_FastLoop（用户自定义函数）。
    • 频率：100Hz，适配快速响应的自定义需求。
    • 最大耗时：75微秒，限制用户任务耗时，避免影响系统。

  28. #ifdef USERHOOK_50HZLOOP 条件块

    • 仅当用户定义了USERHOOK_50HZLOOP宏时编译，用户自定义50Hz任务（如“中等频率控制逻辑”）。

  29. #ifdef USERHOOK_MEDIUMLOOP 条件块

    • 仅当用户定义了USERHOOK_MEDIUMLOOP宏时编译，用户自定义10Hz任务（如“低优先级状态监控”）。

  30. #ifdef USERHOOK_SLOWLOOP 条件块

    • 仅当用户定义了USERHOOK_SLOWLOOP宏时编译，用户自定义3.3Hz任务（如“周期性调试信息输出”）。

  31. #ifdef USERHOOK_SUPERSLOWLOOP 条件块

    • 仅当用户定义了USERHOOK_SUPERSLOWLOOP宏时编译，用户自定义1Hz任务（如“系统自检”）。

  32. SCHED_TASK(read_airspeed, 10, 100)

    • 任务：空速传感器数据读取（read_airspeed），获取水下航行器相对水流的速度（辅助导航）。
    • 频率：10Hz，空速变化较慢，10Hz足够。
    • 最大耗时：100微秒，传感器数据解析简单。

核心设计逻辑：

  1. 频率分层：高频任务（200-400Hz）用于实时控制（如IMU、地面站通信），中频（10-50Hz）用于传感器更新（如GPS、深度），低频（1-3Hz）用于低优先级操作（如日志、自检）。
  2. 耗时控制：每个任务的“最大耗时”严格限制，防止单个任务占用过多CPU时间导致系统卡顿。
  3. 模块化：通过条件编译（#if ...）仅启用配置的功能（如光流、机械爪），减少冗余代码。
  4. 可扩展性：USERHOOK_* 允许在不修改核心代码的情况下添加自定义任务，适配个性化需求。

通过这套调度表，ArduSub实现了在有限CPU资源下的高效运行，确保水下航行器的实时控制、传感器数据处理和地面站通信等功能稳定协同。

三、调度器接口（get_scheduler_tasks）

1. get_scheduler_tasks() 函数
void Sub::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;}
  • 这是调度器的配置接口，用于向AP_Scheduler提供任务列表、任务数量和日志掩码。
  • 调度器通过此函数获取所有需要执行的任务信息，从而按频率和优先级调度执行。
  • tasks：输出参数，指向任务数组的指针（即scheduler_tasks数组）。
  • task_count：输出参数，任务数组的大小（通过ARRAY_SIZE宏计算）。
  • log_bit：输出参数，日志掩码（MASK_LOG_PM），用于指定需要记录的任务类型。

执行流程

  1. tasks = &scheduler_tasks[0]：将任务数组的首地址赋给输出参数，使调度器能访问所有任务。
  2. task_count = ARRAY_SIZE(scheduler_tasks)：计算任务数组的元素个数，告诉调度器有多少个任务需要管理。
  3. log_bit = MASK_LOG_PM：设置日志掩码，指定记录哪些类型的任务（MASK_LOG_PM可能表示记录所有动力系统相关任务）。

设计意图

  • 解耦调度器与具体任务：调度器只需通过此接口获取任务信息，无需关心任务的具体实现。
  • 配置灵活性：通过修改log_bit可动态调整日志记录的粒度，便于调试和分析。
2. _failsafe_priorities 数组
constexpr int8_t Sub::_failsafe_priorities[5];

  • 定义故障安全机制的优先级列表，用于在系统出现异常时决定哪些故障需要优先处理。

  • constexpr表示该数组在编译时初始化，提高运行效率。

  • int8_t：每个优先级用8位整数表示（范围-128~127），数值越小优先级越高。

  • [5]：数组大小为5，对应5种不同类型的故障。

优先级数组定义为：

static constexpr int8_t _failsafe_priorities[] = { Failsafe_Action_Disarm, Failsafe_Action_Surface, Failsafe_Action_Warn, Failsafe_Action_None, -1 // the priority list must end with a sentinel of -1 };
元素 含义 适用场景 Failsafe_Action_Disarm 解除武装（关闭所有推进器电机） 最紧急的故障（如致命错误、严重漏水），强制停止动力以避免危险 Failsafe_Action_Surface 上浮至水面 中等紧急故障（如通信中断、传感器失效），让航行器回到水面便于回收 Failsafe_Action_Warn 发出警告（如指示灯闪烁、地面站报警） 轻微故障（如电池电量偏低、GPS信号弱），仅提示用户不主动干预 Failsafe_Action_None 不采取动作 极轻微故障（如非关键传感器临时异常），系统自行恢复无需处理 -1 哨兵值（数组结束标记） 用于遍历数组时判断边界，避免越界访问

当系统同时检测到多种故障时（例如 “电池低电量” 和 “GPS 信号丢失” 同时发生），系统会通过该数组确定处理顺序：优先执行优先级更高的动作（数组中位置更靠前的元素）。若高优先级动作已执行（如已触发 “上浮至水面”），低优先级动作可能被忽略或延迟处理。

四、主循环与周期性任务

1. 最高频循环（fast_loop，400Hz）

Sub::fast_loop() 是一个以 400Hz频率运行 的主循环（即每秒执行400次，周期2.5毫秒），专门处理对实时性要求极高的任务——这些任务若延迟会直接影响水下机器人的控制精度（如姿态抖动、定深偏差）。

  1. ins.update();
// update INS immediately to get current gyro data populatedins.update();
  • 功能：立即更新惯性导航系统（INS，Inertial Navigation System），获取最新的陀螺仪、加速度计原始数据。
  • 必要性：INS是姿态控制的“眼睛”，400Hz的高频更新确保后续控制算法使用的是当前时刻的惯性数据（而非几毫秒前的旧数据），避免控制滞后。
  • 细节：INS数据包括角速度（用于姿态角速度控制）、线加速度（用于速度/位置估计），是EKF（扩展卡尔曼滤波器）数据融合的核心输入。
  1. 姿态速率控制器（条件执行）
// don\'t run rate controller in manual or motordetection modesif (control_mode != MANUAL && control_mode != MOTOR_DETECT) { // run low level rate controllers that only require IMU data attitude_control.rate_controller_run();}
  • 功能：运行低级别姿态速率控制器（如“角速度闭环控制”），根据目标角速度（如用户指令或自主导航需求）和当前IMU测量的角速度，计算出电机的控制量。
  • 条件限制：
    • 手动模式（MANUAL）：用户直接通过遥控器控制电机输出，无需自动速率控制。
    • 电机检测模式（MOTOR_DETECT）：系统在校准电机，此时不启用控制算法。
  • 为什么放400Hz循环：
    姿态速率控制对实时性最敏感——若控制周期超过5毫秒，可能导致“震荡”（比如目标角速度10°/s，实际因延迟变成15°/s，系统反复修正）。400Hz的高频可将控制延迟压缩到2.5毫秒内，保证稳定性。
  1. motors_output();
// send outputs to the motors librarymotors_output();
  • 功能：将姿态控制器计算出的电机控制量（如PWM信号或推力指令）发送到电子调速器（ESC），驱动推进器转动。
  • 关键作用：这是“控制指令→物理动作”的最后一步，必须高频执行——若延迟，会导致“控制滞后”（比如指令要求加速，但电机延迟响应，导致位置偏差）。
  1. read_AHRS();
// run EKF state estimator (expensive)// --------------------read_AHRS();
  • 功能：更新AHRS（姿态航向参考系统，Attitude and Heading Reference System），核心是运行EKF（扩展卡尔曼滤波器）进行传感器数据融合。
  • EKF的作用：融合IMU、GPS、罗盘、深度传感器等数据，输出当前的姿态（横滚、俯仰、偏航）、位置、速度等关键状态量。
  • “expensive”注释：EKF是计算密集型任务（涉及矩阵运算），但必须放在400Hz循环——姿态估计的延迟会直接导致控制算法基于“过时的姿态”计算，引发系统震荡。
  1. read_inertia();
// Inertial Nav// --------------------read_inertia();
  • 功能：处理惯性导航数据，基于EKF输出的位置和速度，更新本地导航状态（如相对于家位置的偏移、当前运动方向）。
  • 作用：为上层控制（如定速、航点导航）提供实时的位置/速度数据，确保导航指令（如“向前移动1米”）能基于最新状态计算。
  1. check_ekf_yaw_reset();
// check if ekf has reset target headingcheck_ekf_yaw_reset();
  • 功能：检查EKF是否重置了“目标航向”（偏航角参考）。EKF可能在初始化、传感器异常时重置航向。
  • 必要性：航向突变会导致控制算法的目标航向突然变化，若不处理，可能引发“偏航震荡”（比如EKF重置后，机器人突然认为自己“偏航了90°”，进而剧烈修正）。该函数会平滑过渡目标航向，避免突变。
  1. update_flight_mode();
// run the attitude controllersupdate_flight_mode();
  • 功能：根据当前飞行模式（如定深、定点、手动），执行对应的高层姿态控制逻辑。
  • 模式适配：不同模式的控制目标不同（如定深模式优先维持深度，定点模式优先维持位置），高频更新确保模式切换时的平滑过渡（如从手动模式切换到定深模式时，控制逻辑无缝衔接）。
  1. update_home_from_EKF();
// update home from EKF if necessaryupdate_home_from_EKF();
  • 功能：根据EKF的最新估计，更新“家位置”（Home Position，通常是起飞/下潜点）。
  • 场景：家位置是导航的参考原点（如“返回 home”指令），若EKF后续修正了位置（如GPS信号变好后更精确的定位），需同步更新家位置，确保导航准确性。
  1. update_surface_and_bottom_detector();
// check if we\'ve reached the surface or bottomupdate_surface_and_bottom_detector();
  • 功能：检测水下机器人是否到达水面或水底（基于深度传感器、测距仪数据）。
  • 安全意义：水下环境中，碰撞水面或水底可能损坏设备，高频检测能及时触发保护逻辑（如停止下潜、紧急上浮）。
  1. 相机挂载快速更新（条件编译）
#if HAL_MOUNT_ENABLED // camera mount\'s fast update camera_mount.update_fast();#endif
  • 功能：若启用相机挂载（如水下摄像头云台），高频更新其姿态（如跟踪目标、保持水平）。
  • 必要性：相机姿态控制需要与机器人本体姿态同步（比如机器人转弯时，相机需保持对准目标），400Hz更新确保画面平滑无抖动。
  1. 传感器健康日志
// log sensor healthif (should_log(MASK_LOG_ANY)) { Log_Sensor_Health();}
  • 功能：若日志功能启用，记录当前传感器的健康状态（如IMU是否正常、GPS信号强度、深度传感器是否在线）。
  • 作用：高频记录传感器状态，便于后期分析故障（如控制异常时，可回溯传感器是否在同一时间点出现异常）。
  1. AP_Vehicle::fast_loop();
AP_Vehicle::fast_loop();
  • 功能：调用父类（AP_Vehicle）的高频循环，执行通用的高频任务（如底层硬件状态检查、跨平台兼容逻辑）。
  • 设计意义：体现代码复用——不同飞行器（无人机、水下机器人）的高频通用逻辑（如硬件监控）放在父类，子类（Sub）专注于水下特有的逻辑。

核心设计原则

  1. 高频必要性：400Hz的频率确保所有与“姿态控制、执行器输出、传感器融合”相关的任务延迟≤2.5ms，这是水下机器人稳定控制的底线（水下阻力大，延迟易导致超调）。
  2. 任务优先级：仅将“实时性要求最高”的任务放在此循环（如IMU更新、电机输出），计算密集但实时性要求低的任务（如日志、状态检查）要么条件执行，要么放在低频循环。
  3. 数据依赖链：从“传感器数据获取（ins.update()）→状态估计（read_AHRS()）→控制计算（attitude_control）→执行器输出（motors_output()）”形成闭环，每个环节高频衔接，确保控制精度。
2. 50Hz循环（fifty_hz_loop）

ArduSub水下航行器的50Hz任务循环负责处理中等频率的关键安全检查和RC控制逻辑。该循环以50次/秒（周期20毫秒）的频率执行，平衡实时性需求和系统资源消耗。

1. 函数定位与整体功能

// 50 Hz tasksvoid Sub::fifty_hz_loop(){ // ...}
  • 频率选择：50Hz（周期20ms）适合处理对实时性有一定要求，但无需像400Hz循环那样极致高频的任务（如故障检测、RC信号处理）。
  • 核心功能：
    • 多重故障安全检查（确保系统安全运行）。
    • RC遥控器输入输出处理（连接用户与系统）。

2. 故障安全检查（Failsafe Checks）

飞行员输入故障检测

failsafe_pilot_input_check();
  • 功能：检查RC遥控器输入是否丢失或异常（如信号强度低于阈值、通道值超出合理范围）。
  • 安全逻辑：若检测到输入故障，触发相应保护措施（如保持当前姿态、自动返航或紧急上浮）。
  • 频率必要性：50Hz足够快速响应遥控器信号丢失（人类操作的响应延迟通常在100ms以上，20ms周期完全满足需求）。

碰撞检测

failsafe_crash_check();
  • 功能：分析传感器数据（如IMU的加速度突变、深度异常变化），判断是否发生碰撞。
  • 触发条件：例如检测到突然的剧烈加速度（超过正常运动范围），或深度快速变化（可能撞击水面/水底）。
  • 应对措施：紧急关闭电机、切换到安全模式，防止进一步损坏。

EKF状态估计器故障检测

failsafe_ekf_check();
  • 功能：监控EKF（扩展卡尔曼滤波器）的健康状态（如估计误差是否过大、滤波器是否发散）。
  • 重要性：EKF是姿态和位置估计的核心，若其输出不可靠，导航和控制将失效。
  • 触发逻辑：若EKF报告异常（如方差过大、传感器融合失败），触发故障保护（如切换到备用定位方法）。

传感器故障检测

failsafe_sensors_check();
  • 功能：检查所有关键传感器（IMU、GPS、深度计、罗盘等）的工作状态。
  • 检测内容：传感器是否离线、数据是否超出合理范围（如深度计显示负值）、信号是否不稳定。
  • 应对策略：根据故障严重程度，选择性禁用依赖该传感器的功能（如GPS丢失时禁用定点模式）。

3. RC输入输出处理

读取RC遥控器输入

rc().read_input();
  • 功能：从RC接收机读取用户输入（如遥控器摇杆位置、开关状态）。
  • 数据流向：将PWM/PPM/SBUS等信号解码为通道值（通常为1000-2000μs），存储到RC_Channel对象中。
  • 后续处理：这些值将被传递给控制算法（如手动模式下直接转换为电机输出，自动模式下作为参考指令）。

输出到辅助伺服通道

SRV_Channels::output_ch_all();
  • 功能：将控制信号输出到所有辅助伺服通道（如机械臂、相机云台、灯光等）。
  • 应用场景：
    • 控制相机角度（如俯仰、横滚）。
    • 操作机械爪抓取物体。
    • 控制灯光亮度或开关。
  • 执行时机：50Hz的频率确保伺服动作平滑（人眼无法分辨50Hz以上的变化，更高频率对伺服控制意义不大）。

为什么选择50Hz？

  • 故障检测的时效性：20ms的周期足够快速发现危险（如遥控器信号丢失），同时避免过于频繁的检查消耗CPU资源。
  • RC信号处理的平衡点：
    • 大多数RC接收机的更新频率为50-60Hz，50Hz循环能及时捕获最新输入。
    • 若频率过低（如10Hz），会导致操作延迟明显（100ms延迟 vs 20ms延迟）；若过高（如400Hz），则RC信号本身并未更新，造成计算浪费。
  • 与其他循环的协同：
    • 400Hz循环处理姿态控制（对延迟最敏感）。
    • 50Hz循环处理故障安全和RC（次敏感）。
    • 低频循环（1-10Hz）处理日志、状态检查等非紧急任务。

fifty_hz_loop()是ArduSub系统的安全与控制枢纽，通过中等频率的循环执行：

  • 保障系统安全：多重故障检测确保在危险发生时快速响应（如20ms内发现遥控器信号丢失）。
  • 连接用户与系统：及时读取并执行用户操作指令，同时控制辅助设备（如相机、机械臂）。
  • 优化资源分配：避免将这些任务放入400Hz循环，减少CPU负载，使高频循环专注于核心控制。
3. 10Hz任务（update_batt_compass等）

Sub::ten_hz_logging_loop 以10Hz（每秒10次）的频率执行，专门处理中等频率的日志记录。其设计逻辑是：

  • 对于变化较慢的关键数据（如电池状态、遥控器输入、控制参数），10Hz的频率足够捕捉其动态变化（无需更高频率，避免冗余）；
  • 对于已在更高频日志（如25Hz、400Hz）中记录的数据，通过条件判断避免重复记录，节省存储和计算资源。
  1. 姿态与角速度日志（条件记录）
// log attitude data if we\'re not already logging at the higher rateif (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); ahrs_view.Write_Rate(motors, attitude_control, pos_control); if (should_log(MASK_LOG_PID)) { logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info()); }}
  • 核心逻辑：仅当启用“中等频率姿态日志”（MASK_LOG_ATTITUDE_MED）且未启用“高频姿态日志”（MASK_LOG_ATTITUDE_FAST）时记录，避免重复（高频日志已包含这些数据）。
  • 具体记录内容：
    • Log_Write_Attitude()：记录当前姿态（横滚角、俯仰角、偏航角）。
    • ahrs_view.Write_Rate(...)：记录姿态角速度（结合电机输出和控制指令，反映实际运动状态）。
    • 若启用MASK_LOG_PID（PID日志）：分别记录横滚、俯仰、偏航的速率PID控制器（底层控制核心）和Z轴加速度PID的参数（如比例项、积分项、微分项输出），用于调试控制算法的稳定性（例如“是否超调”“响应速度是否合适”）。
  1. 电机与电池日志
if (should_log(MASK_LOG_MOTBATT)) { Log_Write_MotBatt();}
  • 日志掩码：MASK_LOG_MOTBATT（电机与电池日志）。
  • 记录内容：Log_Write_MotBatt() 通常记录电机输出（如各推进器的PWM值）、电池状态（电压、电流、剩余电量）。
  • 作用：监控动力系统健康状态（如电池是否欠压、电机输出是否异常波动），是排查“动力不足”“续航短”等问题的关键数据。
  1. 遥控器输入/输出日志
if (should_log(MASK_LOG_RCIN)) { logger.Write_RCIN();}if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT();}
  • 日志掩码：
    • MASK_LOG_RCIN：遥控器输入日志（如用户通过摇杆发送的指令，各通道PWM值）。
    • MASK_LOG_RCOUT：系统输出日志（如发送给电机、舵机的PWM指令）。
  • 作用：对比输入与输出，判断“用户指令是否被正确执行”（例如“用户推油门，但电机输出未变化”可能是RC信号丢失或电机故障）。
  1. 导航调参日志
if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || !mode_has_manual_throttle(control_mode))) { pos_control.write_log();}
  • 日志掩码：MASK_LOG_NTUN（导航调参日志）。
  • 条件限制：仅当当前控制模式需要GPS（如定点、航点模式）或无手动油门（如自动定深）时记录，因为这些模式依赖位置控制器的精确输出。
  • 记录内容：pos_control.write_log() 记录位置控制器的状态（如位置误差、速度指令、控制输出限制），用于调试导航精度（如“为什么无法稳定在目标点”）。
  1. IMU振动日志
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { AP::ins().Write_Vibration();}
  • 日志掩码：IMU相关日志（普通IMU、快速IMU、原始IMU数据）。
  • 记录内容：Write_Vibration() 记录IMU传感器的振动数据（如三轴振动加速度）。
  • 重要性：振动是IMU测量误差的主要来源（振动会导致加速度计/陀螺仪数据噪声增大），该日志用于分析“是否需要减震”“传感器安装是否牢固”。
  1. 控制调参日志
if (should_log(MASK_LOG_CTUN)) { attitude_control.control_monitor_log();}
  • 日志掩码：MASK_LOG_CTUN（控制调参日志）。
  • 记录内容：control_monitor_log() 记录姿态控制器的监控数据（如控制误差、输出饱和状态、抗积分饱和逻辑）。
  • 作用：调试姿态控制的稳定性（如“是否因输出限幅导致响应变慢”“积分项是否累积过多导致超调”）。

核心设计思想

  1. 按需记录：通过日志掩码（MASK_LOG_*）精确控制记录内容，用户可根据调试需求启用/禁用特定日志（如仅记录PID数据排查控制问题）。
  2. 频率适配：10Hz的频率匹配中等变化率数据（如电池电压、遥控器指令），避免高频日志（如400Hz IMU原始数据）的存储压力，同时保证数据的时间连续性。
  3. 避免冗余：通过!should_log(MASK_LOG_ATTITUDE_FAST)等条件，防止同一数据在不同频率循环中重复记录，优化存储效率。
4. 1Hz循环（one_hz_loop）

Sub::one_hz_loop 以1Hz频率执行，专注于低时效性任务：

  • 系统状态监控（如预臂检查、飞行状态判断）；
  • 配置参数更新（如传感器朝向、油门范围）；
  • 低频率调试与日志（避免高频占用带宽或存储）。
    这些任务对实时性要求低，1Hz足够覆盖其变化速度（如电池状态、解锁条件等通常几秒才会显著变化）。
  1. 调试信息周期性发送（用户添加逻辑）
if(send_cnt==10) { send_cnt = 0; float vx_cm = inertial_nav.get_velocity().x; float vy_cm = inertial_nav.get_velocity().y; gcs().send_text(MAV_SEVERITY_INFO,\"vx cm in world is %f, vy %f\",vx_cm,vy_cm); gcs().send_text(MAV_SEVERITY_INFO,\"DVL raw %f, y %f, z %f\",velocity_dvl_raw.x,velocity_dvl_raw.y,velocity_dvl_raw.z); gcs().send_text(MAV_SEVERITY_INFO,\"X %f Y %f Z %f R %f P %f Y %f \",motors.get_forward(),motors.get_lateral(),motors.get_throttle(),motors.get_roll(),motors.get_pitch(),motors.get_yaw()); }send_cnt++;
  • 功能：每10秒（send_cnt从0累加到10，1Hz循环下正好10秒）向地面站发送一组调试信息，包括：
    • 世界坐标系下的X/Y方向速度（vx_cm/vy_cm）；
    • DVL（多普勒测速仪）原始速度数据（水下常用的高精度测速传感器）；
    • 电机输出量（前进、侧向、油门、横滚、俯仰、偏航的控制信号）。
  • 设计意图：调试信息无需高频发送（10秒1次足够监控趋势），避免占用地面站通信带宽（尤其在低带宽数传场景下）。
  1. 预臂检查与状态更新
bool arm_check = arming.pre_arm_checks(false);ap.pre_arm_check = arm_check;AP_Notify::flags.pre_arm_check = arm_check;AP_Notify::flags.pre_arm_gps_check = position_ok();AP_Notify::flags.flying = motors.armed();
  • 预臂检查（pre_arm_checks）：
    调用arming.pre_arm_checks(false)检查系统是否满足“解锁电机”的条件（如传感器是否正常、参数是否合理、电池是否有电等），结果存入arm_check。
  • 状态同步：
    • 将预臂检查结果同步到系统状态（ap.pre_arm_check）和通知系统（AP_Notify::flags），通过指示灯、地面站提示等方式告知用户“是否可以解锁”。
    • pre_arm_gps_check：通过position_ok()判断GPS/位置估计是否可用（解锁的重要前提）。
    • flying：标记当前是否处于“飞行中”（实际为水下航行，通过电机是否解锁判断）。
  1. 系统状态日志记录
if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::AP_STATE, ap.value);}
  • 功能：若日志功能启用（should_log(MASK_LOG_ANY)），记录系统核心状态数据（AP_STATE），包括解锁状态、故障标志、模式信息等。
  • 频率适配：系统状态变化缓慢（如从“锁定”到“解锁”是离散事件），1Hz记录足够反映状态变迁，无需高频。
  1. 未解锁时的配置更新
if (!motors.armed()) { // make it possible to change ahrs orientation at runtime during initial config ahrs.update_orientation(); motors.update_throttle_range();}
  • AHRS朝向更新：
    若电机未解锁（!motors.armed()，处于配置阶段），允许通过ahrs.update_orientation()动态更新AHRS（姿态传感器）的安装朝向（如用户在地面调整了传感器角度，无需重启即可生效）。
  • 油门范围更新：
    motors.update_throttle_range() 重新计算电机的油门输出范围（如最小/最大PWM值），确保解锁时电机输出符合配置（尤其适用于新电机校准后）。
  1. 辅助设备与控制参数更新
// update assigned functions and enable auxiliary servosSRV_Channels::enable_aux_servos();// update position controller alt limitsupdate_poscon_alt_max();// log terrain dataterrain_logging();
  • 辅助舵机启用：
    SRV_Channels::enable_aux_servos() 刷新辅助伺服通道的功能配置（如机械爪、相机云台、灯光等），确保设备按最新参数工作（配置变化后1秒内生效）。
  • 位置控制器限幅更新：
    update_poscon_alt_max() 调整位置控制器的最大深度/高度限制（可能根据任务需求或用户设置动态修改，1Hz更新足够及时）。
  • 地形日志：
    terrain_logging() 记录地形数据（水下场景较少依赖地形，但若启用相关功能，1Hz记录可反映地形变化趋势）。
  1. 飞行状态标记（罗盘学习适配）
// need to set \"likely flying\" when armed to allow for compass// learning to runset_likely_flying(hal.util->get_soft_armed());
  • 功能：设置“可能在飞行中”的状态（set_likely_flying），当系统“软解锁”（hal.util->get_soft_armed()，一种预解锁状态）时，允许罗盘进入“飞行中学习模式”。
  • 罗盘学习逻辑：飞行中（水下航行）的磁场环境与地面不同（如推进器电流产生磁场干扰），罗盘需要在飞行状态下动态校准，该标记为其提供触发条件。

核心设计逻辑

  1. 低频率适配：所有任务均为低时效性需求（状态检查、配置更新、低频日志），1Hz频率可平衡资源消耗与功能需求——若频率过高（如10Hz），会重复执行无意义的操作（如预臂检查1秒内结果不变）；若过低（如0.1Hz），则状态反馈延迟明显。

  2. 调试与配置友好：

    • 10秒一次的调试信息（send_cnt逻辑）避免占用通信带宽，同时提供足够的监控粒度；
    • 未解锁时的配置更新（AHRS朝向、油门范围）允许用户实时调整参数，无需重启系统。

  3. 安全性保障：预臂检查、飞行状态标记等功能确保系统在低频率下仍能可靠判断“是否可解锁”“是否处于安全状态”，为高频控制循环提供基础保障。

5. 25Hz循环（Sub::twentyfive_hz_logging ）

这段代码是ArduSub水下航行器中以25Hz频率运行的日志记录循环（twentyfive_hz_logging），主要负责记录中等频率的关键数据（如高频姿态、PID参数、IMU数据）。25Hz的频率（周期40ms）在数据完整性和存储效率之间取得平衡，适用于需要较高时间分辨率但又非极致高频的场景。以下是详细解释：

函数整体定位
Sub::twentyfive_hz_logging 以25Hz频率执行，专门处理高频日志记录。其设计逻辑是：

  • 对于变化较快的关键数据（如姿态、角速度、PID控制器输出），25Hz的频率能捕捉其动态变化细节；
  • 通过条件判断避免与更高频日志（如400Hz IMU原始数据）重复记录，节省存储资源。
  1. 高频姿态与角速度日志（条件记录）
if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); ahrs_view.Write_Rate(motors, attitude_control, pos_control); if (should_log(MASK_LOG_PID)) { logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info()); }}
  • 核心逻辑：仅当启用“高频姿态日志”（MASK_LOG_ATTITUDE_FAST）时执行，记录内容包括：
    • Log_Write_Attitude()：记录当前姿态（横滚角、俯仰角、偏航角），25Hz的频率可清晰捕捉姿态变化过程（如水下转弯、上浮动作）。
    • ahrs_view.Write_Rate(...)：记录姿态角速度（结合电机输出和控制指令），用于分析姿态响应速度（例如“俯仰角变化是否符合预期”）。
    • 若启用MASK_LOG_PID：记录横滚、俯仰、偏航的速率PID控制器和Z轴加速度PID的参数（比例项、积分项、微分项输出）。
  1. IMU数据日志（条件记录）
// log IMU data if we\'re not already logging at the higher rateif (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) { AP::ins().Write_IMU();}
  • 核心逻辑：仅当启用“IMU日志”（MASK_LOG_IMU）且未启用“IMU原始数据日志”（MASK_LOG_IMU_RAW）时执行。
    • Write_IMU()：记录IMU处理后的数据（如滤波后的加速度、角速度），25Hz的频率适合分析中低频振动或姿态变化。
    • 避免冗余：若已启用MASK_LOG_IMU_RAW（通常以400Hz记录原始IMU数据），则跳过此处记录，避免重复存储相同信息。

核心设计思想

  1. 频率分层：

    • 10Hz日志（ten_hz_logging_loop）记录变化较慢的数据（如电池、RC输入）；
    • 25Hz日志（当前函数）记录中等变化率数据（姿态、PID参数、IMU滤波值）；
    • 400Hz日志记录高频原始数据（如IMU原始值）。
      通过这种分层，在有限的存储容量下，实现对不同变化率数据的合理采样。

  2. 按需记录：

    • 通过日志掩码（MASK_LOG_*）和条件判断（如!should_log(MASK_LOG_IMU_RAW)），用户可灵活选择记录内容，避免不必要的存储开销。
    • 例如：调试姿态控制时启用MASK_LOG_ATTITUDE_FAST和MASK_LOG_PID；排查振动问题时启用MASK_LOG_IMU。

  3. 资源优化：

    • 25Hz的频率是平衡数据价值与存储成本的结果：
      • 对于姿态和PID参数，25Hz足够捕捉控制算法的动态特性（人类操作响应延迟约100ms，25Hz周期40ms远小于此）；
      • 相比400Hz日志，25Hz日志文件体积缩小16倍，显著延长日志存储时间。
6. 3.3Hz循环（Sub::twentyfive_hz_logging ）

以3.3Hz频率运行的安全监控循环（three_hz_loop，周期约300毫秒），主要负责中等频率的安全检查和设备状态监控，覆盖漏水检测、环境参数异常、通信状态等关键安全维度。3.3Hz的频率（每秒约3次）既能及时发现潜在危险，又不会因过于频繁的检查占用过多系统资源

  1. 漏水检测器更新
leak_detector.update();
  • 功能：更新漏水检测器的状态（leak_detector是漏水检测模块的实例）。
  • 实现逻辑：通常通过防水外壳内的湿度传感器或漏水探针，检测是否有液体侵入（如海水渗入机身）。update()方法会读取传感器数据、滤波去噪，并更新内部状态（如“是否检测到漏水”）。
  • 重要性：水下设备的密封性是核心安全指标，漏水可能导致电路短路、设备失效，需高频监控但无需极致实时（300ms一次足够在漏水初期发现）。
  1. 漏水故障安全检查
failsafe_leak_check();
  • 功能：基于leak_detector的更新结果，执行漏水故障的安全处理逻辑。
  • 触发条件：若检测到漏水（如传感器数据超过阈值），则触发故障安全措施，例如：
    • 向地面站发送紧急警报（gcs().send_text(MAV_SEVERITY_EMERGENCY, \"Leak detected!\")）；
    • 自动切换到“紧急上浮模式”（快速返回水面，减少进水风险）；
    • 若漏水严重，触发“安全锁定”（关闭电机，避免设备失控下沉）。
  1. 内部压力异常检查
failsafe_internal_pressure_check();
  • 功能：监控水下机器人内部的气压/水压（通常针对密封舱体），检测是否存在异常压力变化。
  • 异常场景：
    • 内部压力突然升高：可能是密封舱体破裂（外部水压渗入），或内部设备过热导致空气膨胀；
    • 内部压力过低：可能是舱体漏气（如密封圈失效），外部水压可能挤压舱体导致损坏。
  • 应对措施：触发警告或紧急上浮，避免设备因压力异常变形或进水。
  1. 内部温度异常检查
failsafe_internal_temperature_check();
  • 功能：监控设备内部（如主控板、电池、电机控制器）的温度，检测是否超出安全范围。
  • 风险点：水下环境散热条件差（水的导热性虽好，但设备密封后散热受限），温度过高可能导致电路老化、电池鼓包甚至起火。
  • 触发逻辑：若温度超过阈值（如主控板超过60℃），则：
    • 降低高功耗设备功率（如减少电机输出）；
    • 发送温度警告至地面站；
    • 极端情况下触发紧急上浮，利用水面散热或手动干预。
  1. 地面站通信状态检查
// check if we\'ve lost contact with the ground stationfailsafe_gcs_check();
  • 功能：检查与地面控制站（GCS）的通信是否中断（如长时间未收到GCS心跳包）。
  • 通信中断风险：水下机器人依赖GCS进行远程控制和状态监控，通信丢失可能导致设备失控（尤其在自主模式故障时）。
  • 故障安全措施：
    • 若短时间中断（如几秒）：维持当前模式，等待通信恢复；
    • 若长时间中断（如10-30秒）：触发“失联保护”（如返回最后已知的安全位置、悬浮待命）。
  1. 地形数据故障检查
// check if we\'ve lost terrain datafailsafe_terrain_check();
  • 功能：检查地形数据（如水下地形高度、障碍物信息）是否丢失或不可靠。
  • 地形数据作用：在依赖地形的导航模式（如“避障”“沿地形巡航”）中，地形数据是路径规划的基础，数据丢失可能导致碰撞。
  • 应对措施：若地形数据不可用，自动切换到不依赖地形的模式（如“定深模式”），并发送警告至GCS。
  1. 电子围栏检查（条件编译）
#if AC_FENCE == ENABLED // check if we have breached a fence fence_check();#endif // AC_FENCE_ENABLED
  • 功能：仅当启用电子围栏功能（AC_FENCE == ENABLED）时执行，检查机器人是否超出预设的安全区域（如“禁止进入某区域”“最大下潜深度限制”）。
  • 围栏类型：可能包括虚拟地理边界（如经纬度范围）、深度边界（如最大下潜100米）等。
  • 触发后处理：若超出围栏，自动限制运动（如停止向禁入区域移动、强制返回安全区）。
  1. 伺服与继电器事件更新
ServoRelayEvents.update_events();
  • 功能：更新伺服电机（如机械爪、相机云台）和继电器（如灯光、水泵）的定时事件或状态切换。
  • 应用场景：
    • 定时触发相机拍照（如每3秒拍一张，与3.3Hz频率匹配）；
    • 控制漏水检测泵的周期性工作（如每300ms启动一次抽水检测）；
    • 切换灯光模式（如“搜索模式”与“待机模式”的定时切换）。
  • 频率适配：这些设备的动作无需高频控制（机械结构响应速度远低于300ms），3.3Hz足够满足需求。

核心设计思想

  1. 安全优先级分层：
    该循环聚焦于“中期安全隐患”——这些风险（如漏水、温度升高）不会瞬间致命，但需在几秒内发现并处理（慢于高频控制需求，快于低频状态记录）。3.3Hz的频率平衡了“及时性”和“资源消耗”。

  2. 多维度安全覆盖：
    从设备物理安全（漏水、压力）、环境适配（温度、地形）、通信可靠性（GCS连接）到操作边界（电子围栏），形成全方位的安全监控网络，避免单一安全漏洞导致事故。

  3. 与其他循环协同：

    • 高频循环（400Hz）：处理实时控制（如姿态、深度）；
    • 3.3Hz循环：处理中期安全监控；
    • 低频循环（1Hz）：处理长期状态（如预臂检查、日志汇总）。
      三者形成“控制-监控-记录”的多层次安全体系，确保水下机器人在复杂环境中可靠运行。
      。

五、辅助功能

  • read_AHRS()：更新姿态与航向参考系统（AHRS），融合IMU数据得到当前姿态。
  • control_check_barometer()：检查深度传感器状态，确保定深控制有效。
  • 航点辅助函数：get_wp_distance_m()等计算航点距离、方位，支持自主导航。

六、设计特点

  1. 分层调度：按任务实时性需求分配频率（400Hz→1Hz），确保核心控制（如姿态、电机）优先执行，非关键任务（如日志）低频率运行，优化CPU资源。
  2. 模块化与可配置：通过条件编译（#if OPTFLOW == ENABLED）支持功能扩展，不同硬件可按需启用模块。
  3. 安全性：多重故障检查（EKF、传感器、RC输入）在各频率循环中执行，快速响应异常。
  4. 可调试性：1Hz循环中的调试信息发送、详细日志记录，便于问题定位。
网络标签：

相关问题