forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
Lorenz Meier
commit
b52aeea746
|
|
@ -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()
|
||||
|
|
@ -62,7 +62,8 @@ MODULES += modules/gpio_led
|
|||
# Estimation modules (EKF / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/position_estimator_mc
|
||||
MODULES += modules/attitude_estimator_so3_comp
|
||||
#MODULES += modules/position_estimator_mc
|
||||
MODULES += modules/position_estimator
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
|
||||
|
|
@ -79,6 +80,7 @@ MODULES += modules/multirotor_pos_control
|
|||
# Logging
|
||||
#
|
||||
MODULES += modules/sdlog
|
||||
MODULES += modules/sdlog2
|
||||
|
||||
#
|
||||
# Library modules
|
||||
|
|
|
|||
|
|
@ -70,6 +70,14 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
|
|||
-march=armv7-m \
|
||||
-mfloat-abi=soft
|
||||
|
||||
ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
|
||||
-ffixed-r10
|
||||
|
||||
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
|
||||
-ffixed-r10
|
||||
|
||||
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
|
||||
|
||||
# Pick the right set of flags for the architecture.
|
||||
#
|
||||
ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
|
||||
|
|
@ -91,8 +99,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
|
|||
|
||||
# enable precise stack overflow tracking
|
||||
# note - requires corresponding support in NuttX
|
||||
INSTRUMENTATIONDEFINES = -finstrument-functions \
|
||||
-ffixed-r10
|
||||
INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
|
||||
|
||||
# Language-specific flags
|
||||
#
|
||||
ARCHCFLAGS = -std=gnu99
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Example User <mail@example.com>
|
||||
* Copyright (c) 2012, 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
|
||||
|
|
@ -33,27 +32,33 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4_deamon_app.c
|
||||
* Deamon application example for PX4 autopilot
|
||||
* @file px4_daemon_app.c
|
||||
* daemon application example for PX4 autopilot
|
||||
*
|
||||
* @author Example User <mail@example.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
static bool thread_should_exit = false; /**< daemon exit flag */
|
||||
static bool thread_running = false; /**< daemon status flag */
|
||||
static int daemon_task; /**< Handle of daemon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
* daemon management function.
|
||||
*/
|
||||
__EXPORT int px4_deamon_app_main(int argc, char *argv[]);
|
||||
__EXPORT int px4_daemon_app_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
* Mainloop of daemon.
|
||||
*/
|
||||
int px4_deamon_thread_main(int argc, char *argv[]);
|
||||
int px4_daemon_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
|
|
@ -64,20 +69,19 @@ static void
|
|||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
warnx("%s\n", reason);
|
||||
errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* The daemon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int px4_deamon_app_main(int argc, char *argv[])
|
||||
int px4_daemon_app_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
|
@ -85,17 +89,17 @@ int px4_deamon_app_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("deamon already running\n");
|
||||
warnx("daemon already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("deamon",
|
||||
daemon_task = task_spawn("daemon",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
4096,
|
||||
px4_deamon_thread_main,
|
||||
px4_daemon_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -107,9 +111,9 @@ int px4_deamon_app_main(int argc, char *argv[])
|
|||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tdeamon app is running\n");
|
||||
warnx("\trunning\n");
|
||||
} else {
|
||||
printf("\tdeamon app not started\n");
|
||||
warnx("\tnot started\n");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -118,18 +122,18 @@ int px4_deamon_app_main(int argc, char *argv[])
|
|||
exit(1);
|
||||
}
|
||||
|
||||
int px4_deamon_thread_main(int argc, char *argv[]) {
|
||||
int px4_daemon_thread_main(int argc, char *argv[]) {
|
||||
|
||||
printf("[deamon] starting\n");
|
||||
warnx("[daemon] starting\n");
|
||||
|
||||
thread_running = true;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
printf("Hello Deamon!\n");
|
||||
warnx("Hello daemon!\n");
|
||||
sleep(10);
|
||||
}
|
||||
|
||||
printf("[deamon] exiting.\n");
|
||||
warnx("[daemon] exiting.\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
Synopsis
|
||||
|
||||
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
|
||||
|
||||
Option -d is for debugging packet. See code for detailed packet structure.
|
||||
|
|
@ -0,0 +1,833 @@
|
|||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_main.c
|
||||
*
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <poll.h>
|
||||
#include <fcntl.h>
|
||||
#include <float.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <limits.h>
|
||||
#include <math.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#include "attitude_estimator_so3_comp_params.h"
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
|
||||
static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
|
||||
static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
|
||||
static bool bFilterInit = false;
|
||||
|
||||
//! Auxiliary variables to reduce number of repeated operations
|
||||
static float q0q0, q0q1, q0q2, q0q3;
|
||||
static float q1q1, q1q2, q1q3;
|
||||
static float q2q2, q2q3;
|
||||
static float q3q3;
|
||||
|
||||
//! Serial packet related
|
||||
static int uart;
|
||||
static int baudrate;
|
||||
|
||||
/**
|
||||
* Mainloop of attitude_estimator_so3_comp.
|
||||
*/
|
||||
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
|
||||
"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
|
||||
"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The attitude_estimator_so3_comp app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int attitude_estimator_so3_comp_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("attitude_estimator_so3_comp already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
attitude_estimator_so3_comp_task = task_spawn("attitude_estimator_so3_comp",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
12400,
|
||||
attitude_estimator_so3_comp_thread_main,
|
||||
(const char **)argv);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
|
||||
while(thread_running){
|
||||
usleep(200000);
|
||||
printf(".");
|
||||
}
|
||||
printf("terminated.");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tattitude_estimator_so3_comp app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tattitude_estimator_so3_comp app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// Fast inverse square-root
|
||||
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
float invSqrt(float number) {
|
||||
volatile long i;
|
||||
volatile float x, y;
|
||||
volatile const float f = 1.5F;
|
||||
|
||||
x = number * 0.5F;
|
||||
y = number;
|
||||
i = * (( long * ) &y);
|
||||
i = 0x5f375a86 - ( i >> 1 );
|
||||
y = * (( float * ) &i);
|
||||
y = y * ( f - ( x * y * y ) );
|
||||
return y;
|
||||
}
|
||||
|
||||
//! Using accelerometer, sense the gravity vector.
|
||||
//! Using magnetometer, sense yaw.
|
||||
void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz)
|
||||
{
|
||||
float initialRoll, initialPitch;
|
||||
float cosRoll, sinRoll, cosPitch, sinPitch;
|
||||
float magX, magY;
|
||||
float initialHdg, cosHeading, sinHeading;
|
||||
|
||||
initialRoll = atan2(-ay, -az);
|
||||
initialPitch = atan2(ax, -az);
|
||||
|
||||
cosRoll = cosf(initialRoll);
|
||||
sinRoll = sinf(initialRoll);
|
||||
cosPitch = cosf(initialPitch);
|
||||
sinPitch = sinf(initialPitch);
|
||||
|
||||
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
|
||||
|
||||
magY = my * cosRoll - mz * sinRoll;
|
||||
|
||||
initialHdg = atan2f(-magY, magX);
|
||||
|
||||
cosRoll = cosf(initialRoll * 0.5f);
|
||||
sinRoll = sinf(initialRoll * 0.5f);
|
||||
|
||||
cosPitch = cosf(initialPitch * 0.5f);
|
||||
sinPitch = sinf(initialPitch * 0.5f);
|
||||
|
||||
cosHeading = cosf(initialHdg * 0.5f);
|
||||
sinHeading = sinf(initialHdg * 0.5f);
|
||||
|
||||
q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
|
||||
q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
|
||||
q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
|
||||
q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
|
||||
|
||||
// auxillary variables to reduce number of repeated operations, for 1st pass
|
||||
q0q0 = q0 * q0;
|
||||
q0q1 = q0 * q1;
|
||||
q0q2 = q0 * q2;
|
||||
q0q3 = q0 * q3;
|
||||
q1q1 = q1 * q1;
|
||||
q1q2 = q1 * q2;
|
||||
q1q3 = q1 * q3;
|
||||
q2q2 = q2 * q2;
|
||||
q2q3 = q2 * q3;
|
||||
q3q3 = q3 * q3;
|
||||
}
|
||||
|
||||
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
|
||||
float recipNorm;
|
||||
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
|
||||
|
||||
//! Make filter converge to initial solution faster
|
||||
//! This function assumes you are in static position.
|
||||
//! WARNING : in case air reboot, this can cause problem. But this is very
|
||||
//! unlikely happen.
|
||||
if(bFilterInit == false)
|
||||
{
|
||||
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
|
||||
bFilterInit = true;
|
||||
}
|
||||
|
||||
//! If magnetometer measurement is available, use it.
|
||||
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
|
||||
float hx, hy, hz, bx, bz;
|
||||
float halfwx, halfwy, halfwz;
|
||||
|
||||
// Normalise magnetometer measurement
|
||||
// Will sqrt work better? PX4 system is powerful enough?
|
||||
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
|
||||
mx *= recipNorm;
|
||||
my *= recipNorm;
|
||||
mz *= recipNorm;
|
||||
|
||||
// Reference direction of Earth's magnetic field
|
||||
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
|
||||
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
|
||||
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
|
||||
bx = sqrt(hx * hx + hy * hy);
|
||||
bz = hz;
|
||||
|
||||
// Estimated direction of magnetic field
|
||||
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
|
||||
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
|
||||
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
|
||||
|
||||
// Error is sum of cross product between estimated direction and measured direction of field vectors
|
||||
halfex += (my * halfwz - mz * halfwy);
|
||||
halfey += (mz * halfwx - mx * halfwz);
|
||||
halfez += (mx * halfwy - my * halfwx);
|
||||
}
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
float halfvx, halfvy, halfvz;
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Estimated direction of gravity and magnetic field
|
||||
halfvx = q1q3 - q0q2;
|
||||
halfvy = q0q1 + q2q3;
|
||||
halfvz = q0q0 - 0.5f + q3q3;
|
||||
|
||||
// Error is sum of cross product between estimated direction and measured direction of field vectors
|
||||
halfex += ay * halfvz - az * halfvy;
|
||||
halfey += az * halfvx - ax * halfvz;
|
||||
halfez += ax * halfvy - ay * halfvx;
|
||||
}
|
||||
|
||||
// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
|
||||
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
|
||||
// Compute and apply integral feedback if enabled
|
||||
if(twoKi > 0.0f) {
|
||||
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
|
||||
gyro_bias[1] += twoKi * halfey * dt;
|
||||
gyro_bias[2] += twoKi * halfez * dt;
|
||||
gx += gyro_bias[0]; // apply integral feedback
|
||||
gy += gyro_bias[1];
|
||||
gz += gyro_bias[2];
|
||||
}
|
||||
else {
|
||||
gyro_bias[0] = 0.0f; // prevent integral windup
|
||||
gyro_bias[1] = 0.0f;
|
||||
gyro_bias[2] = 0.0f;
|
||||
}
|
||||
|
||||
// Apply proportional feedback
|
||||
gx += twoKp * halfex;
|
||||
gy += twoKp * halfey;
|
||||
gz += twoKp * halfez;
|
||||
}
|
||||
|
||||
//! Integrate rate of change of quaternion
|
||||
#if 0
|
||||
gx *= (0.5f * dt); // pre-multiply common factors
|
||||
gy *= (0.5f * dt);
|
||||
gz *= (0.5f * dt);
|
||||
#endif
|
||||
|
||||
// Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
|
||||
//! q_k = q_{k-1} + dt*\dot{q}
|
||||
//! \dot{q} = 0.5*q \otimes P(\omega)
|
||||
dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
|
||||
dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
|
||||
dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
|
||||
dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
q0 += dt*dq0;
|
||||
q1 += dt*dq1;
|
||||
q2 += dt*dq2;
|
||||
q3 += dt*dq3;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
q0q0 = q0 * q0;
|
||||
q0q1 = q0 * q1;
|
||||
q0q2 = q0 * q2;
|
||||
q0q3 = q0 * q3;
|
||||
q1q1 = q1 * q1;
|
||||
q1q2 = q1 * q2;
|
||||
q1q3 = q1 * q3;
|
||||
q2q2 = q2 * q2;
|
||||
q2q3 = q2 * q3;
|
||||
q3q3 = q3 * q3;
|
||||
}
|
||||
|
||||
void send_uart_byte(char c)
|
||||
{
|
||||
write(uart,&c,1);
|
||||
}
|
||||
|
||||
void send_uart_bytes(uint8_t *data, int length)
|
||||
{
|
||||
write(uart,data,(size_t)(sizeof(uint8_t)*length));
|
||||
}
|
||||
|
||||
void send_uart_float(float f) {
|
||||
uint8_t * b = (uint8_t *) &f;
|
||||
|
||||
//! Assume float is 4-bytes
|
||||
for(int i=0; i<4; i++) {
|
||||
|
||||
uint8_t b1 = (b[i] >> 4) & 0x0f;
|
||||
uint8_t b2 = (b[i] & 0x0f);
|
||||
|
||||
uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
|
||||
uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
|
||||
|
||||
send_uart_bytes(&c1,1);
|
||||
send_uart_bytes(&c2,1);
|
||||
}
|
||||
}
|
||||
|
||||
void send_uart_float_arr(float *arr, int length)
|
||||
{
|
||||
for(int i=0;i<length;++i)
|
||||
{
|
||||
send_uart_float(arr[i]);
|
||||
send_uart_byte(',');
|
||||
}
|
||||
}
|
||||
|
||||
int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
|
||||
{
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 0: speed = B0; break;
|
||||
case 50: speed = B50; break;
|
||||
case 75: speed = B75; break;
|
||||
case 110: speed = B110; break;
|
||||
case 134: speed = B134; break;
|
||||
case 150: speed = B150; break;
|
||||
case 200: speed = B200; break;
|
||||
case 300: speed = B300; break;
|
||||
case 600: speed = B600; break;
|
||||
case 1200: speed = B1200; break;
|
||||
case 1800: speed = B1800; break;
|
||||
case 2400: speed = B2400; break;
|
||||
case 4800: speed = B4800; break;
|
||||
case 9600: speed = B9600; break;
|
||||
case 19200: speed = B19200; break;
|
||||
case 38400: speed = B38400; break;
|
||||
case 57600: speed = B57600; break;
|
||||
case 115200: speed = B115200; break;
|
||||
case 230400: speed = B230400; break;
|
||||
case 460800: speed = B460800; break;
|
||||
case 921600: speed = B921600; break;
|
||||
default:
|
||||
printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
|
||||
uart = open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
*is_usb = false;
|
||||
|
||||
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
|
||||
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else {
|
||||
*is_usb = true;
|
||||
}
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
/*
|
||||
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
|
||||
*/
|
||||
|
||||
/*
|
||||
* EKF Attitude Estimator main function.
|
||||
*
|
||||
* Estimates the attitude recursively once started.
|
||||
*
|
||||
* @param argc number of commandline arguments (plus command name)
|
||||
* @param argv strings containing the arguments
|
||||
*/
|
||||
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
|
||||
//! Serial debug related
|
||||
int ch;
|
||||
struct termios uart_config_original;
|
||||
bool usb_uart;
|
||||
bool debug_mode = false;
|
||||
char *device_name = "/dev/ttyS2";
|
||||
baudrate = 115200;
|
||||
|
||||
//! Time constant
|
||||
float dt = 0.005f;
|
||||
|
||||
/* output euler angles */
|
||||
float euler[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
float Rot_matrix[9] = {1.f, 0, 0,
|
||||
0, 1.f, 0,
|
||||
0, 0, 1.f
|
||||
}; /**< init: identity matrix */
|
||||
|
||||
float acc[3] = {0.0f, 0.0f, 0.0f};
|
||||
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||
float mag[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* work around some stupidity in task_create's argv handling */
|
||||
argc -= 2;
|
||||
argv += 2;
|
||||
|
||||
//! -d <device_name>, default : /dev/ttyS2
|
||||
//! -b <baud_rate>, default : 115200
|
||||
while ((ch = getopt(argc,argv,"d:b:")) != EOF){
|
||||
switch(ch){
|
||||
case 'b':
|
||||
baudrate = strtoul(optarg, NULL, 10);
|
||||
if(baudrate == 0)
|
||||
printf("invalid baud rate '%s'",optarg);
|
||||
break;
|
||||
case 'd':
|
||||
device_name = optarg;
|
||||
debug_mode = true;
|
||||
break;
|
||||
default:
|
||||
usage("invalid argument");
|
||||
}
|
||||
}
|
||||
|
||||
if(debug_mode){
|
||||
printf("Opening debugging port for 3D visualization\n");
|
||||
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
|
||||
if (uart < 0)
|
||||
printf("could not open %s", device_name);
|
||||
else
|
||||
printf("Open port success\n");
|
||||
}
|
||||
|
||||
// print text
|
||||
printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
|
||||
fflush(stdout);
|
||||
|
||||
int overloadcounter = 19;
|
||||
|
||||
/* store start time to guard against too slow update rates */
|
||||
uint64_t last_run = hrt_absolute_time();
|
||||
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
|
||||
//! Initialize attitude vehicle uORB message.
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
|
||||
struct vehicle_status_s state;
|
||||
memset(&state, 0, sizeof(state));
|
||||
|
||||
uint64_t last_data = 0;
|
||||
uint64_t last_measurement = 0;
|
||||
|
||||
/* subscribe to raw data */
|
||||
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
|
||||
/* rate-limit raw data updates to 200Hz */
|
||||
orb_set_interval(sub_raw, 4);
|
||||
|
||||
/* subscribe to param changes */
|
||||
int sub_params = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
/* subscribe to system state*/
|
||||
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* advertise attitude */
|
||||
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
|
||||
int loopcounter = 0;
|
||||
int printcounter = 0;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
/* advertise debug value */
|
||||
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
||||
// orb_advert_t pub_dbg = -1;
|
||||
|
||||
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
|
||||
// XXX write this out to perf regs
|
||||
|
||||
/* keep track of sensor updates */
|
||||
uint32_t sensor_last_count[3] = {0, 0, 0};
|
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
|
||||
|
||||
struct attitude_estimator_so3_comp_params so3_comp_params;
|
||||
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
|
||||
|
||||
/* initialize parameter handles */
|
||||
parameters_init(&so3_comp_param_handles);
|
||||
|
||||
uint64_t start_time = hrt_absolute_time();
|
||||
bool initialized = false;
|
||||
|
||||
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
|
||||
unsigned offset_count = 0;
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
|
||||
|
||||
/* Main loop*/
|
||||
while (!thread_should_exit) {
|
||||
|
||||
struct pollfd fds[2];
|
||||
fds[0].fd = sub_raw;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = sub_params;
|
||||
fds[1].events = POLLIN;
|
||||
int ret = poll(fds, 2, 1000);
|
||||
|
||||
if (ret < 0) {
|
||||
/* XXX this is seriously bad - should be an emergency */
|
||||
} else if (ret == 0) {
|
||||
/* check if we're in HIL - not getting sensor data is fine then */
|
||||
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
|
||||
|
||||
if (!state.flag_hil_enabled) {
|
||||
fprintf(stderr,
|
||||
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), sub_params, &update);
|
||||
|
||||
/* update parameters */
|
||||
parameters_update(&so3_comp_param_handles, &so3_comp_params);
|
||||
}
|
||||
|
||||
/* only run filter if sensor values changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
/* get latest measurements */
|
||||
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
|
||||
|
||||
if (!initialized) {
|
||||
|
||||
gyro_offsets[0] += raw.gyro_rad_s[0];
|
||||
gyro_offsets[1] += raw.gyro_rad_s[1];
|
||||
gyro_offsets[2] += raw.gyro_rad_s[2];
|
||||
offset_count++;
|
||||
|
||||
if (hrt_absolute_time() - start_time > 3000000LL) {
|
||||
initialized = true;
|
||||
gyro_offsets[0] /= offset_count;
|
||||
gyro_offsets[1] /= offset_count;
|
||||
gyro_offsets[2] /= offset_count;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
perf_begin(so3_comp_loop_perf);
|
||||
|
||||
/* Calculate data time difference in seconds */
|
||||
dt = (raw.timestamp - last_measurement) / 1000000.0f;
|
||||
last_measurement = raw.timestamp;
|
||||
uint8_t update_vect[3] = {0, 0, 0};
|
||||
|
||||
/* Fill in gyro measurements */
|
||||
if (sensor_last_count[0] != raw.gyro_counter) {
|
||||
update_vect[0] = 1;
|
||||
sensor_last_count[0] = raw.gyro_counter;
|
||||
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
|
||||
sensor_last_timestamp[0] = raw.timestamp;
|
||||
}
|
||||
|
||||
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
|
||||
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
|
||||
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
|
||||
|
||||
/* update accelerometer measurements */
|
||||
if (sensor_last_count[1] != raw.accelerometer_counter) {
|
||||
update_vect[1] = 1;
|
||||
sensor_last_count[1] = raw.accelerometer_counter;
|
||||
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||
sensor_last_timestamp[1] = raw.timestamp;
|
||||
}
|
||||
|
||||
acc[0] = raw.accelerometer_m_s2[0];
|
||||
acc[1] = raw.accelerometer_m_s2[1];
|
||||
acc[2] = raw.accelerometer_m_s2[2];
|
||||
|
||||
/* update magnetometer measurements */
|
||||
if (sensor_last_count[2] != raw.magnetometer_counter) {
|
||||
update_vect[2] = 1;
|
||||
sensor_last_count[2] = raw.magnetometer_counter;
|
||||
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||
sensor_last_timestamp[2] = raw.timestamp;
|
||||
}
|
||||
|
||||
mag[0] = raw.magnetometer_ga[0];
|
||||
mag[1] = raw.magnetometer_ga[1];
|
||||
mag[2] = raw.magnetometer_ga[2];
|
||||
|
||||
uint64_t now = hrt_absolute_time();
|
||||
unsigned int time_elapsed = now - last_run;
|
||||
last_run = now;
|
||||
|
||||
if (time_elapsed > loop_interval_alarm) {
|
||||
//TODO: add warning, cpu overload here
|
||||
// if (overloadcounter == 20) {
|
||||
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
|
||||
// overloadcounter = 0;
|
||||
// }
|
||||
|
||||
overloadcounter++;
|
||||
}
|
||||
|
||||
static bool const_initialized = false;
|
||||
|
||||
/* initialize with good values once we have a reasonable dt estimate */
|
||||
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
|
||||
dt = 0.005f;
|
||||
parameters_update(&so3_comp_param_handles, &so3_comp_params);
|
||||
const_initialized = true;
|
||||
}
|
||||
|
||||
/* do not execute the filter if not initialized */
|
||||
if (!const_initialized) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint64_t timing_start = hrt_absolute_time();
|
||||
|
||||
// NOTE : Accelerometer is reversed.
|
||||
// Because proper mount of PX4 will give you a reversed accelerometer readings.
|
||||
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
|
||||
|
||||
// Convert q->R.
|
||||
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
|
||||
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12
|
||||
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13
|
||||
Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21
|
||||
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
|
||||
Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23
|
||||
Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31
|
||||
Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32
|
||||
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
|
||||
|
||||
//1-2-3 Representation.
|
||||
//Equation (290)
|
||||
//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
|
||||
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
|
||||
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
|
||||
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
|
||||
euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw
|
||||
|
||||
/* swap values for next iteration, check for fatal inputs */
|
||||
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
|
||||
/* Do something */
|
||||
} else {
|
||||
/* due to inputs or numerical failure the output is invalid, skip it */
|
||||
continue;
|
||||
}
|
||||
|
||||
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
|
||||
|
||||
last_data = raw.timestamp;
|
||||
|
||||
/* send out */
|
||||
att.timestamp = raw.timestamp;
|
||||
|
||||
// XXX Apply the same transformation to the rotation matrix
|
||||
att.roll = euler[0] - so3_comp_params.roll_off;
|
||||
att.pitch = euler[1] - so3_comp_params.pitch_off;
|
||||
att.yaw = euler[2] - so3_comp_params.yaw_off;
|
||||
|
||||
//! Euler angle rate. But it needs to be investigated again.
|
||||
/*
|
||||
att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
|
||||
att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
|
||||
att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3);
|
||||
*/
|
||||
att.rollspeed = gyro[0];
|
||||
att.pitchspeed = gyro[1];
|
||||
att.yawspeed = gyro[2];
|
||||
|
||||
att.rollacc = 0;
|
||||
att.pitchacc = 0;
|
||||
att.yawacc = 0;
|
||||
|
||||
//! Quaternion
|
||||
att.q[0] = q0;
|
||||
att.q[1] = q1;
|
||||
att.q[2] = q2;
|
||||
att.q[3] = q3;
|
||||
att.q_valid = true;
|
||||
|
||||
/* TODO: Bias estimation required */
|
||||
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
|
||||
|
||||
/* copy rotation matrix */
|
||||
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
|
||||
att.R_valid = true;
|
||||
|
||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
||||
} else {
|
||||
warnx("NaN in roll/pitch/yaw estimate!");
|
||||
}
|
||||
|
||||
perf_end(so3_comp_loop_perf);
|
||||
|
||||
//! This will print out debug packet to visualization software
|
||||
if(debug_mode)
|
||||
{
|
||||
float quat[4];
|
||||
quat[0] = q0;
|
||||
quat[1] = q1;
|
||||
quat[2] = q2;
|
||||
quat[3] = q3;
|
||||
send_uart_float_arr(quat,4);
|
||||
send_uart_byte('\n');
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
loopcounter++;
|
||||
}// while
|
||||
|
||||
thread_running = false;
|
||||
|
||||
/* Reset the UART flags to original state */
|
||||
if (!usb_uart)
|
||||
tcsetattr(uart, TCSANOW, &uart_config_original);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_params.c
|
||||
*
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include "attitude_estimator_so3_comp_params.h"
|
||||
|
||||
/* This is filter gain for nonlinear SO3 complementary filter */
|
||||
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
|
||||
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
|
||||
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
|
||||
will compensate gyro bias which depends on temperature and vibration of your vehicle */
|
||||
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
|
||||
//! You can set this gain higher if you want more fast response.
|
||||
//! But note that higher gain will give you also higher overshoot.
|
||||
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
|
||||
//! This gain is depend on your vehicle status.
|
||||
|
||||
/* offsets in roll, pitch and yaw of sensor plane and body */
|
||||
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
|
||||
|
||||
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
|
||||
{
|
||||
/* Filter gain parameters */
|
||||
h->Kp = param_find("SO3_COMP_KP");
|
||||
h->Ki = param_find("SO3_COMP_KI");
|
||||
|
||||
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
|
||||
h->roll_off = param_find("ATT_ROLL_OFFS");
|
||||
h->pitch_off = param_find("ATT_PITCH_OFFS");
|
||||
h->yaw_off = param_find("ATT_YAW_OFFS");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
|
||||
{
|
||||
/* Update filter gain */
|
||||
param_get(h->Kp, &(p->Kp));
|
||||
param_get(h->Ki, &(p->Ki));
|
||||
|
||||
/* Update attitude offset */
|
||||
param_get(h->roll_off, &(p->roll_off));
|
||||
param_get(h->pitch_off, &(p->pitch_off));
|
||||
param_get(h->yaw_off, &(p->yaw_off));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
|
||||
*
|
||||
* @file attitude_estimator_so3_comp_params.h
|
||||
*
|
||||
* Implementation of nonlinear complementary filters on the SO(3).
|
||||
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
|
||||
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
|
||||
*
|
||||
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
|
||||
* Quaternion realization of [1] is based on [2].
|
||||
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
|
||||
*
|
||||
* References
|
||||
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
|
||||
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct attitude_estimator_so3_comp_params {
|
||||
float Kp;
|
||||
float Ki;
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
};
|
||||
|
||||
struct attitude_estimator_so3_comp_param_handles {
|
||||
param_t Kp, Ki;
|
||||
param_t roll_off, pitch_off, yaw_off;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#
|
||||
# Attitude estimator (Nonlinear SO3 complementary Filter)
|
||||
#
|
||||
|
||||
MODULE_COMMAND = attitude_estimator_so3_comp
|
||||
|
||||
SRCS = attitude_estimator_so3_comp_main.cpp \
|
||||
attitude_estimator_so3_comp_params.c
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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_ */
|
||||
|
|
@ -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_ */
|
||||
Loading…
Reference in New Issue