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)); }
- 油门控制输入(
throttle_in
)和输出 (throttle_out
);
- 期望深度
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
接口,而日志获取的期望深度就是获取深度控制中设置的值;
- 气压计高度
BarAlt
直接从压力深度传感器获取;
- 对于装载测距仪机器,
desired_rangefinder_alt
和rangefinder_alt
为测距仪期望高度和采集的实际高度;
- 期望爬升率
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" }, ... }
序号 | 缩写 | 全称 | 中文 | 单位 |
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 |

- Author:felixfixit
- URL:http://www.felixmicrospace.top/article/althold_mode_log
- Copyright:All articles in this blog, except for special statements, adopt BY-NC-SA agreement. Please indicate the source!