Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: new dataflash logging system

Browse Source 
this allows us to remove the display functions in the vehicle code,
and also allows us to store the format of a log file in the log. It
also stores the parameters used in a flight, rather than the
parameters set at the time the log is dumped
This commit is contained in:
Andrew Tridgell 2013-04-19 17:49:16 +10:00
parent 17d7f1fbe5
commit 916e8d0992
5 changed files with 495 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
libraries/DataFlash/DataFlash.h
View File

@ -6,6 +6,11 @@
#ifndef DataFlash_h
#define DataFlash_h
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_GPS.h>
#include <AP_InertialSensor.h>
#include <AP_AHRS.h>
#include <stdint.h>
// the last page holds the log format in first 4 bytes. Please change
@ -39,19 +44,49 @@ public:
virtual void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) = 0;
virtual uint16_t get_num_logs(void) = 0;
virtual uint16_t start_new_log(void) = 0;
virtual void log_read_process(uint16_t log_num,
void log_read_process(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
void (*callback)(uint8_t msgid)) = 0;
void (*callback)(uint8_t msgid));
virtual void LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port) = 0;
virtual void DumpPageInfo(AP_HAL::BetterStream *port) = 0;
virtual void ShowDeviceInfo(AP_HAL::BetterStream *port) = 0;
virtual void ListAvailableLogs(AP_HAL::BetterStream *port) = 0;
/* logging methods common to all vehicles */
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const GPS *gps, int32_t relative_alt);
void Log_Write_IMU(const AP_InertialSensor *ins);
/*
every logged packet starts with 3 bytes
*/
struct log_Header {
uint8_t head1, head2, msgid;
};
protected:
/*
print column headers
*/
void _print_format_headers(uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port);
/*
read and print a log entry using the format strings from the given structure
*/
void _print_log_entry(uint8_t msg_type,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port);
void Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token,
enum ap_var_type type);
void Log_Write_Parameters(void);
};
/*
@ -66,6 +101,74 @@ public:
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
/*
Format characters in the format string for binary log messages
b : int8_t
B : uint8_t
h : int16_t
H : uint16_t
i : int32_t
I : uint32_t
f : float
N : char[16]
c : int16_t * 100
C : uint16_t * 100
e : int32_t * 100
E : uint32_t * 100
L : uint32_t latitude/longitude
*/
// 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_Parameter {
LOG_PACKET_HEADER;
char name[16];
float value;
};
struct PACKED log_GPS {
LOG_PACKET_HEADER;
uint8_t status;
uint32_t gps_time;
uint8_t num_sats;
int16_t hdop;
int32_t latitude;
int32_t longitude;
int32_t rel_altitude;
int32_t altitude;
uint32_t ground_speed;
int32_t ground_course;
};
struct PACKED log_IMU {
LOG_PACKET_HEADER;
float gyro_x, gyro_y, gyro_z;
float accel_x, accel_y, accel_z;
};
#define LOG_COMMON_STRUCTURES \
{ LOG_PARAMETER_MSG, sizeof(log_Parameter), \
"PARM", "Nf", "Name,Value" }, \
{ LOG_GPS_MSG, sizeof(log_GPS), \
"GPS", "BIBcLLeeEe", "Status,Time,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs" }, \
{ LOG_IMU_MSG, sizeof(log_IMU), \
"IMU", "ffffff", "GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }
// message types for common messages
#define LOG_PARAMETER_MSG 128
#define LOG_GPS_MSG 129
#define LOG_IMU_MSG 130
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

7
libraries/DataFlash/DataFlash_Block.h
View File

@ -16,7 +16,7 @@
// we use an invalie logging format to test the chip erase
#define DF_LOGGING_FORMAT_INVALID 0x28122012
class DataFlash_Block : DataFlash_Class
class DataFlash_Block : public DataFlash_Class
{
public:
// initialisation
@ -43,6 +43,11 @@ public:
void log_read_process(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
void (*callback)(uint8_t msgid));
void LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port);
void DumpPageInfo(AP_HAL::BetterStream *port);
void ShowDeviceInfo(AP_HAL::BetterStream *port);
void ListAvailableLogs(AP_HAL::BetterStream *port);

73
libraries/DataFlash/DataFlash_File.cpp
View File

@ -288,9 +288,82 @@ uint16_t DataFlash_File::start_new_log(void)
fprintf(f, "%u\n", (unsigned)log_num);
fclose(f);
free(fname);
Log_Write_Parameters();
return log_num;
}
/*
Read the log and print it on port
*/
void DataFlash_File::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port)
{
uint8_t log_step = 0;
if (!_initialised) {
return;
}
if (_read_fd != -1) {
::close(_read_fd);
}
char *fname = _log_file_name(log_num);
if (fname == NULL) {
return;
}
_read_fd = ::open(fname, O_RDONLY);
free(fname);
if (_read_fd == -1) {
return;
}
_read_offset = 0;
if (start_page != 0) {
::lseek(_read_fd, start_page * DATAFLASH_PAGE_SIZE, SEEK_SET);
}
_print_format_headers(num_types, structure, port);
while (true) {
uint8_t data;
if (::read(_read_fd, &data, 1) != 1) {
// reached end of file
break;
}
_read_offset++;
// This is a state machine to read the packets
switch(log_step) {
case 0:
if (data == HEAD_BYTE1) {
log_step++;
}
break;
case 1:
if (data == HEAD_BYTE2) {
log_step++;
} else {
log_step = 0;
}
break;
case 2:
log_step = 0;
_print_log_entry(data, num_types, structure, port);
break;
}
if (_read_offset >= (end_page+1) * DATAFLASH_PAGE_SIZE) {
break;
}
}
::close(_read_fd);
_read_fd = -1;
}
/*
start processing a log, called by CLI to display logs
*/

7
libraries/DataFlash/DataFlash_File.h
View File

@ -10,7 +10,7 @@
#ifndef DataFlash_File_h
#define DataFlash_File_h
class DataFlash_File : DataFlash_Class
class DataFlash_File : public DataFlash_Class
{
public:
// constructor
@ -40,6 +40,11 @@ public:
void log_read_process(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
void (*callback)(uint8_t msgid));
void LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port);
void DumpPageInfo(AP_HAL::BetterStream *port);
void ShowDeviceInfo(AP_HAL::BetterStream *port);
void ListAvailableLogs(AP_HAL::BetterStream *port);

308
libraries/DataFlash/LogFile.cpp
View File

@ -2,6 +2,10 @@
#include <AP_HAL.h>
#include "DataFlash.h"
#include <stdlib.h>
#include <AP_Param.h>
extern const AP_HAL::HAL& hal;
// This function determines the number of whole or partial log files in the DataFlash
// Wholly overwritten files are (of course) lost.
@ -53,6 +57,7 @@ uint16_t DataFlash_Block::start_new_log(void)
SetFileNumber(1);
StartWrite(1);
//Serial.println("start log from 0");
Log_Write_Parameters();
return 1;
}
@ -73,6 +78,7 @@ uint16_t DataFlash_Block::start_new_log(void)
SetFileNumber(new_log_num);
StartWrite(last_page + 1);
}
Log_Write_Parameters();
return new_log_num;
}
@ -240,13 +246,221 @@ uint16_t DataFlash_Block::find_last_page_of_log(uint16_t log_number)
return -1;
}
#define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr)
/*
print the log column names
*/
void DataFlash_Class::_print_format_headers(uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port)
{
uint8_t i;
for (i=0; i<num_types; i++) {
port->printf_P(PSTR("FMT, %S, %u, %S, %S\n"),
structure[i].name,
(unsigned)PGM_UINT8(&structure[i].msg_type),
structure[i].format,
structure[i].labels);
}
}
/*
read and print a log entry using the format strings from the given structure
*/
void DataFlash_Class::_print_log_entry(uint8_t msg_type,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port)
{
uint8_t i;
for (i=0; i<num_types; i++) {
if (msg_type == PGM_UINT8(&structure[i].msg_type)) {
break;
}
}
if (i == num_types) {
port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type);
return;
}
uint8_t msg_len = PGM_UINT8(&structure[i].msg_len);
uint8_t pkt[msg_len];
ReadPacket(pkt, msg_len);
port->printf_P(PSTR("%S, "), structure[i].name);
for (uint8_t ofs=3, fmt_ofs=0; ofs<msg_len; fmt_ofs++) {
char fmt = PGM_UINT8(&structure[i].format[fmt_ofs]);
switch (fmt) {
case 'b': {
port->printf_P(PSTR("%d"), (int)pkt[ofs]);
ofs += 1;
break;
}
case 'B': {
port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]);
ofs += 1;
break;
}
case 'h': {
int16_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%d"), (int)v);
ofs += sizeof(v);
break;
}
case 'H': {
uint16_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%u"), (unsigned)v);
ofs += sizeof(v);
break;
}
case 'i': {
int32_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%ld"), (long)v);
ofs += sizeof(v);
break;
}
case 'I': {
uint32_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%lu"), (unsigned long)v);
ofs += sizeof(v);
break;
}
case 'f': {
float v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%f"), v);
ofs += sizeof(v);
break;
}
case 'c': {
int16_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%.2f"), 0.01f*v);
ofs += sizeof(v);
break;
}
case 'C': {
uint16_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%.2f"), 0.01f*v);
ofs += sizeof(v);
break;
}
case 'e': {
int32_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%.2f"), 0.01f*v);
ofs += sizeof(v);
break;
}
case 'E': {
uint32_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
port->printf_P(PSTR("%.2f"), 0.01f*v);
ofs += sizeof(v);
break;
}
case 'L': {
int32_t v;
memcpy(&v, &pkt[ofs], sizeof(v));
int32_t dec_portion, frac_portion;
int32_t abs_lat_or_lon = labs(v);
// extract decimal portion (special handling of negative numbers to ensure we round towards zero)
dec_portion = abs_lat_or_lon / 10000000UL;
// extract fractional portion
frac_portion = abs_lat_or_lon - dec_portion*10000000UL;
// print output including the minus sign
if (v < 0) {
port->printf_P(PSTR("-"));
}
port->printf_P(PSTR("%ld.%07ld"),(long)dec_portion,(long)frac_portion);
ofs += sizeof(v);
break;
}
case 'N': {
char v[17];
memcpy(&v, &pkt[ofs], sizeof(v));
v[16] = 0;
port->printf_P(PSTR("%s"), v);
ofs += 16;
break;
}
default:
ofs = msg_len;
break;
}
if (ofs < msg_len) {
port->printf_P(PSTR(", "));
}
}
port->println();
}
/*
Read the log and print it on port
*/
void DataFlash_Block::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
uint8_t num_types,
const struct LogStructure *structure,
AP_HAL::BetterStream *port)
{
uint8_t log_step = 0;
uint16_t page = start_page;
if (df_BufferIdx != 0) {
FinishWrite();
}
_print_format_headers(num_types, structure, port);
StartRead(start_page);
while (true) {
uint8_t data;
ReadBlock(&data, 1);
// This is a state machine to read the packets
switch(log_step) {
case 0:
if (data == HEAD_BYTE1) {
log_step++;
}
break;
case 1:
if (data == HEAD_BYTE2) {
log_step++;
} else {
log_step = 0;
}
break;
case 2:
log_step = 0;
_print_log_entry(data, num_types, structure, port);
break;
}
uint16_t new_page = GetPage();
if (new_page != page) {
if (new_page == end_page || new_page == start_page) {
return;
}
page = new_page;
}
}
}
/*
Read the DataFlash log memory
Call the callback() function on each log message found in the page
range. Return the number of log messages found
Note that for the block oriented backend the log_num is ignored
This is obsolete and will be removed when plane and copter are
converted to LogReadProcess()
*/
void DataFlash_Block::log_read_process(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
@ -358,3 +572,89 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
}
port->println();
}
/*
write a parameter to the log
*/
void DataFlash_Class::Log_Write_Parameter(const char *name, float value)
{
struct log_Parameter pkt = {
LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
name : {},
value : value
};
strncpy(pkt.name, name, sizeof(pkt.name));
WriteBlock(&pkt, sizeof(pkt));
}
/*
write a parameter to the log
*/
void DataFlash_Class::Log_Write_Parameter(const AP_Param *ap,
const AP_Param::ParamToken &token,
enum ap_var_type type)
{
char name[16];
ap->copy_name_token(token, &name[0], sizeof(name), true);
Log_Write_Parameter(name, ap->cast_to_float(type));
}
/*
write all parameters to the log - used when starting a new log so
the log file has a full record of the parameters
*/
void DataFlash_Class::Log_Write_Parameters(void)
{
AP_Param::ParamToken token;
AP_Param *ap;
enum ap_var_type type;
for (ap=AP_Param::first(&token, &type);
ap;
ap=AP_Param::next_scalar(&token, &type)) {
Log_Write_Parameter(ap, token, type);
// slow down the parameter dump to prevent saturating
// the dataflash write bandwidth
hal.scheduler->delay(1);
}
}
// Write an GPS packet
void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt)
{
struct log_GPS pkt = {
LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
status : (uint8_t)gps->status(),
gps_time : gps->time,
num_sats : gps->num_sats,
hdop : gps->hdop,
latitude : gps->latitude,
longitude : gps->longitude,
rel_altitude : relative_alt,
altitude : gps->altitude,
ground_speed : gps->ground_speed,
ground_course : gps->ground_course
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write an raw accel/gyro data packet
void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor *ins)
{
Vector3f gyro = ins->get_gyro();
Vector3f accel = ins->get_accel();
struct log_IMU pkt = {
LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
gyro_x : gyro.x,
gyro_y : gyro.y,
gyro_z : gyro.z,
accel_x : accel.x,
accel_y : accel.y,
accel_z : accel.z
};
WriteBlock(&pkt, sizeof(pkt));
}