DataFlash: dump the format of logs at the start of the log

This commit is contained in:
Andrew Tridgell 2013-04-19 23:19:16 +10:00
parent 6d6641e1a3
commit c29d870497
3 changed files with 63 additions and 10 deletions

View File

@ -57,6 +57,9 @@ public:
virtual void ListAvailableLogs(AP_HAL::BetterStream *port) = 0;
/* logging methods common to all vehicles */
uint16_t StartNewLog(uint8_t num_types,
const struct LogStructure *structure);
void Log_Write_Format(const struct LogStructure *structure);
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);
@ -110,6 +113,7 @@ Format characters in the format string for binary log messages
i : int32_t
I : uint32_t
f : float
n : char[4]
N : char[16]
c : int16_t * 100
C : uint16_t * 100
@ -130,6 +134,14 @@ struct LogStructure {
/*
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];
};
struct PACKED log_Parameter {
LOG_PACKET_HEADER;
char name[16];
@ -157,6 +169,8 @@ struct PACKED log_IMU {
};
#define LOG_COMMON_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Parameter), \
"FMT", "BBnN", "Type,Length,Name,Format" }, \
{ LOG_PARAMETER_MSG, sizeof(log_Parameter), \
"PARM", "Nf", "Name,Value" }, \
{ LOG_GPS_MSG, sizeof(log_GPS), \
@ -165,9 +179,10 @@ struct PACKED 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
#define LOG_FORMAT_MSG 128
#define LOG_PARAMETER_MSG 129
#define LOG_GPS_MSG 130
#define LOG_IMU_MSG 131
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

View File

@ -289,8 +289,6 @@ uint16_t DataFlash_File::start_new_log(void)
fclose(f);
free(fname);
Log_Write_Parameters();
return log_num;
}

View File

@ -57,7 +57,6 @@ uint16_t DataFlash_Block::start_new_log(void)
SetFileNumber(1);
StartWrite(1);
//Serial.println("start log from 0");
Log_Write_Parameters();
return 1;
}
@ -78,7 +77,6 @@ uint16_t DataFlash_Block::start_new_log(void)
SetFileNumber(new_log_num);
StartWrite(last_page + 1);
}
Log_Write_Parameters();
return new_log_num;
}
@ -257,7 +255,7 @@ void DataFlash_Class::_print_format_headers(uint8_t num_types,
{
uint8_t i;
for (i=0; i<num_types; i++) {
port->printf_P(PSTR("FMT, %S, %u, %S, %S\n"),
port->printf_P(PSTR("LABL, %S, %u, %S, %S\n"),
structure[i].name,
(unsigned)PGM_UINT8(&structure[i].msg_type),
structure[i].format,
@ -383,12 +381,20 @@ void DataFlash_Class::_print_log_entry(uint8_t msg_type,
ofs += sizeof(v);
break;
}
case 'n': {
char v[5];
memcpy(&v, &pkt[ofs], sizeof(v));
v[sizeof(v)-1] = 0;
port->printf_P(PSTR("%s"), v);
ofs += sizeof(v)-1;
break;
}
case 'N': {
char v[17];
memcpy(&v, &pkt[ofs], sizeof(v));
v[16] = 0;
v[sizeof(v)-1] = 0;
port->printf_P(PSTR("%s"), v);
ofs += 16;
ofs += sizeof(v)-1;
break;
}
default:
@ -573,6 +579,40 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
port->println();
}
// This function starts a new log file in the DataFlash, and writes
// the format of supported messages in the log, plus all parameters
uint16_t DataFlash_Class::StartNewLog(uint8_t num_types, const struct LogStructure *structures)
{
uint16_t ret;
ret = start_new_log();
// write log formats so the log is self-describing
for (uint8_t i=0; i<num_types; i++) {
Log_Write_Format(&structures[i]);
}
// and all current parameters
Log_Write_Parameters();
return ret;
}
/*
write a structure format to the log
*/
void DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
{
struct log_Format pkt = {
LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
type : PGM_UINT8(&s->msg_type),
length : PGM_UINT8(&s->msg_len),
name : {},
format : {}
};
strncpy_P(pkt.name, s->name, sizeof(pkt.name));
strncpy_P(pkt.format, s->format, sizeof(pkt.format));
WriteBlock(&pkt, sizeof(pkt));
}
/*
write a parameter to the log
*/