版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lixiaoweimashixiao/article/details/82744125
目录
文章目录
摘要
本节主要分析ardupilot3.6版本的log固件代码，欢迎批评指正！！！
1.官网资料学习
添加一个新的日志消息（Adding a new Log Message）
DataFlash日志存储在飞行控制器的DataFlash内存中，可以在飞行后下载。这些日志提供了关于每次飞行的详细信息，这可能非常重要，尤其是在试图诊断出故障原因时
这个页面解释了如何编写一个新的dataflash日志消息。
简单的方法（The Easy Way）
用：ardupilot/libraries/DataFlash/DataFlash.h 的DataFlash_Class:instance()->Log_Write函数写消息
void Log_Write(const char *name, const char *labels, const char *fmt, ...);
void Log_Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);
在Compass_learn.cpp中可以找到正在使用的顶层函数的一个例子：
if (sample_available)
{
DataFlash_Class::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
AP_HAL::micros64(),
(double)best_offsets.x,
(double)best_offsets.y,
(double)best_offsets.z,
(double)best_error,
(double)best_yaw_deg,
(double)worst_error,
num_samples);
}
- 第一个参数是消息名称。这应该是4个字符或更少，并且是唯一的。
- 第二个参数是一个逗号分隔的字段名列表
- 第三个参数是一个格式字符串，每个字母表示对应字段的格式。每个字母的含义可以在ardupilot/libraries/DataFlash/LogStructure.h中找到。(意思就是格式代码的简称，比如整形16位，就用a来表示。)
我们分析下："QffffffI"是什么意思？Q:uint64_t,可以看出AP_HAL::micros64()就是这种类型：uint64_t micros64()，因此Q表示一种类型的简写；f：float表示浮点类型，(double)best_offsets.x,(double)best_offsets.y, (double)best_offsets.z,(double)best_error, (double)best_yaw_deg,
(double)worst_error,num_samples)本来都是浮点类型，到这里知道什么意思了吧。
/*
二进制日志消息格式字符串中的格式字符:Format characters in the format string for binary log messages
a : int16_t[32]
b : int8_t
B : uint8_t
h : int16_t
H : uint16_t
i : int32_t
I : uint32_t
f : float
d : double
n : char[4]
N : char[16]
Z : char[64]
c : int16_t * 100
C : uint16_t * 100
e : int32_t * 100
E : uint32_t * 100
L : int32_t latitude/longitude
M : uint8_t flight mode
q : int64_t
Q : uint64_t
*/
- 剩下的参数是要记录的实际值。您可能注意到，在上面的示例中，有些字段的格式是float (f)，但转换为(double)，这是正确的，并且是避免编译器警告所必需的
AP_HAL::micros64(), //第二个参数是一个逗号分隔的字段名列表
(double)best_offsets.x, //第三个参数是一个格式字符串，每个字母表示对应字段的格式。
(double)best_offsets.y,
(double)best_offsets.z,
(double)best_error,
(double)best_yaw_deg,
(double)worst_error,
num_samples);
第二个Log_Write()函数与第一个函数相同，只是它接受另外两个字符串参数“units”和“mults”。 与“format”参数类似，这些参数中的每个字符指定以下字段的单位（即“d”代表度数）或乘数（即“2”代表* 100，“B”代表* 0.01）。 这些帮助图形工具在向用户显示时正确缩放输出
例如，下面是一个“测试”日志消息，它输出当前的系统时间和高度。
DataFlash_Class::instance()->Log_Write("TEST", "TimeUS,Alt",
"sm", // units: seconds, meters,这个是单位s和米
"FB", // mult: 1e-6, 1e-2，表示的数量级
"Qf", // format: uint64_t, float ，数据类型，和上面的讲解一样
AP_HAL::micros64(), //显示的时间单位
(double)alt_in_cm); //显示的高度信息
- 时间以秒为单位（“s”），乘数为“F”表示该值应除以1百万，格式为“Q”表示它输出为64位无符号整数。
- 海拔高度单位为米（“m”），乘数为“B”表示该值应除以100（从厘米转换为米）和格式“f”，因为该值为浮点数。
上面的要是不明白，可以具体看下面的表示
扫描二维码关注公众号，回复：
3600438 查看本文章
//这里所有的单元都应该是基本单元。这意味着电池容量在这里是“amp*.”。请保持名称与Tools/autotest/param_metadata/param.py:33一致。
const struct UnitStructure log_Units[] = {
{ '-', "" }, // no units e.g. Pi, or a string
{ '?', "UNKNOWN" }, // Units which haven't been worked out yet....
{ 'A', "A" }, // Ampere
{ 'd', "deg" }, // of the angular variety, -180 to 180
{ 'b', "B" }, // bytes
{ 'k', "deg/s" }, // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians
{ 'D', "deglatitude" }, // degrees of latitude
{ 'e', "deg/s/s" }, // degrees per second per second. Degrees are NOT SI, but is some situations more user-friendly than radians
{ 'E', "rad/s" }, // radians per second
{ 'G', "Gauss" }, // Gauss is not an SI unit, but 1 tesla = 10000 gauss so a simple replacement is not possible here
{ 'h', "degheading" }, // 0.? to 359.?
{ 'i', "A.s" }, // Ampere second
{ 'J', "W.s" }, // Joule (Watt second)
// { 'l', "l" }, // litres
{ 'L', "rad/s/s" }, // radians per second per second
{ 'm', "m" }, // metres
{ 'n', "m/s" }, // metres per second
// { 'N', "N" }, // Newton
{ 'o', "m/s/s" }, // metres per second per second
{ 'O', "degC" }, // degrees Celsius. Not SI, but Kelvin is too cumbersome for most users
{ '%', "%" }, // percent
{ 'S', "satellites" }, // number of satellites
{ 's', "s" }, // seconds
{ 'q', "rpm" }, // rounds per minute. Not SI, but sometimes more intuitive than Hertz
{ 'r', "rad" }, // radians
{ 'U', "deglongitude" }, // degrees of longitude
{ 'u', "ppm" }, // pulses per minute
{ 'U', "us" }, // pulse width modulation in microseconds
{ 'v', "V" }, // Volt
{ 'P', "Pa" }, // Pascal
{ 'w', "Ohm" }, // Ohm
{ 'z', "Hz" } // Hertz
};
// this multiplier information applies to the raw value present in the
// log. Any adjustment implied by the format field (e.g. the "centi"
// in "centidegrees" is *IGNORED* for the purposes of scaling.
// Essentially "format" simply tells you the C-type, and format-type h
// (int16_t) is equivalent to format-type c (int16_t*100)
// tl;dr a GCS shouldn't/mustn't infer any scaling from the unit name
const struct MultiplierStructure log_Multipliers[] = {
{ '-', 0 }, // no multiplier e.g. a string
{ '?', 1 }, // multipliers which haven't been worked out yet....
// <leave a gap here, just in case....>
{ '2', 1e2 },
{ '1', 1e1 },
{ '0', 1e0 },
{ 'A', 1e-1 },
{ 'B', 1e-2 },
{ 'C', 1e-3 },
{ 'D', 1e-4 },
{ 'E', 1e-5 },
{ 'F', 1e-6 },
{ 'G', 1e-7 },
// <leave a gap here, just in case....>
{ '!', 3.6 }, // (ampere*second => milliampere*hour) and (km/h => m/s)
{ '/', 3600 }, // (ampere*second => ampere*hour)
};
复杂的方法（The Harder Way）
对于通常使用的消息，特别是以相对高的速率(50hz或更高)输出的消息，可以使用稍微更高效的日志记录方法。。
- 确定日志记录是来自库（即，对Log_Write的调用将来 libraries directory的某个地方）还是来自无人机类型代码（即，来自ArduCopter、ArduPlane等）（这里主要注意上面说的两点）
- 在定义的 vehicle’s #define list or enum in defines.h或者 DataFlash/LogStructure.h’s LogMessages enum中添加一个新的入口定义到不同类型的模型中。
- 定义一个结构体来保存需要保存的值到不同类型的无人机的Log.cpp文件或DataFlash/LogStructure.h中。所有日志文件消息都应该以时间单位：time_us作为它们的第一个字段。
struct PACKED log_Test {
LOG_PACKET_HEADER;
uint64_t time_us; //时间
float a_value; //测量值
}
//下面这个是自己重新拷贝的，跟上面的一样
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;
int32_t baro_alt;
float 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 Copter::Log_Write_Control_Tuning()
{
// get terrain altitude
float terr_alt = 0.0f;
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
if (!terrain.height_above_terrain(terr_alt, true)) {
terr_alt = DataFlash.quiet_nan();
}
#endif
float _target_rangefinder_alt;
if (target_rangefinder_alt_used) {
_target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
} else {
_target_rangefinder_alt = DataFlash.quiet_nan();
}
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_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_rangefinder_alt : _target_rangefinder_alt,
rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt,
target_climb_rate : (int16_t)pos_control->get_vel_target_z(),
climb_rate : climb_rate
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
- 将日志消息的名称、单元、系数和格式字符串添加到不同结构模型的vehicle’s LogStructure array数组或DataFlash/LogStructure.h’s LOG_EXTRA_STRUCTURES array数组中
void Copter::Log_Write_Control_Tuning() {}
void Copter::Log_Write_Performance() {}
void Copter::Log_Write_Attitude(void) {}
void Copter::Log_Write_EKF_POS() {}
void Copter::Log_Write_MotBatt() {}
void Copter::Log_Write_Event(uint8_t id) {}
void Copter::Log_Write_Data(uint8_t id, int32_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
void Copter::Log_Write_Data(uint8_t id, int16_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint16_t value) {}
void Copter::Log_Write_Data(uint8_t id, float value) {}
void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Sensor_Health() {}
void Copter::Log_Write_Precland() {}
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Copter::Log_Write_Vehicle_Startup_Messages() {}
//添加自己的测试代码
void Copter::Log_Write_Test()
//添加自己的测试代码
- 向模型代码或DataFlash库添加一个名为Log_Write_< something-or-other>的新方法，它填充结构，然后调用DataFlash/WriteBlock()
void Copter::Log_Write_Test()
{
struct log_Test pkt =
{
LOG_PACKET_HEADER_INIT(LOG_TEST_MSG),
time_us : AP_HAL::micros64(),
a_value : 1234
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
- 在您希望记录值时，从调度器或代码中的其他地方调用此新函数。
2.ardupilot的数据闪存（The DataFlash library）
**ardupilot的数据闪存日志记录在Mincard上，最后保存格式为.bin文件
自动驾驶仪需要的另一种存储方式就是飞行日志存储，ardupilot基于DataFlash library库来实现。这个库的名字看上去有些怪怪的，实际上这个库最开始是为APM1的DataFlash芯片设计的，它原本是一个硬件驱动库，后来慢慢演变为一个通用日志系统，这个可以在代码中找到蛛丝马迹（这些都是以前的痕迹，不是最好的代码实现方式）。
现在，DataFlash API主要用于实现日志存储模型设计。它允许您自定义日志消息的数据结构。例如GPS消息，用于记录GPS传感器的日志数据。它能够非常有效存储这些数据，它同时也对其他库提供相应的APIs，用来进行日志回传、下载。
如果您最近下载日志时已经在ArduPilot看到了“*.bin”文件，那么您会看到ArduPilot用于存储日志消息的格式。这个时“自我描述”，意思是地面站可以计算出日志文件中消息的格式，而不必有一些共同的方案。在每个日志文件的前端是一组FMT消息，它们具有众所周知的格式，并且描述按照消息的格式。
去看看库/DATAFLIS/Stask/DATAFLASHYTest/DATAFLASHASTest.CPP。您将在顶部看到一个小表，它定义了我们将要编写的日志消息，在本例中是一个“TEST”消息，其中包含4个无符号16位整数和两个带符号32位整数（这就是“HHHHii”的意思）。它还给出了这6个变量的名称（巧妙标记的V1到V4和L1和L2）。
在loop()函数中，你会看到这样一个相当奇怪的调用：
DataFlash是用来存储日志的。日志有固定的格式，固定的日志头和日志包头
DataFlash.get_log_boundaries(log_num, start, end);
DataFlash API隐藏了底层如何存储log文件的细节。另外，对于Pixhawk or Linux这样的支持 Posix IO的系统，日志文件是存储在microSD卡的“LOGS”目录中的。用户可以直接抽出SD卡，直接拷贝到电脑中。
在像APM2这样的板上并不是那么简单。APM2在数据流芯片上有4M字节的存储，可以通过SPI接口访问。接口本身是面向页面的，因此需要填充一个512字节（或者可能是528字节！）页，然后告诉芯片将该页复制到持久存储，同时填充下一页。在这个DataFlash上执行随机IO是不好的——它是为需要连续写入的代码设计的，这在日志记录时发生。与自动驾驶仪喜欢记录的数据量相比，4M字节的大小实际上不是很大，因此当数据填充时，我们还需要处理包装。
所有这些复杂性都隐藏在一个API后面，该API提出了“日志编号”的概念，这只是一组在自动驾驶仪的一次飞行中写入的字节。APM1和APM2上的DataFlash实现在每个页面的前端使用少量标记字节来表示正在写入哪个日志编号。这些日志号对应于当用户要求检索其日志时下载的日志号。
3.ardupilot的日志格式
DataFlash是用来存储日志的。日志有固定的格式，固定的日志头和日志包头
1.日志头格式
// 用于定义日志格式的结构----structure used to define logging format
struct LogStructure {
uint8_t msg_type;
uint8_t msg_len;
const char name[5];
const char format[16];
const char labels[64];
};
//所有车辆类型常见的日志结构-----log structures common to all vehicle types
struct PACKED log_Format
{
LOG_PACKET_HEADER;
uint8_t type;
uint8_t length;
char name[4];
char format[16];
char labels[64];
};
2.日志包头
#define LOG_PACKET_HEADER uint8_t head1, head2, msgid; //日志包头
#define LOG_PACKET_HEADER_INIT(id) head1 : HEAD_BYTE1, head2 : HEAD_BYTE2, msgid : id
#define LOG_PACKET_HEADER_LEN 3 // bytes required for LOG_PACKET_HEADER
// once the logging code is all converted we will remove these from
// this header
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
3.日志举例
libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp
struct PACKED log_Test {
LOG_PACKET_HEADER;
uint16_t v1, v2, v3, v4;
int32_t l1, l2;
};
static const struct LogStructure log_structure[] = {
LOG_COMMON_STRUCTURES,
{ LOG_TEST_MSG, sizeof(log_Test),
"TEST", "HHHHii", "V1,V2,V3,V4,L1,L2" }
};
日志是以文件的形式存储到microSD card，可以直接拔出SD卡拷贝到PC
每个页首都有“日志文件的编号”和“日志文件的页号”。当用户下载日志时，非常有用
void DataFlashTest::setup(void)
{
dataflash.Init(log_structure, ARRAY_SIZE(log_structure));
dataflash.set_vehicle_armed(true);
hal.console->printf("Dataflash Log Test 1.0\n");
// Test
hal.scheduler->delay(20);
dataflash.ShowDeviceInfo(hal.console);
if (dataflash.NeedPrep()) {
hal.console->printf("Preparing dataflash...\n");
dataflash.Prep();
}
// We start to write some info (sequentialy) starting from page 1
// This is similar to what we will do...
dataflash.StartUnstartedLogging();
log_num = dataflash.find_last_log();
hal.console->printf("Using log number %u\n", log_num);
hal.console->printf("After testing perform erase before using DataFlash for logging!\n");
hal.console->printf("\n");
hal.console->printf("Writing to flash... wait...\n");
uint32_t total_micros = 0;
uint16_t i;
for (i = 0; i < NUM_PACKETS; i++) {
uint32_t start = AP_HAL::micros();
// note that we use g++ style initialisers to make larger
// structures easier to follow
struct log_Test pkt = {
LOG_PACKET_HEADER_INIT(LOG_TEST_MSG),
v1 : (uint16_t)(2000 + i),
v2 : (uint16_t)(2001 + i),
v3 : (uint16_t)(2002 + i),
v4 : (uint16_t)(2003 + i),
l1 : (int32_t)(i * 5000),
l2 : (int32_t)(i * 16268)
};
dataflash.WriteBlock(&pkt, sizeof(pkt));
total_micros += AP_HAL::micros() - start;
hal.scheduler->delay(20);
}
hal.console->printf("Average write time %.1f usec/byte\n",
(double)total_micros/((double)i*sizeof(struct log_Test)));
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
dataflash.flush();
#endif
hal.scheduler->delay(100);
}
void DataFlashTest::loop(void)
{
uint16_t start, end;
hal.console->printf("Start read of log %u\n", log_num);
dataflash.get_log_boundaries(log_num, start, end);
dataflash.LogReadProcess(log_num, start, end,
FUNCTOR_BIND_MEMBER(&DataFlashTest::print_mode, void, AP_HAL::BetterStream *, uint8_t),//print_mode,
hal.console);
hal.console->printf("\nTest complete. Test will repeat in 20 seconds\n");
hal.scheduler->delay(20000);
}
4.日志存储相关的函数
用户需要设置page大小512 byte，当写满一页以后告诉芯片复制一页
日志本身是通过DataFlash_File.cpp写到SD卡，还提供了DataFlash_Empty.cpp的块设备读写接口，下面是应用铁电的读写实例
缓冲区buffer
static uint8_t buffer[2][DF_PAGE_SIZE];
void DataFlash_Flash::Init(const struct LogStructure *structure, uint8_t num_types)
{
DataFlash_Backend::Init(structure, num_types);
if (flash_fd == 0) {
flash_fd = open(MTD_LOG_FILE, O_RDWR, 0777);
if (flash_fd == -1) {
printf("DataFlash_Flash init failed\n");
}
}
df_PageSize = DF_PAGE_SIZE; //页大小
df_NumPages = DF_NUM_PAGES - 1; //页数量
}
读取flash数据到buffer
void DataFlash_Flash::PageToBuffer(unsigned char BufferNum, uint16_t PageAdr)
{
PageAdr -= 1;
uint16_t ofs = PageAdr * DF_PAGE_SIZE;
memset(buffer[BufferNum], 0, DF_PAGE_SIZE);
if (lseek(flash_fd, ofs, SEEK_SET) != ofs) {
printf("PageToBuffer lseek err.\n");
return;
}
if (read(flash_fd, buffer[BufferNum], DF_PAGE_SIZE) != DF_PAGE_SIZE)
{
printf("PageToBuffer read err.\n");
return;
}
}
记录需要写入flash的页地址和缓冲区编号
void DataFlash_Flash::BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait)
{
PageAdr -= 1;
uint16_t ofs = PageAdr * DF_PAGE_SIZE;
if(flash_fd < 0) return;
if (lseek(flash_fd, ofs, SEEK_SET) != ofs) {
printf("BufferToPage lseek err.\n");
return;
}
if (::write(flash_fd, &buffer[BufferNum], DF_PAGE_SIZE) != DF_PAGE_SIZE)
{
printf("BufferToPage write err.\n");
return;
}
}
**数据写入缓冲区**
void DataFlash_Flash::BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr,
const void *pHeader, uint8_t hdr_size,
const void *pBuffer, uint16_t size)
{
if (!_writes_enabled) {
return;
}
memset(&buffer[BufferNum][IntPageAdr], 0, size+hdr_size);
if (hdr_size) {
memcpy(&buffer[BufferNum][IntPageAdr],
pHeader,
hdr_size);
}
memcpy(&buffer[BufferNum][IntPageAdr+hdr_size],
pBuffer,
size);
}
**从缓冲区读取数据**
bool DataFlash_Flash::BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size)
{
memset(pBuffer, 0, size);
memcpy(pBuffer, &buffer[BufferNum][IntPageAdr], size);
return true;
}
4.ardupilot的日志学习
1.初始化过程
void Copter::setup()
{
//从参数表中加载默认参数----------Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
//初始化储存的多旋翼布局-----------setup storage layout for copter
StorageManager::set_layout_copter();
//传感器初始化，注册
init_ardupilot();
//初始化整个主loop任务调度-------initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}
需要注意的地方：
用于定义日志格式的结构
struct LogStructure {
uint8_t msg_type; //消息类型
uint8_t msg_len; //消息长度
const char *name; //消息名字
const char *format; //数据格式类型
const char *labels; //名字标签
const char *units; //单位
const char *multipliers; //数量级
};
const struct LogStructure Copter::log_structure[] =
{
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
"ATUN", "QBBfffffff", "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", "s--ddd---o", "F--BBB---0" },
{ LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
"ATDE", "Qff", "TimeUS,Angle,Rate", "sdk", "FBB" },
#endif
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },
#if OPTFLOW == ENABLED
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
#endif
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
//类型，长度，名字，格式，标签，单位，乘法
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
{ LOG_EVENT_MSG, sizeof(log_Event),
"EV", "QB", "TimeUS,Id", "s-", "F-" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),
"D16", "QBh", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),
"DU16", "QBH", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),
"D32", "QBi", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),
"DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
"DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" },
#if FRAME_CONFIG == HELI_FRAME
{ LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qff", "TimeUS,DRRPM,ERRPM", "s--", "F--" },
#endif
#if PRECISION_LANDING == ENABLED
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBBfffffffIIB", "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasUS,EKFOutl,Est", "s--ddmmddms--","F--00BB00BC--" },
#endif
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
{ LOG_CONTROL_TUNING1_MSG, sizeof(log_Control_Tuning1),
"CTUN1", "Qffffffefcfhh", "TimeUS1,ThI1,ABst1,ThO1,ThH1,DAlt1,Alt1,BAlt1,DSAlt1,SAlt1,TAlt1,DCRt1,CRt1", "s----mmmmmmnn", "F----00B0BBBA" },
};
void Copter::init_ardupilot()
{
.....................
#if LOGGING_ENABLED == ENABLED
log_init(); //闪存日志初始化
#endif
.....................
}
void Copter::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); //初始化Dataflash
}
void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_types)
{
gcs().send_text(MAV_SEVERITY_INFO, "Preparing log system");
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
validate_structures(structures, num_types);
dump_structures(structures, num_types);
#endif
if (_next_backend == DATAFLASH_MAX_BACKENDS)
{
AP_HAL::panic("Too many backends");
return;
}
_num_types = num_types;
_structures = structures;
#if defined(HAL_BOARD_LOG_DIRECTORY)
#if HAL_OS_POSIX_IO || HAL_OS_FATFS_IO
if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
DFMessageWriter_DFLogStart *message_writer =
new DFMessageWriter_DFLogStart();
if (message_writer != nullptr) {
backends[_next_backend] = new DataFlash_File(*this,
message_writer,
HAL_BOARD_LOG_DIRECTORY);
}
if (backends[_next_backend] == nullptr) {
hal.console->printf("Unable to open DataFlash_File");
} else {
_next_backend++;
}
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
DFMessageWriter_DFLogStart *message_writer =
new DFMessageWriter_DFLogStart();
if (message_writer != nullptr) {
#if defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS)
backends[_next_backend] = new DataFlash_File(*this, message_writer, HAL_BOARD_LOG_DIRECTORY);
#else
backends[_next_backend] = new DataFlash_Revo(*this, message_writer); // restore dataflash logs
#endif
}
if (backends[_next_backend] == nullptr) {
printf("Unable to open DataFlash_Revo");
} else {
_next_backend++;
}
}
#endif
#endif // HAL_BOARD_LOG_DIRECTORY
#if DATAFLASH_MAVLINK_SUPPORT
if (_params.backend_types == DATAFLASH_BACKEND_MAVLINK ||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
AP_HAL::panic("Too many backends");
return;
}
DFMessageWriter_DFLogStart *message_writer =
new DFMessageWriter_DFLogStart();
if (message_writer != nullptr) {
backends[_next_backend] = new DataFlash_MAVLink(*this,
message_writer);
}
if (backends[_next_backend] == nullptr) {
hal.console->printf("Unable to open DataFlash_MAVLink");
} else {
_next_backend++;
}
}
#endif
for (uint8_t i=0; i<_next_backend; i++)
{
backends[i]->Init()；//后端初始化
}
Prep();
EnableWrites(true);
gcs().send_text(MAV_SEVERITY_INFO, "Prepared log system");
}
2.更新过程
由于ardupilot默认记录log到SD卡，需要解锁才能进行，如果想不解锁就进行记录，需要修改参数设置LOG_DISARMED
SCHED_TASK(update_altitude, 10, 100), //更新高度信息
void Copter::update_altitude()
{
//读取气压高度值--------read in baro altitude
read_barometer();
//写高度信息到dataflash logs中---------write altitude info to dataflash logs
if (should_log(MASK_LOG_CTUN))
{
Log_Write_Control_Tuning();
}
}
void Copter::Log_Write_Control_Tuning()
{
//获取地形高度----get terrain altitude
float terr_alt = 0.0f;
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
if (!terrain.height_above_terrain(terr_alt, true))
{
terr_alt = DataFlash.quiet_nan();
}
#endif
float _target_rangefinder_alt;
if (target_rangefinder_alt_used)
{
_target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
} else
{
_target_rangefinder_alt = DataFlash.quiet_nan();
}
struct log_Control_Tuning pkt =
{
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), //LOG_CONTROL_TUNING_MSG=0x04
time_us : AP_HAL::micros64(), //运行时间
throttle_in : attitude_control->get_throttle_in(), //throttle_in为油门输入，0到1000的整数值。
angle_boost : attitude_control->angle_boost(), //用于倾斜补偿的油门增加量
throttle_out : motors->get_throttle(), //throttle_out油门输出，0到1000的float数值。
throttle_hover : motors->get_throttle_hover(), //评估油门值需要的油门滑动条范围，修改这个值可以修正摇杆的中立点
desired_alt : pos_control->get_alt_target() / 100.0f, //desired_alt目标高度， _pos_target.z的值除以100，单位为米。
inav_alt : inertial_nav.get_altitude() / 100.0f, //飞机目前高度，_relpos_cm.z的值除以100，单位为米。
baro_alt : baro_alt, // 气压计的高度，单位为厘米
desired_rangefinder_alt : (int16_t)target_rangefinder_alt, //目标测距仪高度
rangefinder_alt : rangefinder_state.alt_cm, //测距仪高度
terr_alt : terr_alt, //地形高度
target_climb_rate : (int16_t)pos_control->get_vel_target_z(), //目标爬升率
climb_rate : climb_rate //climb_rate为当前的爬升速率，此值在inertia.cpp中的read_inertial_altitude函数中更新，实际上就是读取的inertial_nav的_velocity_cm.z的值
};
DataFlash.WriteBlock(&pkt, sizeof(pkt)); //把包写进flash中
}
5.ardupilot添加一条自己的参数，并显示到地面站上
1.参数初始化
void Copter::setup()
{
//从参数表中加载默认参数----------Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
//初始化储存的多旋翼布局-----------setup storage layout for copter
StorageManager::set_layout_copter();
//传感器初始化，注册
/******************需要修改的地方**************************/
/******************需要修改的地方**************************/
/******************需要修改的地方**************************/
init_ardupilot();
/******************需要修改的地方**************************/
/******************需要修改的地方**************************/
/******************需要修改的地方**************************/
//初始化整个主loop任务调度-------initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}
#if LOGGING_ENABLED == ENABLED
log_init(); //闪存日志初始化,进入修改
#endif
void Copter::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
}
const struct LogStructure Copter::log_structure[] =
{
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
"ATUN", "QBBfffffff", "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", "s--ddd---o", "F--BBB---0" },
{ LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
"ATDE", "Qff", "TimeUS,Angle,Rate", "sdk", "FBB" },
#endif
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },
#if OPTFLOW == ENABLED
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
#endif
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
//类型，长度，名字，格式，标签，单位，乘法
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
{ LOG_EVENT_MSG, sizeof(log_Event),
"EV", "QB", "TimeUS,Id", "s-", "F-" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),
"D16", "QBh", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),
"DU16", "QBH", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),
"D32", "QBi", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),
"DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
"DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },
{ LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" },
#if FRAME_CONFIG == HELI_FRAME
{ LOG_HELI_MSG, sizeof(log_Heli),
"HELI", "Qff", "TimeUS,DRRPM,ERRPM", "s--", "F--" },
#endif
#if PRECISION_LANDING == ENABLED
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
"PL", "QBBfffffffIIB", "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasUS,EKFOutl,Est", "s--ddmmddms--","F--00BB00BC--" },
#endif
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
/******************需要修改的地方**************************/
/******************需要修改的地方**************************/
/******************需要修改的地方**************************/
{ LOG_CONTROL_CT1_MSG,sizeof(log_Control_CT1),
"CT1", "Q", "TimeUS", "s", "F" },//这里一定要注意书写方法，特别是CT1,你不能随便写这个长度，长度不能超过四个字母，我这里折腾很久，需要注意
};
添加log枚举变量的地方
enum LoggingParameters {
TYPE_AIRSTART_MSG,
TYPE_GROUNDSTART_MSG,
LOG_CONTROL_TUNING_MSG,
LOG_OPTFLOW_MSG,
LOG_EVENT_MSG,
LOG_ERROR_MSG,
LOG_DATA_INT16_MSG,
LOG_DATA_UINT16_MSG,
LOG_DATA_INT32_MSG,
LOG_DATA_UINT32_MSG,
LOG_DATA_FLOAT_MSG,
LOG_AUTOTUNE_MSG,
LOG_AUTOTUNEDETAILS_MSG,
LOG_MOTBATT_MSG,
LOG_PARAMTUNE_MSG,
LOG_HELI_MSG,
LOG_PRECLAND_MSG,
LOG_GUIDEDTARGET_MSG,
LOG_CONTROL_CT1_MSG,
};
定义结构体
struct PACKED log_Control_CT1 {
LOG_PACKET_HEADER;
uint64_t time_us;
};
2.更新写入过程
SCHED_TASK(update_mylog, 10, 100), //更新测试参数
需要注意需要在Copter.h中声明update_mylog()这个函数
void ten_hz_logging_loop();
void twentyfive_hz_logging();
void three_hz_loop();
void one_hz_loop();
void update_GPS(void);
void init_simple_bearing();
void update_simple_mode(void);
void update_super_simple_bearing(bool force_update);
void read_AHRS(void);
void update_altitude();
void update_mylog();//声明函数
函数实现
void Copter::update_mylog()
{
if (should_log(MASK_LOG_CT1))
{
Log_Write_Control_CT1();
}
}
其中：需要注意
（1）这里要定义
#define MASK_LOG_CT1 (1<<20)
#define MASK_LOG_ANY 0x1FFFFF
（2） if (should_log(MASK_LOG_CT1))这里需要设置的参数特别重要，不然if函数是进不去的
(3)函数体
声明Copter.h
void Log_Write_Control_Tuning();
void Log_Write_Control_CT1(); //添加参数
void Log_Write_Performance();
实现函数
void Copter::Log_Write_Control_CT1()
{
struct log_Control_CT1 pkt =
{
LOG_PACKET_HEADER_INIT(LOG_CONTROL_CT1_MSG),
time_us : AP_HAL::micros64()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
