From 916e8d099207c702c0c5d9da6d08002ab95d5d59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Apr 2013 17:49:16 +1000 Subject: [PATCH] DataFlash: new dataflash logging system 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 --- libraries/DataFlash/DataFlash.h | 109 ++++++++- libraries/DataFlash/DataFlash_Block.h | 7 +- libraries/DataFlash/DataFlash_File.cpp | 73 ++++++ libraries/DataFlash/DataFlash_File.h | 7 +- libraries/DataFlash/LogFile.cpp | 308 ++++++++++++++++++++++++- 5 files changed, 495 insertions(+), 9 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index ae364db9d3..433bf38772 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -6,6 +6,11 @@ #ifndef DataFlash_h #define DataFlash_h +#include +#include +#include +#include +#include #include // 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, - uint16_t start_page, uint16_t end_page, - void (*callback)(uint8_t msgid)) = 0; + void log_read_process(uint16_t log_num, + uint16_t start_page, uint16_t end_page, + 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" diff --git a/libraries/DataFlash/DataFlash_Block.h b/libraries/DataFlash/DataFlash_Block.h index 3f65471e77..a66ccde405 100644 --- a/libraries/DataFlash/DataFlash_Block.h +++ b/libraries/DataFlash/DataFlash_Block.h @@ -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); diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index f0a2c03603..fd1f4b740e 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -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 */ diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index 3e1a66dadc..986931e7ee 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -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); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 29ec48964a..81d812c038 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -2,6 +2,10 @@ #include #include "DataFlash.h" +#include +#include + +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; iprintf_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; iprintf_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; ofsprintf_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)); +} +