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

DataFlash: Log_Write support

Browse Source 
A generic logging method to avoid the need to set up a format and structures etc
This commit is contained in:
Peter Barker 2016-04-20 15:07:48 +10:00 committed by Andrew Tridgell
parent 334af1ecd7
commit 77dd170e03
5 changed files with 410 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
libraries/DataFlash/DataFlash.cpp
View File

@ -214,4 +214,157 @@ uint32_t DataFlash_Class::num_dropped() const
// end functions pass straight through to backend
void DataFlash_Class::internal_error() const {
// _internal_errors++;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
abort();
#endif
}
/* Log_Write support */
void DataFlash_Class::Log_Write(const char *name, const char *labels, const char *fmt, ...)
{
va_list arg_list;
int16_t msg_type = msg_type_for_name(name, labels, fmt);
if (msg_type == -1) {
// unable to map name to a messagetype; could be out of
// msgtypes, could be out of slots, ...
internal_error();
return;
}
for (uint8_t i=0; i<_next_backend; i++) {
va_start(arg_list, fmt);
if (backends[i]->Log_Write_Emit_FMT(msg_type)) {
backends[i]->Log_Write(msg_type, arg_list);
} else {
// backend failed to write format (and has never written it)
}
va_end(arg_list);
}
}
int16_t DataFlash_Class::msg_type_for_name(const char *name, const char *labels, const char *fmt)
{
for (uint8_t i=0; i<_log_write_fmt_count;i++) {
if (log_write_fmts[i].name == name) { // ptr comparison
// already have an ID for this name:
return log_write_fmts[i].msg_type;
}
}
if (_log_write_fmt_count >= ARRAY_SIZE(log_write_fmts)) {
// out of slots to remember formats
return -1;
}
// no message type allocated for this name. Try to allocate one:
int16_t msg_type = find_free_msg_type();
if (msg_type == -1) {
return -1;
}
log_write_fmts[_log_write_fmt_count].msg_type = msg_type;
log_write_fmts[_log_write_fmt_count].name = name;
log_write_fmts[_log_write_fmt_count].fmt = fmt;
log_write_fmts[_log_write_fmt_count].labels = labels;
int16_t tmp = Log_Write_calc_msg_len(fmt);
if (tmp == -1) {
return -1;
}
log_write_fmts[_log_write_fmt_count].msg_len = tmp;
_log_write_fmt_count++;
return msg_type;
}
// returns true if the msg_type is already taken
bool DataFlash_Class::msg_type_in_use(const uint8_t msg_type) const
{
// check static list of messages (e.g. from LOG_BASE_STRUCTURES)
// check the write format types to see if we've used this one
for (uint16_t i=0; i<_num_types;i++) {
if (structure(i)->msg_type == msg_type) {
// in use
return true;
}
}
// check the write messages we've already put out:
for (uint8_t i=0; i<_log_write_fmt_count; i++) {
if (log_write_fmts[i].msg_type == msg_type) {
return true;
}
}
return false;
}
// find a free message type
int16_t DataFlash_Class::find_free_msg_type() const
{
// avoid using 255 here; perhaps we want to use it to extend things later
for (uint16_t msg_type=254; msg_type>0; msg_type--) { // more likely to be free at end
if (! msg_type_in_use(msg_type)) {
return msg_type;
}
}
return -1;
}
bool DataFlash_Class::fill_log_write_logstructure(struct LogStructure &logstruct, const uint8_t msg_type) const
{
// find log structure information corresponding to msg_type:
uint8_t i;
for (i=0; i<_log_write_fmt_count; i++) {
if(log_write_fmts[i].msg_type == msg_type) {
break;
}
}
if (i >= _log_write_fmt_count) {
// asked to fill for a type we don't recognise
internal_error();
return false;
}
logstruct.msg_type = msg_type;
strncpy((char*)logstruct.name, log_write_fmts[i].name, sizeof(logstruct.name)); /* cast away the "const" (*gulp*) */
strncpy((char*)logstruct.format, log_write_fmts[i].fmt, sizeof(logstruct.format));
strncpy((char*)logstruct.labels, log_write_fmts[i].labels, sizeof(logstruct.labels));
logstruct.msg_len = log_write_fmts[i].msg_len;
return true;
}
/* calculate the length of output of a format string. Note that this
* returns an int16_t; if it returns -1 then an error has occured.
* This was mechanically converted from init_field_types in
* Tools/Replay/MsgHandler.cpp */
int16_t DataFlash_Class::Log_Write_calc_msg_len(const char *fmt) const
{
uint8_t len = LOG_PACKET_HEADER_LEN;
for (uint8_t i=0; i<strlen(fmt); i++) {
switch(fmt[i]) {
case 'b' : len += sizeof(int8_t); break;
case 'c' : len += sizeof(int16_t); break;
case 'd' : len += sizeof(double); break;
case 'e' : len += sizeof(int32_t); break;
case 'f' : len += sizeof(float); break;
case 'h' : len += sizeof(int16_t); break;
case 'i' : len += sizeof(int32_t); break;
case 'n' : len += sizeof(char[4]); break;
case 'B' : len += sizeof(uint8_t); break;
case 'C' : len += sizeof(uint16_t); break;
case 'E' : len += sizeof(uint32_t); break;
case 'H' : len += sizeof(uint16_t); break;
case 'I' : len += sizeof(uint32_t); break;
case 'L' : len += sizeof(int32_t); break;
case 'M' : len += sizeof(uint8_t); break;
case 'N' : len += sizeof(char[16]); break;
case 'Z' : len += sizeof(char[64]); break;
case 'q' : len += sizeof(int64_t); break;
case 'Q' : len += sizeof(uint64_t); break;
default: return -1;
}
}
return len;
}
/* End of Log_Write support */
#undef FOR_EACH_BACKEND

39
libraries/DataFlash/DataFlash.h
View File

@ -5,6 +5,9 @@
/* ************************************************************ */
#pragma once
// maximum number of dynamic Log_Write formats supported
#define MAX_LOG_WRITE_FMT_COUNT 5
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
@ -133,6 +136,8 @@ public:
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control);
void Log_Write(const char *name, const char *labels, const char *fmt, ...);
// This structure provides information on the internal member data of a PID for logging purposes
struct PID_Info {
float desired;
@ -189,4 +194,38 @@ private:
uint8_t _next_backend;
DataFlash_Backend *backends[DATAFLASH_MAX_BACKENDS];
const char *_firmware_string;
void internal_error() const;
/*
* support for dynamic Log_Write; user-supplies name, format,
* labels and values in a single function call.
*/
// this structure looks much like struct LogStructure in
// LogStructure.h, however we need to remember a pointer value for
// efficiency of finding message types
struct {
uint8_t msg_type;
uint8_t msg_len;
const char *name;
const char *fmt;
const char *labels;
} log_write_fmts[MAX_LOG_WRITE_FMT_COUNT];
uint8_t _log_write_fmt_count = 0; // number of formats currently known
// return (possibly allocating) a msg_type value corresponding to name
int16_t msg_type_for_name(const char *name, const char *labels, const char *fmt);
// returns true if msg_type is associated with a message
bool msg_type_in_use(uint8_t msg_type) const;
// return a msg_type which is not currently in use (or -1 if none available)
int16_t find_free_msg_type() const;
// fill LogStructure with information about msg_type
bool fill_log_write_logstructure(struct LogStructure &logstruct, const uint8_t msg_type) const;
// calculate the length of a message using fields specified in
// fmt; includes the message header
int16_t Log_Write_calc_msg_len(const char *fmt) const;
};

199
libraries/DataFlash/DataFlash_Backend.cpp
View File

@ -53,6 +53,7 @@ void DataFlash_Backend::periodic_tasks()
void DataFlash_Backend::start_new_log_reset_variables()
{
_startup_messagewriter->reset();
memset(log_write_fmts_sent,0,sizeof(log_write_fmts_sent));
}
void DataFlash_Backend::internal_error() {
@ -109,3 +110,201 @@ void DataFlash_Backend::WriteMoreStartupMessages()
_startup_messagewriter->process();
_writing_startup_messages = false;
}
/*
* support for Log_Write():
*/
bool DataFlash_Backend::Log_Write_Emit_FMT(uint8_t msg_type)
{
for(uint8_t i=0;i<_written_log_write_fmt_count;i++) {
if(log_write_fmts_sent[i] == msg_type) {
// have already emitted this format
return true;
}
}
// have not emitted this format; try to do so now.
if (_written_log_write_fmt_count >= ARRAY_SIZE(log_write_fmts_sent)) {
// this is a bug; frontend should never ask us to write more
// formats than we have room to store "sent" markers for
internal_error();
return false;
}
// get log structure from front end:
struct LogStructure logstruct = {
// these will be overwritten, but need to keep the compiler happy:
0,
0,
"IGNO",
"",
""
};
if (!_front.fill_log_write_logstructure(logstruct, msg_type)) {
// this is a bug; we've been asked to write out the FMT
// message for a msg_type, but the frontend can't supply the
// required information
internal_error();
return false;
}
if (!Log_Write_Format(&logstruct)) {
return false;
}
log_write_fmts_sent[_written_log_write_fmt_count] = msg_type;
_written_log_write_fmt_count++;
return true;
}
bool DataFlash_Backend::Log_Write(const uint8_t msg_type, va_list arg_list, bool is_critical)
{
// stack-allocate a buffer so we can WriteBlock(); this could be
// 255 bytes! If we were willing to lose the WriteBlock
// abstraction we could do WriteBytes() here instead?
const char *fmt = NULL;
uint8_t msg_len;
for (uint8_t i=0; i<_written_log_write_fmt_count;i++) {
if (_front.log_write_fmts[i].msg_type == msg_type) {
fmt = _front.log_write_fmts[i].fmt;
msg_len = _front.log_write_fmts[i].msg_len;
break;
}
}
if (fmt == NULL) {
// this is a bug.
internal_error();
return false;
}
if (bufferspace_available() < msg_len) {
return false;
}
uint8_t buffer[msg_len];
uint8_t offset = 0;
buffer[offset++] = HEAD_BYTE1;
buffer[offset++] = HEAD_BYTE2;
buffer[offset++] = msg_type;
for (uint8_t i=0; i<strlen(fmt); i++) {
// this was mechanically converted....
switch(fmt[i]) {
case 'b': {
int8_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int8_t));
offset += sizeof(int8_t);
break;
}
case 'c': {
int16_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int16_t));
offset += sizeof(int16_t);
break;
}
case 'd': {
double tmp = va_arg(arg_list, double);
memcpy(&buffer[offset], &tmp, sizeof(double));
offset += sizeof(double);
break;
}
case 'e': {
int32_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int32_t));
offset += sizeof(int32_t);
break;
}
case 'f': {
float tmp = va_arg(arg_list, double);
memcpy(&buffer[offset], &tmp, sizeof(float));
offset += sizeof(float);
break;
}
case 'h': {
int16_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int16_t));
offset += sizeof(int16_t);
break;
}
case 'i': {
int32_t tmp = va_arg(arg_list, int32_t);
memcpy(&buffer[offset], &tmp, sizeof(int32_t));
offset += sizeof(int32_t);
break;
}
case 'n': {
char *tmp = va_arg(arg_list, char*);
memcpy(&buffer[offset], tmp, 4);
offset += 4;
break;
}
case 'B': {
uint8_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
offset += sizeof(uint8_t);
break;
}
case 'C': {
uint16_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(uint16_t));
offset += sizeof(uint16_t);
break;
}
case 'E': {
uint32_t tmp = va_arg(arg_list, uint32_t);
memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
offset += sizeof(uint32_t);
break;
}
case 'H': {
uint16_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(uint16_t));
offset += sizeof(uint16_t);
break;
}
case 'I': {
uint32_t tmp = va_arg(arg_list, uint32_t);
memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
offset += sizeof(uint32_t);
break;
}
case 'L': {
int32_t tmp = va_arg(arg_list, int32_t);
memcpy(&buffer[offset], &tmp, sizeof(int32_t));
offset += sizeof(int32_t);
break;
}
case 'M': {
uint8_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
offset += sizeof(uint8_t);
break;
}
case 'N': {
char *tmp = va_arg(arg_list, char*);
memcpy(&buffer[offset], tmp, 16);
offset += 16;
break;
}
case 'Z': {
char *tmp = va_arg(arg_list, char*);
memcpy(&buffer[offset], tmp, 64);
offset += 64;
break;
}
case 'q': {
int64_t tmp = va_arg(arg_list, int64_t);
memcpy(&buffer[offset], &tmp, sizeof(int64_t));
offset += sizeof(int64_t);
break;
}
case 'Q': {
uint64_t tmp = va_arg(arg_list, uint64_t);
memcpy(&buffer[offset], &tmp, sizeof(uint64_t));
offset += sizeof(uint64_t);
break;
}
}
}
return WritePrioritisedBlock(buffer, msg_len, is_critical);
}

19
libraries/DataFlash/DataFlash_Backend.h
View File

@ -97,7 +97,18 @@ public:
uint32_t num_dropped(void) const {
return _dropped;
}
/*
* Log_Write support
*/
// write a FMT message out (if it hasn't been done already).
// Returns true if the FMT message has ever been written.
bool Log_Write_Emit_FMT(uint8_t msg_type);
// write a log message out to the log of msg_type type, with
// values contained in arg_list:
bool Log_Write(uint8_t msg_type, va_list arg_list, bool is_critical=false);
protected:
uint32_t dropped;
uint8_t internal_errors; // uint8_t - wishful thinking?
@ -139,4 +150,10 @@ private:
uint32_t _last_periodic_1Hz;
uint32_t _last_periodic_10Hz;
// Log_Write support
uint8_t _written_log_write_fmt_count;
// array containing msg_types of formats already emitted
uint8_t log_write_fmts_sent[MAX_LOG_WRITE_FMT_COUNT];
};

1
libraries/DataFlash/LogStructure.h
View File

@ -6,6 +6,7 @@
*/
#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