zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #287 from DrTon/sdlog2 

sdlog2 - new APM compatible logger
sbg
Lorenz Meier 12 years ago
parent
68931f38d5 aedacc7bc8
commit
39d6dd3dc6
8 changed files with 1909 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 293
      Tools/sdlog2_dump.py
  2. 1
      makefiles/config_px4fmu_default.mk
  3. 133
      src/modules/sdlog2/logbuffer.c
  4. 68
      src/modules/sdlog2/logbuffer.h
  5. 43
      src/modules/sdlog2/module.mk
  6. 1121
      src/modules/sdlog2/sdlog2.c
  7. 98
      src/modules/sdlog2/sdlog2_format.h
  8. 152
      src/modules/sdlog2/sdlog2_messages.h

293
Tools/sdlog2_dump.py
Unescape View File

@ -0,0 +1,293 @@ @@ -0,0 +1,293 @@
#!/usr/bin/env python
"""Dump binary log generated by sdlog2 or APM as CSV
Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]
-v Use plain debug output instead of CSV.
-e Recover from errors.
-d Use "delimiter" in CSV. Default is ",".
-n Use "null" as placeholder for empty values in CSV. Default is empty.
-m MSG[.field1,field2,...]
Dump only messages of specified type, and only specified fields.
Multiple -m options allowed."""
__author__ = "Anton Babushkin"
__version__ = "1.2"
import struct, sys
class SDLog2Parser:
BLOCK_SIZE = 8192
MSG_HEADER_LEN = 3
MSG_HEAD1 = 0xA3
MSG_HEAD2 = 0x95
MSG_FORMAT_PACKET_LEN = 89
MSG_FORMAT_STRUCT = "BB4s16s64s"
MSG_TYPE_FORMAT = 0x80
FORMAT_TO_STRUCT = {
"b": ("b", None),
"B": ("B", None),
"h": ("h", None),
"H": ("H", None),
"i": ("i", None),
"I": ("I", None),
"f": ("f", None),
"n": ("4s", None),
"N": ("16s", None),
"Z": ("64s", None),
"c": ("h", 0.01),
"C": ("H", 0.01),
"e": ("i", 0.01),
"E": ("I", 0.01),
"L": ("i", 0.0000001),
"M": ("b", None),
"q": ("q", None),
"Q": ("Q", None),
}
__csv_delim = ","
__csv_null = ""
__msg_filter = []
__time_msg = None
__debug_out = False
__correct_errors = False
def __init__(self):
return
def reset(self):
self.__msg_descrs = {} # message descriptions by message type map
self.__msg_labels = {} # message labels by message name map
self.__msg_names = [] # message names in the same order as FORMAT messages
self.__buffer = "" # buffer for input binary data
self.__ptr = 0 # read pointer in buffer
self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label"
self.__csv_data = {} # current values for all columns
self.__csv_updated = False
self.__msg_filter_map = {} # filter in form of map, with '*" expanded to full list of fields
def setCSVDelimiter(self, csv_delim):
self.__csv_delim = csv_delim
def setCSVNull(self, csv_null):
self.__csv_null = csv_null
def setMsgFilter(self, msg_filter):
self.__msg_filter = msg_filter
def setTimeMsg(self, time_msg):
self.__time_msg = time_msg
def setDebugOut(self, debug_out):
self.__debug_out = debug_out
def setCorrectErrors(self, correct_errors):
self.__correct_errors = correct_errors
def process(self, fn):
self.reset()
if self.__debug_out:
# init __msg_filter_map
for msg_name, show_fields in self.__msg_filter:
self.__msg_filter_map[msg_name] = show_fields
first_data_msg = True
f = open(fn, "r")
bytes_read = 0
while True:
chunk = f.read(self.BLOCK_SIZE)
if len(chunk) == 0:
break
self.__buffer = self.__buffer[self.__ptr:] + chunk
self.__ptr = 0
while self.__bytesLeft() >= self.MSG_HEADER_LEN:
head1 = ord(self.__buffer[self.__ptr])
head2 = ord(self.__buffer[self.__ptr+1])
if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2):
if self.__correct_errors:
self.__ptr += 1
continue
else:
raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2))
msg_type = ord(self.__buffer[self.__ptr+2])
if msg_type == self.MSG_TYPE_FORMAT:
# parse FORMAT message
if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN:
break
self.__parseMsgDescr()
else:
# parse data message
msg_descr = self.__msg_descrs[msg_type]
if msg_descr == None:
raise Exception("Unknown msg type: %i" % msg_type)
msg_length = msg_descr[0]
if self.__bytesLeft() < msg_length:
break
if first_data_msg:
# build CSV columns and init data map
self.__initCSV()
first_data_msg = False
self.__parseMsg(msg_descr)
bytes_read += self.__ptr
if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
self.__printCSVRow()
f.close()
def __bytesLeft(self):
return len(self.__buffer) - self.__ptr
def __filterMsg(self, msg_name):
show_fields = "*"
if len(self.__msg_filter_map) > 0:
show_fields = self.__msg_filter_map.get(msg_name)
return show_fields
def __initCSV(self):
if len(self.__msg_filter) == 0:
for msg_name in self.__msg_names:
self.__msg_filter.append((msg_name, "*"))
for msg_name, show_fields in self.__msg_filter:
if show_fields == "*":
show_fields = self.__msg_labels.get(msg_name, [])
self.__msg_filter_map[msg_name] = show_fields
for field in show_fields:
full_label = msg_name + "." + field
self.__csv_columns.append(full_label)
self.__csv_data[full_label] = None
print self.__csv_delim.join(self.__csv_columns)
def __printCSVRow(self):
s = []
for full_label in self.__csv_columns:
v = self.__csv_data[full_label]
if v == None:
v = self.__csv_null
else:
v = str(v)
s.append(v)
print self.__csv_delim.join(s)
def __parseMsgDescr(self):
data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
msg_type = data[0]
if msg_type != self.MSG_TYPE_FORMAT:
msg_length = data[1]
msg_name = data[2].strip("\0")
msg_format = data[3].strip("\0")
msg_labels = data[4].strip("\0").split(",")
# Convert msg_format to struct.unpack format string
msg_struct = ""
msg_mults = []
for c in msg_format:
try:
f = self.FORMAT_TO_STRUCT[c]
msg_struct += f[0]
msg_mults.append(f[1])
except KeyError as e:
raise Exception("Unsupported format char: %s in message %s (%i)" % (c, msg_name, msg_type))
msg_struct = "<" + msg_struct # force little-endian
self.__msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults)
self.__msg_labels[msg_name] = msg_labels
self.__msg_names.append(msg_name)
if self.__debug_out:
if self.__filterMsg(msg_name) != None:
print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)
self.__ptr += self.MSG_FORMAT_PACKET_LEN
def __parseMsg(self, msg_descr):
msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr
if not self.__debug_out and self.__time_msg != None and msg_name == self.__time_msg and self.__csv_updated:
self.__printCSVRow()
self.__csv_updated = False
show_fields = self.__filterMsg(msg_name)
if (show_fields != None):
data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
for i in xrange(len(data)):
if type(data[i]) is str:
data[i] = data[i].strip("\0")
m = msg_mults[i]
if m != None:
data[i] = data[i] * m
if self.__debug_out:
s = []
for i in xrange(len(data)):
label = msg_labels[i]
if show_fields == "*" or label in show_fields:
s.append(label + "=" + str(data[i]))
print "MSG %s: %s" % (msg_name, ", ".join(s))
else:
# update CSV data buffer
for i in xrange(len(data)):
label = msg_labels[i]
if label in show_fields:
self.__csv_data[msg_name + "." + label] = data[i]
if self.__time_msg != None and msg_name != self.__time_msg:
self.__csv_updated = True
if self.__time_msg == None:
self.__printCSVRow()
self.__ptr += msg_length
def _main():
if len(sys.argv) < 2:
print "Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n"
print "\t-v\tUse plain debug output instead of CSV.\n"
print "\t-e\tRecover from errors.\n"
print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n"
print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n"
print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed."
print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n"
return
fn = sys.argv[1]
debug_out = False
correct_errors = False
msg_filter = []
csv_null = ""
csv_delim = ","
time_msg = None
opt = None
for arg in sys.argv[2:]:
if opt != None:
if opt == "d":
csv_delim = arg
elif opt == "n":
csv_null = arg
elif opt == "t":
time_msg = arg
elif opt == "m":
show_fields = "*"
a = arg.split(".")
if len(a) > 1:
show_fields = a[1].split(",")
msg_filter.append((a[0], show_fields))
opt = None
else:
if arg == "-v":
debug_out = True
elif arg == "-e":
correct_errors = True
elif arg == "-d":
opt = "d"
elif arg == "-n":
opt = "n"
elif arg == "-m":
opt = "m"
elif arg == "-t":
opt = "t"
if csv_delim == "\\t":
csv_delim = "\t"
parser = SDLog2Parser()
parser.setCSVDelimiter(csv_delim)
parser.setCSVNull(csv_null)
parser.setMsgFilter(msg_filter)
parser.setTimeMsg(time_msg)
parser.setDebugOut(debug_out)
parser.setCorrectErrors(correct_errors)
parser.process(fn)
if __name__ == "__main__":
_main()

1
makefiles/config_px4fmu_default.mk
Unescape View File

@ -79,6 +79,7 @@ MODULES += modules/multirotor_pos_control @@ -79,6 +79,7 @@ MODULES += modules/multirotor_pos_control
# Logging
#
MODULES += modules/sdlog
MODULES += modules/sdlog2
#
# Library modules

133
src/modules/sdlog2/logbuffer.c
Unescape View File

@ -0,0 +1,133 @@ @@ -0,0 +1,133 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file logbuffer.c
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include "logbuffer.h"
void logbuffer_init(struct logbuffer_s *lb, int size)
{
lb->size = size;
lb->write_ptr = 0;
lb->read_ptr = 0;
lb->data = malloc(lb->size);
}
int logbuffer_count(struct logbuffer_s *lb)
{
int n = lb->write_ptr - lb->read_ptr;
if (n < 0) {
n += lb->size;
}
return n;
}
int logbuffer_is_empty(struct logbuffer_s *lb)
{
return lb->read_ptr == lb->write_ptr;
}
bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
{
// bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1;
if (available < 0)
available += lb->size;
if (size > available) {
// buffer overflow
return false;
}
char *c = (char *) ptr;
int n = lb->size - lb->write_ptr; // bytes to end of the buffer
if (n < size) {
// message goes over end of the buffer
memcpy(&(lb->data[lb->write_ptr]), c, n);
lb->write_ptr = 0;
} else {
n = 0;
}
// now: n = bytes already written
int p = size - n; // number of bytes to write
memcpy(&(lb->data[lb->write_ptr]), &(c[n]), p);
lb->write_ptr = (lb->write_ptr + p) % lb->size;
return true;
}
int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part)
{
// bytes available to read
int available = lb->write_ptr - lb->read_ptr;
if (available == 0) {
return 0; // buffer is empty
}
int n = 0;
if (available > 0) {
// read pointer is before write pointer, all available bytes can be read
n = available;
*is_part = false;
} else {
// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
n = lb->size - lb->read_ptr;
*is_part = true;
}
*ptr = &(lb->data[lb->read_ptr]);
return n;
}
void logbuffer_mark_read(struct logbuffer_s *lb, int n)
{
lb->read_ptr = (lb->read_ptr + n) % lb->size;
}

68
src/modules/sdlog2/logbuffer.h
Unescape View File

@ -0,0 +1,68 @@ @@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file logbuffer.h
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
#ifndef SDLOG2_RINGBUFFER_H_
#define SDLOG2_RINGBUFFER_H_
#include <stdbool.h>
struct logbuffer_s {
// all pointers are in bytes
int write_ptr;
int read_ptr;
int size;
char *data;
};
void logbuffer_init(struct logbuffer_s *lb, int size);
int logbuffer_count(struct logbuffer_s *lb);
int logbuffer_is_empty(struct logbuffer_s *lb);
bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size);
int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part);
void logbuffer_mark_read(struct logbuffer_s *lb, int n);
#endif

43
src/modules/sdlog2/module.mk
Unescape View File

@ -0,0 +1,43 @@ @@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# sdlog2 Application
#
MODULE_COMMAND = sdlog2
# The main thread only buffers to RAM, needs a high priority
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
SRCS = sdlog2.c \
logbuffer.c

1121
src/modules/sdlog2/sdlog2.c
View File

File diff suppressed because it is too large Load Diff

98
src/modules/sdlog2/sdlog2_format.h
Unescape View File

@ -0,0 +1,98 @@ @@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sdlog2_format.h
*
* General log format structures and macro.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
/*
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[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
*/
#ifndef SDLOG2_FORMAT_H_
#define SDLOG2_FORMAT_H_
#define LOG_PACKET_HEADER_LEN 3
#define LOG_PACKET_HEADER uint8_t head1, head2, msg_type;
#define LOG_PACKET_HEADER_INIT(id) .head1 = HEAD_BYTE1, .head2 = HEAD_BYTE2, .msg_type = id
// 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
struct log_format_s {
uint8_t type;
uint8_t length; // full packet length including header
char name[4];
char format[16];
char labels[64];
};
#define LOG_FORMAT(_name, _format, _labels) { \
.type = LOG_##_name##_MSG, \
.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
}
#define LOG_FORMAT_MSG 0x80
#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
#endif /* SDLOG2_FORMAT_H_ */

152
src/modules/sdlog2/sdlog2_messages.h
Unescape View File

@ -0,0 +1,152 @@ @@ -0,0 +1,152 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sdlog2_messages.h
*
* Log messages and structures definition.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
#ifndef SDLOG2_MESSAGES_H_
#define SDLOG2_MESSAGES_H_
#include "sdlog2_format.h"
/* define message formats */
#pragma pack(push, 1)
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 1
struct log_TIME_s {
uint64_t t;
};
/* --- ATT - ATTITUDE --- */
#define LOG_ATT_MSG 2
struct log_ATT_s {
float roll;
float pitch;
float yaw;
};
/* --- ATSP - ATTITUDE SET POINT --- */
#define LOG_ATSP_MSG 3
struct log_ATSP_s {
float roll_sp;
float pitch_sp;
float yaw_sp;
};
/* --- IMU - IMU SENSORS --- */
#define LOG_IMU_MSG 4
struct log_IMU_s {
float acc_x;
float acc_y;
float acc_z;
float gyro_x;
float gyro_y;
float gyro_z;
float mag_x;
float mag_y;
float mag_z;
};
/* --- SENS - OTHER SENSORS --- */
#define LOG_SENS_MSG 5
struct log_SENS_s {
float baro_pres;
float baro_alt;
float baro_temp;
float diff_pres;
};
/* --- LPOS - LOCAL POSITION --- */
#define LOG_LPOS_MSG 6
struct log_LPOS_s {
float x;
float y;
float z;
float vx;
float vy;
float vz;
float hdg;
int32_t home_lat;
int32_t home_lon;
float home_alt;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
#define LOG_LPSP_MSG 7
struct log_LPSP_s {
float x;
float y;
float z;
float yaw;
};
/* --- GPS - GPS POSITION --- */
#define LOG_GPS_MSG 8
struct log_GPS_s {
uint64_t gps_time;
uint8_t fix_type;
float eph;
float epv;
int32_t lat;
int32_t lon;
float alt;
float vel_n;
float vel_e;
float vel_d;
float cog;
};
#pragma pack(pop)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(ATT, "fff", "Roll,Pitch,Yaw"),
LOG_FORMAT(ATSP, "fff", "RollSP,PitchSP,YawSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
#endif /* SDLOG2_MESSAGES_H_ */
Write Preview
Loading…
Cancel
Save