mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 08:36:08 +08:00
Merged
This commit is contained in:
Lorenz Meier
@@ -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,32 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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/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 +68,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 +88,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_cmd("deamon",
|
||||
daemon_task = task_spawn_cmd("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 +110,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 +121,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.
|
||||
+833
File diff suppressed because it is too large
Load Diff
+63
@@ -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;
|
||||
}
|
||||
+44
@@ -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_ */
|
||||
Reference in New Issue
Block a user