forked from Archive/PX4-Autopilot
Added logging with worker thread for microSD writes, untested, but feature-complete
This commit is contained in:
Lorenz Meier
parent
e24dd0f684
commit
435bae6542
|
|
@ -36,7 +36,8 @@
|
|||
#
|
||||
|
||||
APPNAME = sdlog
|
||||
PRIORITY = SCHED_PRIORITY_DEFAULT - 30
|
||||
# The main thread only buffers to RAM, needs a high priority
|
||||
PRIORITY = SCHED_PRIORITY_MAX - 30
|
||||
STACKSIZE = 2048
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
|
|
|
|||
|
|
@ -69,6 +69,8 @@
|
|||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "sdlog_ringbuffer.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 */
|
||||
|
|
@ -76,6 +78,18 @@ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
|
|||
|
||||
static const char *mountpoint = "/fs/microsd";
|
||||
static const char *mfile_in = "/etc/logging/logconv.m";
|
||||
int sysvector_file = -1;
|
||||
struct sdlog_logbuffer lb;
|
||||
|
||||
/**
|
||||
* System state vector log buffer writing
|
||||
*/
|
||||
static void *sdlog_sysvector_write_thread(void *arg);
|
||||
|
||||
/**
|
||||
* Create the thread to write the system vector
|
||||
*/
|
||||
pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf);
|
||||
|
||||
/**
|
||||
* SD log management function.
|
||||
|
|
@ -94,7 +108,7 @@ static void usage(const char *reason);
|
|||
|
||||
static int file_exist(const char *filename);
|
||||
|
||||
static int file_copy(const char* file_old, const char* file_new);
|
||||
static int file_copy(const char *file_old, const char *file_new);
|
||||
|
||||
/**
|
||||
* Print the current status.
|
||||
|
|
@ -104,13 +118,14 @@ static void print_sdlog_status(void);
|
|||
/**
|
||||
* Create folder for current logging session.
|
||||
*/
|
||||
static int create_logfolder(char* folder_path);
|
||||
static int create_logfolder(char *folder_path);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
|
||||
}
|
||||
|
||||
|
|
@ -126,7 +141,7 @@ uint64_t starttime = 0;
|
|||
* The sd log deamon 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_spawn().
|
||||
*/
|
||||
|
|
@ -157,6 +172,7 @@ int sdlog_main(int argc, char *argv[])
|
|||
if (!thread_running) {
|
||||
printf("\tsdlog is not started\n");
|
||||
}
|
||||
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -164,9 +180,11 @@ int sdlog_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
print_sdlog_status();
|
||||
|
||||
} else {
|
||||
printf("\tsdlog not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
@ -174,7 +192,8 @@ int sdlog_main(int argc, char *argv[])
|
|||
exit(1);
|
||||
}
|
||||
|
||||
int create_logfolder(char* folder_path) {
|
||||
int create_logfolder(char *folder_path)
|
||||
{
|
||||
/* make folder on sdcard */
|
||||
uint16_t foldernumber = 1; // start with folder 0001
|
||||
int mkdir_ret;
|
||||
|
|
@ -193,11 +212,14 @@ int create_logfolder(char* folder_path) {
|
|||
char mfile_out[100];
|
||||
sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber);
|
||||
int ret = file_copy(mfile_in, mfile_out);
|
||||
|
||||
if (!ret) {
|
||||
warnx("copied m file to %s", mfile_out);
|
||||
|
||||
} else {
|
||||
warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
} else if (mkdir_ret == -1) {
|
||||
|
|
@ -216,11 +238,60 @@ int create_logfolder(char* folder_path) {
|
|||
warn("all %d possible folders exist already", MAX_NO_LOGFOLDER);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int sdlog_thread_main(int argc, char *argv[]) {
|
||||
static void *
|
||||
sdlog_sysvector_write_thread(void *arg)
|
||||
{
|
||||
struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg;
|
||||
|
||||
int poll_count = 0;
|
||||
struct sdlog_sysvector sysvect;
|
||||
memset(&sysvect, 0, sizeof(sysvect));
|
||||
|
||||
while (!thread_should_exit) {
|
||||
//pthread_mutex..
|
||||
if (sdlog_logbuffer_read(logbuf, &sysvect) == OK) {
|
||||
sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect));
|
||||
}
|
||||
|
||||
if (poll_count % 100 == 0) {
|
||||
fsync(sysvector_file);
|
||||
}
|
||||
|
||||
poll_count++;
|
||||
usleep(3000);
|
||||
}
|
||||
|
||||
fsync(sysvector_file);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
pthread_t
|
||||
sysvector_write_start(struct sdlog_logbuffer *logbuf)
|
||||
{
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
struct sched_param param;
|
||||
/* low priority, as this is expensive disk I/O */
|
||||
param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
|
||||
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf);
|
||||
return thread;
|
||||
}
|
||||
|
||||
|
||||
int sdlog_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
warnx("starting\n");
|
||||
|
||||
|
|
@ -229,6 +300,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
}
|
||||
|
||||
char folder_path[64];
|
||||
|
||||
if (create_logfolder(folder_path))
|
||||
errx(1, "unable to create logging folder, exiting.");
|
||||
|
||||
|
|
@ -236,7 +308,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
int sensorfile = -1;
|
||||
int actuator_outputs_file = -1;
|
||||
int actuator_controls_file = -1;
|
||||
int sysvector_file = -1;
|
||||
FILE *gpsfile;
|
||||
FILE *blackbox_file;
|
||||
// FILE *vehiclefile;
|
||||
|
|
@ -247,6 +318,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined");
|
||||
|
||||
if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
|
@ -259,28 +331,34 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
|
||||
|
||||
if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0");
|
||||
|
||||
if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
|
||||
sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
|
||||
|
||||
if (NULL == (gpsfile = fopen(path_buf, "w"))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
int gpsfile_no = fileno(gpsfile);
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
|
||||
sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
|
||||
|
||||
if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
int blackbox_file_no = fileno(blackbox_file);
|
||||
|
||||
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
|
||||
|
|
@ -426,7 +504,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
|
||||
thread_running = true;
|
||||
|
||||
int poll_count = 0;
|
||||
/* Initialize log buffer with a size of 5 */
|
||||
sdlog_logbuffer_init(&lb, 5);
|
||||
/* start logbuffer emptying thread */
|
||||
pthread_t sysvector_pthread = sysvector_write_start(&lb);
|
||||
|
||||
starttime = hrt_absolute_time();
|
||||
|
||||
|
|
@ -450,7 +531,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
// fsync(blackbox_file_no);
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
// /* --- VEHICLE COMMAND VALUE --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
|
|
@ -476,14 +557,14 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
// /* copy attitude data into local buffer */
|
||||
// orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
|
||||
|
||||
|
||||
|
||||
// }
|
||||
|
||||
// /* --- VEHICLE ATTITUDE SETPOINT --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
// /* copy local position data into local buffer */
|
||||
// orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
|
||||
|
||||
|
||||
// }
|
||||
|
||||
// /* --- ACTUATOR OUTPUTS 0 --- */
|
||||
|
|
@ -502,12 +583,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
// }
|
||||
// }
|
||||
|
||||
if (poll_count % 100 == 0) {
|
||||
fsync(sysvector_file);
|
||||
}
|
||||
|
||||
poll_count++;
|
||||
|
||||
|
||||
/* copy sensors raw data into local buffer */
|
||||
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
|
|
@ -523,27 +598,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
|
||||
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct {
|
||||
uint64_t timestamp; //[us]
|
||||
float gyro[3]; //[rad/s]
|
||||
float accel[3]; //[m/s^2]
|
||||
float mag[3]; //[gauss]
|
||||
float baro; //pressure [millibar]
|
||||
float baro_alt; //altitude above MSL [meter]
|
||||
float baro_temp; //[degree celcius]
|
||||
float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
|
||||
float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
|
||||
float vbat; //battery voltage in [volt]
|
||||
float adc[3]; //remaining auxiliary ADC ports [volt]
|
||||
float local_position[3]; //tangent plane mapping into x,y,z [m]
|
||||
int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
|
||||
float attitude[3]; //pitch, roll, yaw [rad]
|
||||
float rotMatrix[9]; //unitvectors
|
||||
float vicon[6];
|
||||
float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
|
||||
float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality
|
||||
} sysvector = {
|
||||
struct sdlog_sysvector sysvect = {
|
||||
.timestamp = buf.raw.timestamp,
|
||||
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
|
||||
.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
|
||||
|
|
@ -552,8 +607,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
.baro_alt = buf.raw.baro_alt_meter,
|
||||
.baro_temp = buf.raw.baro_temp_celcius,
|
||||
.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
|
||||
.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
|
||||
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
|
||||
.actuators = {
|
||||
buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
|
||||
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
|
||||
},
|
||||
.vbat = 0.0f, /* XXX use battery_status uORB topic */
|
||||
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
|
||||
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
|
||||
|
|
@ -563,18 +620,21 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
|
||||
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
|
||||
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}
|
||||
/* XXX add calculated airspeed */
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
|
||||
/* put into buffer for later IO */
|
||||
sdlog_logbuffer_write(&lb, &sysvect);
|
||||
// XXX notify writing thread through pthread mutex
|
||||
|
||||
usleep(3500); // roughly 150 Hz
|
||||
}
|
||||
|
||||
fsync(sysvector_file);
|
||||
|
||||
print_sdlog_status();
|
||||
|
||||
/* wait for write thread to return */
|
||||
(void)pthread_join(sysvector_pthread, NULL);
|
||||
|
||||
warnx("exiting.\n");
|
||||
|
||||
close(sensorfile);
|
||||
|
|
@ -606,43 +666,44 @@ int file_exist(const char *filename)
|
|||
return stat(filename, &buffer);
|
||||
}
|
||||
|
||||
int file_copy(const char* file_old, const char* file_new)
|
||||
int file_copy(const char *file_old, const char *file_new)
|
||||
{
|
||||
FILE *source, *target;
|
||||
source = fopen(file_old, "r");
|
||||
int ret = 0;
|
||||
|
||||
if( source == NULL )
|
||||
{
|
||||
|
||||
if (source == NULL) {
|
||||
warnx("failed opening input file to copy");
|
||||
return 1;
|
||||
}
|
||||
|
||||
target = fopen(file_new, "w");
|
||||
|
||||
if( target == NULL )
|
||||
{
|
||||
|
||||
if (target == NULL) {
|
||||
fclose(source);
|
||||
warnx("failed to open output file to copy");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
char buf[128];
|
||||
int nread;
|
||||
|
||||
while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) {
|
||||
int ret = fwrite(buf, 1, nread, target);
|
||||
|
||||
if (ret <= 0) {
|
||||
warnx("error writing file");
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
fsync(fileno(target));
|
||||
|
||||
fclose(source);
|
||||
fclose(target);
|
||||
|
||||
return ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,91 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* 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 sdlog_log.c
|
||||
* MAVLink text logging.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "sdlog_ringbuffer.h"
|
||||
|
||||
void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size)
|
||||
{
|
||||
lb->size = size;
|
||||
lb->start = 0;
|
||||
lb->count = 0;
|
||||
lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector));
|
||||
}
|
||||
|
||||
int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb)
|
||||
{
|
||||
return lb->count == (int)lb->size;
|
||||
}
|
||||
|
||||
int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb)
|
||||
{
|
||||
return lb->count == 0;
|
||||
}
|
||||
|
||||
|
||||
// XXX make these functions thread-safe
|
||||
void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem)
|
||||
{
|
||||
int end = (lb->start + lb->count) % lb->size;
|
||||
memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector));
|
||||
|
||||
if (sdlog_logbuffer_is_full(lb)) {
|
||||
lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
|
||||
|
||||
} else {
|
||||
++lb->count;
|
||||
}
|
||||
}
|
||||
|
||||
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem)
|
||||
{
|
||||
if (!sdlog_logbuffer_is_empty(lb)) {
|
||||
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector));
|
||||
lb->start = (lb->start + 1) % lb->size;
|
||||
--lb->count;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* 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 sdlog_ringbuffer.h
|
||||
* microSD logging
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef SDLOG_RINGBUFFER_H_
|
||||
#define SDLOG_RINGBUFFER_H_
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct sdlog_sysvector {
|
||||
uint64_t timestamp; /**< time [us] */
|
||||
float gyro[3]; /**< [rad/s] */
|
||||
float accel[3]; /**< [m/s^2] */
|
||||
float mag[3]; /**< [gauss] */
|
||||
float baro; /**< pressure [millibar] */
|
||||
float baro_alt; /**< altitude above MSL [meter] */
|
||||
float baro_temp; /**< [degree celcius] */
|
||||
float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
|
||||
float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */
|
||||
float vbat; /**< battery voltage in [volt] */
|
||||
float adc[3]; /**< remaining auxiliary ADC ports [volt] */
|
||||
float local_position[3]; /**< tangent plane mapping into x,y,z [m] */
|
||||
int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
|
||||
float attitude[3]; /**< roll, pitch, yaw [rad] */
|
||||
float rotMatrix[9]; /**< unitvectors */
|
||||
float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */
|
||||
float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
|
||||
float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
struct sdlog_logbuffer {
|
||||
unsigned int start;
|
||||
// unsigned int end;
|
||||
unsigned int size;
|
||||
int count;
|
||||
struct sdlog_sysvector *elems;
|
||||
};
|
||||
|
||||
void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size);
|
||||
|
||||
int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb);
|
||||
|
||||
int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb);
|
||||
|
||||
void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem);
|
||||
|
||||
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue