diff options
-rw-r--r-- | apps/sdlog/Makefile | 3 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 165 | ||||
-rw-r--r-- | apps/sdlog/sdlog_ringbuffer.c | 91 | ||||
-rw-r--r-- | apps/sdlog/sdlog_ringbuffer.h | 86 |
4 files changed, 292 insertions, 53 deletions
diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile index 61f224e99..225b14a32 100644 --- a/apps/sdlog/Makefile +++ b/apps/sdlog/Makefile @@ -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 diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 9b4cd1622..407c47811 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -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; } diff --git a/apps/sdlog/sdlog_ringbuffer.c b/apps/sdlog/sdlog_ringbuffer.c new file mode 100644 index 000000000..d7c8a4759 --- /dev/null +++ b/apps/sdlog/sdlog_ringbuffer.c @@ -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; + } +} diff --git a/apps/sdlog/sdlog_ringbuffer.h b/apps/sdlog/sdlog_ringbuffer.h new file mode 100644 index 000000000..68fff3a91 --- /dev/null +++ b/apps/sdlog/sdlog_ringbuffer.h @@ -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
\ No newline at end of file |