Lazy loaded image
定深模式日志分析
Words 784Read Time 2 min
2024-8-2
2025-7-16
type
date
slug
category
icon
password
姿态控制(包含定深模式)控制油门、高度、爬升率等信息默认存储在日志文件中,通过分析日志文件,我们可以还原出姿态控制过程,分析控制效果,排查控制异常原因。

日志记录源码

分析 【ArduSub/Log.cpp 】源码可知,程序在log_Control_Tuning 结构体中定义了日志需记录的字段,写日志接口Log_Write_Control_Tuning 实例化 为struct log_Control_Tuning pkt ,并将每个字段赋值,调用写日志接口存储进入日志文件中。
// Code to Write and Read packets from AP_Logger log memory // Code to interact with the user to dump or erase logs struct PACKED log_Control_Tuning { LOG_PACKET_HEADER; uint64_t time_us; float throttle_in; float angle_boost; float throttle_out; float throttle_hover; float desired_alt; float inav_alt; float baro_alt; int16_t desired_rangefinder_alt; int16_t rangefinder_alt; float terr_alt; int16_t target_climb_rate; int16_t climb_rate; }; // Write a control tuning packet void Sub::Log_Write_Control_Tuning() { // get terrain altitude float terr_alt = 0.0f; #if AP_TERRAIN_AVAILABLE if (terrain.enabled()) { terrain.height_above_terrain(terr_alt, true); } else { terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f; } #else terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f; #endif struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), time_us : AP_HAL::micros64(), throttle_in : attitude_control.get_throttle_in(), angle_boost : attitude_control.angle_boost(), throttle_out : motors.get_throttle(), throttle_hover : motors.get_throttle_hover(), desired_alt : pos_control.get_pos_target_z_cm() / 100.0f, inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(), rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), climb_rate : climb_rate }; logger.WriteBlock(&pkt, sizeof(pkt)); }
ArduSub/Log.cpp
  1. 油门控制输入(throttle_in)和输出 (throttle_out);
  1. 期望深度desired_alt通过 pos_control 对象提供的get_pos_target_z_cm() 接口获取,并除以100,单位转换为m。结合
    定深模式源码分析
    中深度控制逻辑中,调用了 pos_control对象提供的set_pos_target_z_cm set_pos_target_z_from_climb_rate_cm接口,而日志获取的期望深度就是获取深度控制中设置的值;
  1. 气压计高度 BarAlt 直接从压力深度传感器获取;
  1. 对于装载测距仪机器,desired_rangefinder_altrangefinder_alt 为测距仪期望高度和采集的实际高度;
  1. 期望爬升率target_climb_rate 通过get_vel_target_z_cms()获取,又油门控制输入指定。
 

导出日志分析

日志文件中的记录格式由LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG)指定。通过查看日志文件中 CTUN(Control Tuning)包,结合文件中LoggerMessage字段说明,记录字段含义如下:
// @LoggerMessage: CTUN // @Description: Control Tuning information // @Field: TimeUS: Time since system startup // @Field: ThI: throttle input // @Field: ABst: angle boost // @Field: ThO: throttle output // @Field: ThH: calculated hover throttle // @Field: DAlt: desired altitude // @Field: Alt: achieved altitude // @Field: BAlt: barometric altitude // @Field: DSAlt: desired rangefinder altitude // @Field: SAlt: achieved rangefinder altitude // @Field: TAlt: terrain altitude // @Field: DCRt: desired climb rate // @Field: CRt: climb rate // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), "CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" }, ... }
ArduSub/Log.cpp
序号
缩写
全称
中文
单位
1
ThI
Throttle Input
油门输入
(0,1000)
2
ABst
Angle Boost
飞行器倾斜时的油门补偿
(0,1000)
3
ThO
Throttle Output
油门输出
(0,1000)
4
DAlt
Desired Altitude
期望高度
米(m)
5
Alt
Achieved Altitude
真实高度
米(m)
6
BAlt
Barometric Altitude
气压计高度
米(m)
7
DSAlt
Desired Rangefinder Altitude
声呐期望高度
米(m)
8
SAlt
Achieved Rangefinder Altitude
声呐高度
米(m)
9
TAlt
Terrain Altitude
地形高度
米(m)
10
DCRt
Desired Climb Rate
期望爬升速率
cm/s
11
CRt
Climb Rate
加速度计和气压计计算的爬升速率
cm/s
mission planner 中日志分析界面
mission planner 中日志分析界面
 
上一篇
电机解锁/上锁(Arm/Disarm)源码分析
下一篇
定深模式源码分析

Comments
Loading...