From e2113526043f82700c2cff5d16d9e3a4dee089c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 May 2013 22:15:56 +0400 Subject: [PATCH 01/17] sdlog2 logger app added. New flexible log format, compatible with APM. --- makefiles/config_px4fmu_default.mk | 1 + src/modules/sdlog2/module.mk | 43 ++ src/modules/sdlog2/sdlog2.c | 890 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_format.h | 98 +++ src/modules/sdlog2/sdlog2_messages.h | 82 +++ src/modules/sdlog2/sdlog2_ringbuffer.c | 141 ++++ src/modules/sdlog2/sdlog2_ringbuffer.h | 68 ++ 7 files changed, 1323 insertions(+) create mode 100644 src/modules/sdlog2/module.mk create mode 100644 src/modules/sdlog2/sdlog2.c create mode 100644 src/modules/sdlog2/sdlog2_format.h create mode 100644 src/modules/sdlog2/sdlog2_messages.h create mode 100644 src/modules/sdlog2/sdlog2_ringbuffer.c create mode 100644 src/modules/sdlog2/sdlog2_ringbuffer.h diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1e4d592665..b6fa4014a2 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -78,6 +78,7 @@ MODULES += modules/multirotor_pos_control # Logging # MODULES += modules/sdlog +MODULES += modules/sdlog2 # # Library modules diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk new file mode 100644 index 0000000000..5a9936635a --- /dev/null +++ b/src/modules/sdlog2/module.mk @@ -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 \ + sdlog2_ringbuffer.c diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c new file mode 100644 index 0000000000..38b57082f4 --- /dev/null +++ b/src/modules/sdlog2/sdlog2.c @@ -0,0 +1,890 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * Anton Babushkin + * + * 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.c + * + * Simple SD logger for flight data. Buffers new sensor values and + * does the heavy SD I/O in a low-priority worker thread. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "sdlog2_ringbuffer.h" +#include "sdlog2_messages.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 */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int LOG_BUFFER_SIZE = 2048; +static const int MAX_WRITE_CHUNK = 1024; + +static const char *mountpoint = "/fs/microsd"; +int log_file = -1; +int mavlink_fd = -1; +struct sdlog2_logbuffer lb; + +/* mutex / condition to synchronize threads */ +pthread_mutex_t logbuffer_mutex; +pthread_cond_t logbuffer_cond; + +/** + * System state vector log buffer writing + */ +static void *sdlog2_logbuffer_write_thread(void *arg); + +/** + * Create the thread to write the system vector + */ +pthread_t sdlog2_logwriter_start(struct sdlog2_logbuffer *logbuf); + +/** + * Write a header to log file: list of message formats + */ +void sdlog2_write_formats(int fd); + +/** + * SD log management function. + */ +__EXPORT int sdlog2_main(int argc, char *argv[]); + +/** + * Mainloop of sd log deamon. + */ +int sdlog2_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +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 void handle_command(struct vehicle_command_s *cmd); + +/** + * Print the current status. + */ +static void print_sdlog2_status(void); + +/** + * Create folder for current logging session. + */ +static int create_logfolder(char *folder_path); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + errx(1, "usage: sdlog2 {start|stop|status} [-s ]\n\n"); +} + +unsigned long log_bytes_written = 0; +uint64_t starttime = 0; + +/* logging on or off, default to true */ +bool logging_enabled = true; + +/** + * 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(). + */ +int sdlog2_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("sdlog2 already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("sdlog2", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 4096, + sdlog2_thread_main, + (const char **)argv); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (!thread_running) { + printf("\tsdlog2 is not started\n"); + } + + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + print_sdlog2_status(); + + } else { + printf("\tsdlog2 not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int create_logfolder(char *folder_path) +{ + /* make folder on sdcard */ + uint16_t foldernumber = 1; // start with folder 0001 + int mkdir_ret; + + /* look for the next folder that does not exist */ + while (foldernumber < MAX_NO_LOGFOLDER) { + /* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */ + sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber); + mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); + /* the result is -1 if the folder exists */ + + if (mkdir_ret == 0) { + /* folder does not exist, success */ + + /* copy parser script file */ + // TODO + /* + 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) { + /* folder exists already */ + foldernumber++; + continue; + + } else { + warn("failed creating new folder"); + return -1; + } + } + + if (foldernumber >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); + return -1; + } + + return 0; +} + +static void * +sdlog2_logbuffer_write_thread(void *arg) +{ + /* set name */ + prctl(PR_SET_NAME, "sdlog2 microSD I/O", 0); + + struct sdlog2_logbuffer *logbuf = (struct sdlog2_logbuffer *)arg; + + int poll_count = 0; + + void *read_ptr; + int n = 0; + + while (!thread_should_exit) { + + /* make sure threads are synchronized */ + pthread_mutex_lock(&logbuffer_mutex); + + /* update read pointer if needed */ + if (n > 0) { + sdlog2_logbuffer_mark_read(&lb, n); + } + + /* only wait if no data is available to process */ + if (sdlog2_logbuffer_is_empty(logbuf)) { + /* blocking wait for new data at this line */ + pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex); + } + + /* only get pointer to thread-safe data, do heavy I/O a few lines down */ + n = sdlog2_logbuffer_get_ptr(logbuf, &read_ptr); + + /* continue */ + pthread_mutex_unlock(&logbuffer_mutex); + + if (n > 0) { + /* do heavy IO here */ + if (n > MAX_WRITE_CHUNK) + n = MAX_WRITE_CHUNK; + + n = write(log_file, read_ptr, n); + + if (n > 0) { + log_bytes_written += n; + } + } + + if (poll_count % 100 == 0) { + fsync(log_file); + } + + poll_count++; + } + + fsync(log_file); + + return OK; +} + +pthread_t sdlog2_logwriter_start(struct sdlog2_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, sdlog2_logbuffer_write_thread, logbuf); + return thread; + + // XXX we have to destroy the attr at some point +} + +void sdlog2_write_formats(int fd) +{ + /* construct message format packet */ + struct { + LOG_PACKET_HEADER; + struct log_format_s body; + } log_format_packet = { + LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), + }; + + /* fill message format packet for each format and write to log */ + int i; + + for (i = 0; i < log_formats_num; i++) { + log_format_packet.body = log_formats[i]; + write(fd, &log_format_packet, sizeof(log_format_packet)); + } + + fsync(fd); +} + +int sdlog2_thread_main(int argc, char *argv[]) +{ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + } + + /* log every n'th value (skip three per default) */ + int skip_value = 3; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + int ch; + + while ((ch = getopt(argc, argv, "s:r")) != EOF) { + switch (ch) { + case 's': { + /* log only every n'th (gyro clocked) value */ + unsigned s = strtoul(optarg, NULL, 10); + + if (s < 1 || s > 250) { + errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); + + } else { + skip_value = s; + } + } + break; + + case 'r': + /* log only on request, disable logging per default */ + logging_enabled = false; + break; + + case '?': + if (optopt == 'c') { + warnx("Option -%c requires an argument.\n", optopt); + + } else if (isprint(optopt)) { + warnx("Unknown option `-%c'.\n", optopt); + + } else { + warnx("Unknown option character `\\x%x'.\n", optopt); + } + + default: + usage("unrecognized flag"); + errx(1, "exiting."); + } + } + + if (file_exist(mountpoint) != OK) { + errx(1, "logging mount point %s not present, exiting.", mountpoint); + } + + char folder_path[64]; + + if (create_logfolder(folder_path)) + errx(1, "unable to create logging folder, exiting."); + + /* string to hold the path to the sensorfile */ + char path_buf[64] = ""; + + /* only print logging path, important to find log file later */ + warnx("logging to directory %s\n", folder_path); + + /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ + sprintf(path_buf, "%s/%s.bin", folder_path, "log"); + + if (0 == (log_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { + errx(1, "opening %s failed.\n", path_buf); + } + + /* write log messages formats */ + sdlog2_write_formats(log_file); + + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 25; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + + /* warning! using union here to save memory, elements should be used separately! */ + union { + struct sensor_combined_s raw; + struct vehicle_attitude_s att; + struct vehicle_attitude_setpoint_s att_sp; + struct actuator_outputs_s act_outputs; + struct actuator_controls_s act_controls; + struct actuator_controls_effective_s act_controls_effective; + struct vehicle_command_s cmd; + struct vehicle_local_position_s local_pos; + struct vehicle_global_position_s global_pos; + struct vehicle_gps_position_s gps_pos; + struct vehicle_vicon_position_s vicon_pos; + struct optical_flow_s flow; + struct battery_status_s batt; + struct differential_pressure_s diff_pres; + struct airspeed_s airspeed; + } buf; + memset(&buf, 0, sizeof(buf)); + + struct { + int cmd_sub; + int sensor_sub; + int att_sub; + int att_sp_sub; + int act_0_sub; + int controls_0_sub; + int controls_effective_0_sub; + int local_pos_sub; + int global_pos_sub; + int gps_pos_sub; + int vicon_pos_sub; + int flow_sub; + int batt_sub; + int diff_pres_sub; + int airspeed_sub; + } subs; + + /* log message buffer: header + body */ + struct { + LOG_PACKET_HEADER; + union { + struct log_TS_s log_TS; + struct log_ATT_s log_ATT; + struct log_RAW_s log_RAW; + } body; + } log_msg = { + LOG_PACKET_HEADER_INIT(0) + }; + memset(&log_msg.body, 0, sizeof(log_msg.body)); + + /* --- MANAGEMENT - LOGGING COMMAND --- */ + /* subscribe to ORB for vehicle command */ + subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + fds[fdsc_count].fd = subs.cmd_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GPS POSITION --- */ + /* subscribe to ORB for global position */ + subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + fds[fdsc_count].fd = subs.gps_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- SENSORS RAW VALUE --- */ + /* subscribe to ORB for sensors raw */ + subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + fds[fdsc_count].fd = subs.sensor_sub; + /* do not rate limit, instead use skip counter (aliasing on rate limit) */ + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ATTITUDE VALUE --- */ + /* subscribe to ORB for attitude */ + subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + fds[fdsc_count].fd = subs.att_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ATTITUDE SETPOINT VALUE --- */ + /* subscribe to ORB for attitude setpoint */ + /* struct already allocated */ + subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + fds[fdsc_count].fd = subs.att_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /** --- ACTUATOR OUTPUTS --- */ + subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + fds[fdsc_count].fd = subs.act_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR CONTROL VALUE --- */ + /* subscribe to ORB for actuator control */ + subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.controls_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + /* subscribe to ORB for actuator control */ + subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + fds[fdsc_count].fd = subs.controls_effective_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- LOCAL POSITION --- */ + /* subscribe to ORB for local position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + fds[fdsc_count].fd = subs.local_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GLOBAL POSITION --- */ + /* subscribe to ORB for global position */ + subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + fds[fdsc_count].fd = subs.global_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- VICON POSITION --- */ + /* subscribe to ORB for vicon position */ + subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + fds[fdsc_count].fd = subs.vicon_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- FLOW measurements --- */ + /* subscribe to ORB for flow measurements */ + subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); + fds[fdsc_count].fd = subs.flow_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- BATTERY STATUS --- */ + /* subscribe to ORB for flow measurements */ + subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); + fds[fdsc_count].fd = subs.batt_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- DIFFERENTIAL PRESSURE --- */ + /* subscribe to ORB for flow measurements */ + subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + fds[fdsc_count].fd = subs.diff_pres_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- AIRSPEED --- */ + /* subscribe to ORB for airspeed */ + subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + fds[fdsc_count].fd = subs.airspeed_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* WARNING: If you get the error message below, + * then the number of registered messages (fdsc) + * differs from the number of messages in the above list. + */ + if (fdsc_count > fdsc) { + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); + fdsc_count = fdsc; + } + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + // const int timeout = 1000; + + thread_running = true; + + /* initialize log buffer with specified size */ + sdlog2_logbuffer_init(&lb, LOG_BUFFER_SIZE); + + /* initialize thread synchronization */ + pthread_mutex_init(&logbuffer_mutex, NULL); + pthread_cond_init(&logbuffer_cond, NULL); + + /* start logbuffer emptying thread */ + pthread_t logwriter_pthread = sdlog2_logwriter_start(&lb); + + starttime = hrt_absolute_time(); + + /* track skipping */ + int skip_count = 0; + + while (!thread_should_exit) { + + /* poll all topics */ + int poll_ret = poll(fds, 4, 1000); + + /* handle the poll result */ + if (poll_ret == 0) { + /* XXX this means none of our providers is giving us data - might be an error? */ + } else if (poll_ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else { + + int ifds = 0; + + if (!logging_enabled) { + usleep(100000); + continue; + } + + pthread_mutex_lock(&logbuffer_mutex); + + /* write time stamp message */ + log_msg.msg_type = LOG_TS_MSG; + log_msg.body.log_TS.t = hrt_absolute_time(); + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(TS)); + + /* --- VEHICLE COMMAND VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy command into local buffer */ + orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + handle_command(&buf.cmd); + } + + /* --- GPS POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + } + + /* --- SENSORS RAW VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + log_msg.msg_type = LOG_RAW_MSG; + log_msg.body.log_RAW.accX = buf.raw.accelerometer_m_s2[0]; + log_msg.body.log_RAW.accY = buf.raw.accelerometer_m_s2[1]; + log_msg.body.log_RAW.accZ = buf.raw.accelerometer_m_s2[2]; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(RAW)); + } + + /* --- ATTITUDE VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.roll = buf.att.roll; + log_msg.body.log_ATT.pitch = buf.att.pitch; + log_msg.body.log_ATT.yaw = buf.att.yaw; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATT)); + } + + /* --- ATTITUDE SETPOINT VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); + // TODO not implemented yet + } + + /* --- ACTUATOR OUTPUTS --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- ACTUATOR CONTROL VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- LOCAL POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- GLOBAL POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- VICON POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- FLOW measurements --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- BATTERY STATUS --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- DIFFERENTIAL PRESSURE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* --- AIRSPEED --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet + } + + /* signal the other thread new data, but not yet unlock */ + if (sdlog2_logbuffer_count(&lb) > (lb.size / 2)) { + /* only request write if several packets can be written at once */ + pthread_cond_signal(&logbuffer_cond); + } + + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&logbuffer_mutex); + } + } + + print_sdlog2_status(); + + /* wake up write thread one last time */ + pthread_mutex_lock(&logbuffer_mutex); + pthread_cond_signal(&logbuffer_cond); + /* unlock, now the writer thread may return */ + pthread_mutex_unlock(&logbuffer_mutex); + + /* wait for write thread to return */ + (void)pthread_join(logwriter_pthread, NULL); + + pthread_mutex_destroy(&logbuffer_mutex); + pthread_cond_destroy(&logbuffer_cond); + + warnx("exiting.\n\n"); + + thread_running = false; + + return 0; +} + +void print_sdlog2_status() +{ + float mebibytes = log_bytes_written / 1024.0f / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + + warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); +} + +/** + * @return 0 if file exists + */ +int file_exist(const char *filename) +{ + struct stat buffer; + return stat(filename, &buffer); +} + +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) { + warnx("failed opening input file to copy"); + return 1; + } + + target = fopen(file_new, "w"); + + 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) { + 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; +} + +void handle_command(struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* request to set different system mode */ + switch (cmd->command) { + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + + if (((int)(cmd->param3)) == 1) { + + /* enable logging */ + mavlink_log_info(mavlink_fd, "[log] file:"); + mavlink_log_info(mavlink_fd, "logdir"); + logging_enabled = true; + } + + if (((int)(cmd->param3)) == 0) { + + /* disable logging */ + mavlink_log_info(mavlink_fd, "[log] stopped."); + logging_enabled = false; + } + + break; + + default: + /* silently ignore */ + break; + } + +} diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h new file mode 100644 index 0000000000..f5347a7bdd --- /dev/null +++ b/src/modules/sdlog2/sdlog2_format.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 + */ + +/* +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; + 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) + 8, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + +#define LOG_FORMAT_MSG 128 + +#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(log_msg.body.log_##_name) + +#endif /* SDLOG2_FORMAT_H_ */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h new file mode 100644 index 0000000000..ae98ea0381 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 + */ + +#ifndef SDLOG2_MESSAGES_H_ +#define SDLOG2_MESSAGES_H_ + +#include "sdlog2_format.h" + +/* define message formats */ + +/* === 1: TS - TIME STAMP === */ +#define LOG_TS_MSG 1 +struct log_TS_s { + uint64_t t; +}; + +/* === 2: ATT - ATTITUDE === */ +#define LOG_ATT_MSG 2 +struct log_ATT_s { + float roll; + float pitch; + float yaw; +}; + +/* === 3: RAW - SENSORS === */ +#define LOG_RAW_MSG 3 +struct log_RAW_s { + float accX; + float accY; + float accZ; +}; + +/* construct list of all message formats */ + +static const struct log_format_s log_formats[] = { + LOG_FORMAT(TS, "Q", "t"), + LOG_FORMAT(ATT, "fff", "roll,pitch,yaw"), + LOG_FORMAT(RAW, "fff", "accX,accY,accZ"), +}; + +static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); + +#endif /* SDLOG2_MESSAGES_H_ */ diff --git a/src/modules/sdlog2/sdlog2_ringbuffer.c b/src/modules/sdlog2/sdlog2_ringbuffer.c new file mode 100644 index 0000000000..022a183ba3 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_ringbuffer.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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_ringbuffer.c + * + * Ring FIFO buffer for binary data. + * + * @author Anton Babushkin + */ + +#include +#include + +#include "sdlog2_ringbuffer.h" + +void sdlog2_logbuffer_init(struct sdlog2_logbuffer *lb, int size) +{ + lb->size = size; + lb->write_ptr = 0; + lb->read_ptr = 0; + lb->data = malloc(lb->size); +} + +int sdlog2_logbuffer_free(struct sdlog2_logbuffer *lb) +{ + int n = lb->read_ptr - lb->write_ptr - 1; + + if (n < 0) { + n += lb->size; + } + + return n; +} + +int sdlog2_logbuffer_count(struct sdlog2_logbuffer *lb) +{ + int n = lb->write_ptr - lb->read_ptr; + + if (n < 0) { + n += lb->size; + } + + return n; +} + +int sdlog2_logbuffer_is_empty(struct sdlog2_logbuffer *lb) +{ + return lb->read_ptr == lb->write_ptr; +} + +void sdlog2_logbuffer_write(struct sdlog2_logbuffer *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; + } + + 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; +} + +int sdlog2_logbuffer_get_ptr(struct sdlog2_logbuffer *lb, void **ptr) +{ + // 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, write all available bytes + n = available; + + } else { + // read pointer is after write pointer, write bytes from read_ptr to end + n = lb->size - lb->read_ptr; + } + + *ptr = &(lb->data[lb->read_ptr]); + return n; +} + +void sdlog2_logbuffer_mark_read(struct sdlog2_logbuffer *lb, int n) +{ + lb->read_ptr = (lb->read_ptr + n) % lb->size; +} diff --git a/src/modules/sdlog2/sdlog2_ringbuffer.h b/src/modules/sdlog2/sdlog2_ringbuffer.h new file mode 100644 index 0000000000..287f56c34f --- /dev/null +++ b/src/modules/sdlog2/sdlog2_ringbuffer.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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_ringbuffer.h + * + * Ring FIFO buffer for binary data. + * + * @author Anton Babushkin + */ + +#ifndef SDLOG2_RINGBUFFER_H_ +#define SDLOG2_RINGBUFFER_H_ + +struct sdlog2_logbuffer { + // all pointers are in bytes + int write_ptr; + int read_ptr; + int size; + char *data; +}; + +void sdlog2_logbuffer_init(struct sdlog2_logbuffer *lb, int size); + +int sdlog2_logbuffer_free(struct sdlog2_logbuffer *lb); + +int sdlog2_logbuffer_count(struct sdlog2_logbuffer *lb); + +int sdlog2_logbuffer_is_empty(struct sdlog2_logbuffer *lb); + +void sdlog2_logbuffer_write(struct sdlog2_logbuffer *lb, void *ptr, int size); + +int sdlog2_logbuffer_get_ptr(struct sdlog2_logbuffer *lb, void **ptr); + +void sdlog2_logbuffer_mark_read(struct sdlog2_logbuffer *lb, int n); + +#endif From 691dc8eefd835c437251e544f74f126bb883869c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 May 2013 00:14:10 +0400 Subject: [PATCH 02/17] sdlog2 strick packing fixed, length bug fixed, "sdlog2_dump.py" debug tool added --- Tools/sdlog2_dump.py | 90 ++++++++++++++++++++++++++++++ src/modules/sdlog2/sdlog2.c | 2 + src/modules/sdlog2/sdlog2_format.h | 12 ++-- 3 files changed, 98 insertions(+), 6 deletions(-) create mode 100644 Tools/sdlog2_dump.py diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py new file mode 100644 index 0000000000..bdcbfdda7f --- /dev/null +++ b/Tools/sdlog2_dump.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python + +"""Parse and dump binary log generated by sdlog2 + + Usage: python sdlog2_dump.py """ + +__author__ = "Anton Babushkin" +__version__ = "0.1" + +import struct, sys + +class BufferUnderflow(Exception): + pass + +class SDLog2Parser: + BLOCK_SIZE = 8192 + MSG_HEADER_LEN = 3 + MSG_HEAD1 = 0xA3 + MSG_HEAD2 = 0x95 + MSG_FORMAT_PACKET_LEN = 89 + MSG_TYPE_FORMAT = 0x80 + + def __init__(self): + return + + def reset(self): + self.msg_formats = {} + self.buffer = "" + self.ptr = 0 + + def process(self, fn): + self.reset() + f = open(fn, "r") + 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._bytes_left() >= 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): + raise Exception("Invalid header: %02X %02X, must be %02X %02X" % (head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) + msg_type = ord(self.buffer[self.ptr+2]) + if msg_type == self.MSG_TYPE_FORMAT: + self._parse_msg_format() + else: + msg_format = self.msg_formats[msg_type] + if msg_format == None: + raise Exception("Unknown msg type: %i" % msg_type) + msg_length = msg_format[0] + if self._bytes_left() < msg_length: + break + self._parse_msg(msg_format) + f.close() + + def _bytes_left(self): + return len(self.buffer) - self.ptr + + def _parse_msg_format(self): + if self._bytes_left() < self.MSG_FORMAT_PACKET_LEN: + raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self._bytes_left(), self.MSG_FORMAT_PACKET_LEN)) + msg_type = ord(self.buffer[self.ptr+3]) + msg_length = ord(self.buffer[self.ptr+4]) + msg_name = self.buffer[self.ptr+5:self.ptr+9].strip('\0') + msg_struct = self.buffer[self.ptr+9:self.ptr+25].strip('\0') + msg_labels = self.buffer[self.ptr+25:self.ptr+89].strip('\0').split(",") + print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s" % (msg_type, msg_length, msg_name, msg_struct, str(msg_labels)) + self.msg_formats[msg_type] = (msg_length, msg_name, msg_struct, msg_labels) + self.ptr += self.MSG_FORMAT_PACKET_LEN + + def _parse_msg(self, msg_format): + msg_length = msg_format[0] + msg_name = msg_format[1] + msg_struct = "<" + msg_format[2] + data = struct.unpack(msg_struct, self.buffer[self.ptr+self.MSG_HEADER_LEN:self.ptr+msg_length]) + print "MSG %s: %s" % (msg_name, str(data)) + self.ptr += msg_format[0] + +def _main(): + if len(sys.argv) < 2: + print "Usage:\npython sdlog2_dump.py " + return + fn = sys.argv[1] + parser = SDLog2Parser() + parser.process(fn) + +if __name__ == "__main__": + _main() diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 38b57082f4..8d4d2f8ced 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -492,6 +492,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } subs; /* log message buffer: header + body */ + #pragma pack(push, 1) struct { LOG_PACKET_HEADER; union { @@ -502,6 +503,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } log_msg = { LOG_PACKET_HEADER_INIT(0) }; + #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- MANAGEMENT - LOGGING COMMAND --- */ diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index f5347a7bdd..bbf8b6f12f 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -77,7 +77,7 @@ Format characters in the format string for binary log messages struct log_format_s { uint8_t type; - uint8_t length; + uint8_t length; // full packet length including header char name[4]; char format[16]; char labels[64]; @@ -85,13 +85,13 @@ struct log_format_s { #define LOG_FORMAT(_name, _format, _labels) { \ .type = LOG_##_name##_MSG, \ - .length = sizeof(struct log_##_name##_s) + 8, \ - .name = #_name, \ - .format = _format, \ - .labels = _labels \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ } -#define LOG_FORMAT_MSG 128 +#define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(log_msg.body.log_##_name) From 81e7af31856b0bb5db5dcc8ee90bc98cb6a4cf3d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 May 2013 15:35:43 +0400 Subject: [PATCH 03/17] sdlog2_dump.py now supports all fileds formats and able to parse original APM log --- Tools/sdlog2_dump.py | 85 ++++++++++++++++++++++++++++++++------------ 1 file changed, 63 insertions(+), 22 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index bdcbfdda7f..b74bf47396 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -5,7 +5,7 @@ Usage: python sdlog2_dump.py """ __author__ = "Anton Babushkin" -__version__ = "0.1" +__version__ = "0.2" import struct, sys @@ -18,13 +18,34 @@ class SDLog2Parser: 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), + } def __init__(self): return def reset(self): - self.msg_formats = {} + self.msg_descrs = {} self.buffer = "" self.ptr = 0 @@ -44,39 +65,59 @@ class SDLog2Parser: raise Exception("Invalid header: %02X %02X, must be %02X %02X" % (head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) msg_type = ord(self.buffer[self.ptr+2]) if msg_type == self.MSG_TYPE_FORMAT: - self._parse_msg_format() + self._parse_msg_descr() else: - msg_format = self.msg_formats[msg_type] - if msg_format == None: + msg_descr = self.msg_descrs[msg_type] + if msg_descr == None: raise Exception("Unknown msg type: %i" % msg_type) - msg_length = msg_format[0] + msg_length = msg_descr[0] if self._bytes_left() < msg_length: break - self._parse_msg(msg_format) + self._parse_msg(msg_descr) f.close() def _bytes_left(self): return len(self.buffer) - self.ptr - def _parse_msg_format(self): + def _parse_msg_descr(self): if self._bytes_left() < self.MSG_FORMAT_PACKET_LEN: raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self._bytes_left(), self.MSG_FORMAT_PACKET_LEN)) - msg_type = ord(self.buffer[self.ptr+3]) - msg_length = ord(self.buffer[self.ptr+4]) - msg_name = self.buffer[self.ptr+5:self.ptr+9].strip('\0') - msg_struct = self.buffer[self.ptr+9:self.ptr+25].strip('\0') - msg_labels = self.buffer[self.ptr+25:self.ptr+89].strip('\0').split(",") - print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s" % (msg_type, msg_length, msg_name, msg_struct, str(msg_labels)) - self.msg_formats[msg_type] = (msg_length, msg_name, msg_struct, msg_labels) + data = struct.unpack(self.MSG_FORMAT_STRUCT, self.buffer[self.ptr + 3 : self.ptr + self.MSG_FORMAT_PACKET_LEN]) + msg_type = data[0] + 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 (0x%02X)" % (c, msg_name, msg_type)) + msg_struct = "<" + msg_struct + print msg_format, msg_struct + 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.msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults) self.ptr += self.MSG_FORMAT_PACKET_LEN - def _parse_msg(self, msg_format): - msg_length = msg_format[0] - msg_name = msg_format[1] - msg_struct = "<" + msg_format[2] - data = struct.unpack(msg_struct, self.buffer[self.ptr+self.MSG_HEADER_LEN:self.ptr+msg_length]) - print "MSG %s: %s" % (msg_name, str(data)) - self.ptr += msg_format[0] + def _parse_msg(self, msg_descr): + msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr + data = list(struct.unpack(msg_struct, self.buffer[self.ptr+self.MSG_HEADER_LEN:self.ptr+msg_length])) + s = [] + 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 + s.append(msg_labels[i] + "=" + str(data[i])) + + print "MSG %s: %s" % (msg_name, ", ".join(s)) + self.ptr += msg_length def _main(): if len(sys.argv) < 2: From f5e405ef5b06f4bf26cf3758d962aa7884a7d94c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 May 2013 20:15:46 +0400 Subject: [PATCH 04/17] sdlog2_dump.py now generates customizible CSV output, messages/fields filter added. --- Tools/sdlog2_dump.py | 205 +++++++++++++++++++++++++++++++++---------- 1 file changed, 159 insertions(+), 46 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index b74bf47396..c8fc00a598 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -1,11 +1,21 @@ #!/usr/bin/env python -"""Parse and dump binary log generated by sdlog2 +"""Dump binary log generated by sdlog2 or APM as CSV - Usage: python sdlog2_dump.py """ +Usage: python sdlog2_dump.py [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] + + -v Use plain debug output instead of CSV. + + -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__ = "0.2" +__version__ = "1.0" import struct, sys @@ -40,54 +50,83 @@ class SDLog2Parser: "q": ("q", None), "Q": ("Q", None), } + __csv_delim = "," + __csv_null = "" + __msg_filter = {} + __debug_out = False def __init__(self): return def reset(self): - self.msg_descrs = {} - self.buffer = "" - self.ptr = 0 + self.__msg_descrs = {} + self.__buffer = "" + self.__ptr = 0 + self.__csv_columns = [] + self.__csv_data = {} + + 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 setDebugOut(self, debug_out): + self.__debug_out = debug_out def process(self, fn): self.reset() + first_data_msg = True f = open(fn, "r") 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._bytes_left() >= self.MSG_HEADER_LEN: - head1 = ord(self.buffer[self.ptr]) - head2 = ord(self.buffer[self.ptr+1]) + 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): raise Exception("Invalid header: %02X %02X, must be %02X %02X" % (head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) - msg_type = ord(self.buffer[self.ptr+2]) + msg_type = ord(self.__buffer[self.__ptr+2]) if msg_type == self.MSG_TYPE_FORMAT: - self._parse_msg_descr() + self.__parseMsgDescr() else: - msg_descr = self.msg_descrs[msg_type] + 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._bytes_left() < msg_length: + if self.__bytesLeft() < msg_length: break - self._parse_msg(msg_descr) + if first_data_msg: + print self.__csv_delim.join(self.__csv_columns) + first_data_msg = False + self.__parseMsg(msg_descr) + f.close() - def _bytes_left(self): - return len(self.buffer) - self.ptr + def __bytesLeft(self): + return len(self.__buffer) - self.__ptr - def _parse_msg_descr(self): - if self._bytes_left() < self.MSG_FORMAT_PACKET_LEN: - raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self._bytes_left(), self.MSG_FORMAT_PACKET_LEN)) - data = struct.unpack(self.MSG_FORMAT_STRUCT, self.buffer[self.ptr + 3 : self.ptr + self.MSG_FORMAT_PACKET_LEN]) + def __filterMsg(self, msg_name): + show_fields = "*" + if len(self.__msg_filter) > 0: + show_fields = self.__msg_filter.get(msg_name) + return show_fields + + def __parseMsgDescr(self): + if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: + raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self.__bytesLeft(), self.MSG_FORMAT_PACKET_LEN)) + data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) msg_type = data[0] msg_length = data[1] - msg_name = data[2].strip('\0') - msg_format = data[3].strip('\0') - msg_labels = data[4].strip('\0').split(",") + 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 = [] @@ -97,34 +136,108 @@ class SDLog2Parser: msg_struct += f[0] msg_mults.append(f[1]) except KeyError as e: - raise Exception("Unsupported format char: %s in message %s (0x%02X)" % (c, msg_name, msg_type)) - msg_struct = "<" + msg_struct - print msg_format, msg_struct - 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.msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults) - self.ptr += self.MSG_FORMAT_PACKET_LEN + 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) + show_fields = self.__filterMsg(msg_name) + if show_fields != None: + if self.__debug_out: + 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) + else: + if show_fields == "*": + fields = msg_labels + else: + fields = [] + for field in show_fields: + if field in msg_labels: + fields.append(field) + for field in fields: + msg_field = msg_name + "." + field + self.__csv_columns.append(msg_field) + self.__csv_data[msg_field] = None + self.__ptr += self.MSG_FORMAT_PACKET_LEN - def _parse_msg(self, msg_descr): + def __parseMsg(self, msg_descr): msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr - data = list(struct.unpack(msg_struct, self.buffer[self.ptr+self.MSG_HEADER_LEN:self.ptr+msg_length])) - s = [] - 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 - s.append(msg_labels[i] + "=" + str(data[i])) - - print "MSG %s: %s" % (msg_name, ", ".join(s)) - self.ptr += msg_length - + 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 show_fields == "*" or label in show_fields: + self.__csv_data[msg_name + "." + label] = data[i] + # format and print CSV row + s = [] + for field in self.__csv_columns: + v = self.__csv_data[field] + if v == None: + v = self.__csv_null + else: + v = str(v) + s.append(v) + print self.__csv_delim.join(s) + self.__ptr += msg_length + def _main(): if len(sys.argv) < 2: - print "Usage:\npython sdlog2_dump.py " + print "Usage: python sdlog2_dump.py [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]\n" + print "\t-v\tUse plain debug output instead of CSV.\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." return fn = sys.argv[1] + debug_out = False + msg_filter = {} + csv_null = "" + csv_delim = "," + opt = None + for arg in sys.argv[2:]: + if opt != None: + if opt == "d": + csv_delim = arg + elif opt == "n": + csv_null = arg + if opt == "m": + show_fields = "*" + a = arg.split(".") + if len(a) > 1: + show_fields = set(a[1].split(",")) + msg_filter[a[0]] = show_fields + opt = None + else: + if arg == "-v": + debug_out = True + elif arg == "-d": + opt = "d" + elif arg == "-n": + opt = "n" + elif arg == "-m": + opt = "m" + + if csv_delim == "\\t": + csv_delim = "\t" parser = SDLog2Parser() + parser.setCSVDelimiter(csv_delim) + parser.setCSVNull(csv_null) + parser.setMsgFilter(msg_filter) + parser.setDebugOut(debug_out) parser.process(fn) if __name__ == "__main__": From 84aa52c81a248f70171546ee7af317faa067f0ac Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 May 2013 18:01:03 +0400 Subject: [PATCH 05/17] sdlog2_dump.py now dumps CSV columns in the same order as args --- Tools/sdlog2_dump.py | 124 ++++++++++++++++++++++++------------------- 1 file changed, 69 insertions(+), 55 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index c8fc00a598..70e7403bc3 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -15,7 +15,7 @@ Usage: python sdlog2_dump.py [-v] [-d delimiter] [-n null] [-m MSG[.fi Multiple -m options allowed.""" __author__ = "Anton Babushkin" -__version__ = "1.0" +__version__ = "1.1" import struct, sys @@ -52,18 +52,21 @@ class SDLog2Parser: } __csv_delim = "," __csv_null = "" - __msg_filter = {} + __msg_filter = [] __debug_out = False def __init__(self): return - + def reset(self): - self.__msg_descrs = {} - self.__buffer = "" - self.__ptr = 0 - self.__csv_columns = [] - self.__csv_data = {} + 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.__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 @@ -79,6 +82,10 @@ class SDLog2Parser: 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") while True: @@ -94,8 +101,10 @@ class SDLog2Parser: raise Exception("Invalid header: %02X %02X, must be %02X %02X" % (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 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) @@ -103,61 +112,66 @@ class SDLog2Parser: if self.__bytesLeft() < msg_length: break if first_data_msg: - print self.__csv_delim.join(self.__csv_columns) + # build CSV columns and init data map + self.__initCSV() first_data_msg = False self.__parseMsg(msg_descr) - + f.close() - + def __bytesLeft(self): return len(self.__buffer) - self.__ptr - + def __filterMsg(self, msg_name): show_fields = "*" - if len(self.__msg_filter) > 0: - show_fields = self.__msg_filter.get(msg_name) + 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 __parseMsgDescr(self): if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self.__bytesLeft(), self.MSG_FORMAT_PACKET_LEN)) data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) msg_type = data[0] - 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) - show_fields = self.__filterMsg(msg_name) - if show_fields != None: + 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: - 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) - else: - if show_fields == "*": - fields = msg_labels - else: - fields = [] - for field in show_fields: - if field in msg_labels: - fields.append(field) - for field in fields: - msg_field = msg_name + "." + field - self.__csv_columns.append(msg_field) - self.__csv_data[msg_field] = None + 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 show_fields = self.__filterMsg(msg_name) @@ -180,12 +194,12 @@ class SDLog2Parser: # update CSV data buffer for i in xrange(len(data)): label = msg_labels[i] - if show_fields == "*" or label in show_fields: + if label in show_fields: self.__csv_data[msg_name + "." + label] = data[i] # format and print CSV row s = [] - for field in self.__csv_columns: - v = self.__csv_data[field] + for full_label in self.__csv_columns: + v = self.__csv_data[full_label] if v == None: v = self.__csv_null else: @@ -204,7 +218,7 @@ def _main(): return fn = sys.argv[1] debug_out = False - msg_filter = {} + msg_filter = [] csv_null = "" csv_delim = "," opt = None @@ -218,8 +232,8 @@ def _main(): show_fields = "*" a = arg.split(".") if len(a) > 1: - show_fields = set(a[1].split(",")) - msg_filter[a[0]] = show_fields + show_fields = a[1].split(",") + msg_filter.append((a[0], show_fields)) opt = None else: if arg == "-v": @@ -230,7 +244,7 @@ def _main(): opt = "n" elif arg == "-m": opt = "m" - + if csv_delim == "\\t": csv_delim = "\t" parser = SDLog2Parser() From 8dbda512897b3d2a05d39ebc26aba35b86ce044d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 May 2013 18:04:19 +0400 Subject: [PATCH 06/17] Cleanup --- Tools/sdlog2_dump.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 70e7403bc3..fa02a37237 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -19,9 +19,6 @@ __version__ = "1.1" import struct, sys -class BufferUnderflow(Exception): - pass - class SDLog2Parser: BLOCK_SIZE = 8192 MSG_HEADER_LEN = 3 @@ -102,6 +99,8 @@ class SDLog2Parser: 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 @@ -143,8 +142,6 @@ class SDLog2Parser: print self.__csv_delim.join(self.__csv_columns) def __parseMsgDescr(self): - if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: - raise BufferUnderflow("Data is too short: %i bytes, need %i" % (self.__bytesLeft(), self.MSG_FORMAT_PACKET_LEN)) 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: From 7e95edbbe848ec49ee81dbd85dc8c91558a83aa8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 May 2013 19:02:16 +0400 Subject: [PATCH 07/17] New messages added to sdlog2 --- src/modules/sdlog2/sdlog2.c | 189 +++++++++++++++------------ src/modules/sdlog2/sdlog2_format.h | 2 +- src/modules/sdlog2/sdlog2_messages.h | 77 +++++++++-- 3 files changed, 172 insertions(+), 96 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8d4d2f8ced..48d107945b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -81,6 +81,7 @@ #include #include "sdlog2_ringbuffer.h" +#include "sdlog2_format.h" #include "sdlog2_messages.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -447,7 +448,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 25; + const ssize_t fdsc = 13; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -455,7 +456,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { - struct sensor_combined_s raw; + struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; @@ -478,9 +479,9 @@ int sdlog2_thread_main(int argc, char *argv[]) int sensor_sub; int att_sub; int att_sp_sub; - int act_0_sub; - int controls_0_sub; - int controls_effective_0_sub; + int act_outputs_sub; + int act_controls_sub; + int act_controls_effective_sub; int local_pos_sub; int global_pos_sub; int gps_pos_sub; @@ -496,9 +497,13 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { LOG_PACKET_HEADER; union { - struct log_TS_s log_TS; + struct log_TIME_s log_TIME; struct log_ATT_s log_ATT; - struct log_RAW_s log_RAW; + struct log_ATSP_s log_ATSP; + struct log_IMU_s log_IMU; + struct log_SENS_s log_SENS; + struct log_LPOS_s log_LPOS; + struct log_LPSP_s log_LPSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -514,14 +519,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; /* do not rate limit, instead use skip counter (aliasing on rate limit) */ @@ -529,89 +532,65 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); fds[fdsc_count].fd = subs.att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); fds[fdsc_count].fd = subs.att_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /** --- ACTUATOR OUTPUTS --- */ - subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs.act_0_sub; + /* --- ACTUATOR OUTPUTS --- */ + subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + fds[fdsc_count].fd = subs.act_outputs_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls_0_sub; + subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.act_controls_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.controls_effective_0_sub; + subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + fds[fdsc_count].fd = subs.act_controls_effective_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- LOCAL POSITION --- */ - /* subscribe to ORB for local position */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL POSITION --- */ - /* subscribe to ORB for global position */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- VICON POSITION --- */ - /* subscribe to ORB for vicon position */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); fds[fdsc_count].fd = subs.vicon_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- FLOW measurements --- */ - /* subscribe to ORB for flow measurements */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- BATTERY STATUS --- */ - /* subscribe to ORB for flow measurements */ subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); fds[fdsc_count].fd = subs.batt_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- DIFFERENTIAL PRESSURE --- */ - /* subscribe to ORB for flow measurements */ - subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - fds[fdsc_count].fd = subs.diff_pres_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ - /* subscribe to ORB for airspeed */ - subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -625,7 +604,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * set up poll to block for new data, * wait for a maximum of 1000 ms (1 second) */ - // const int timeout = 1000; + const int poll_timeout = 1000; thread_running = true; @@ -641,13 +620,17 @@ int sdlog2_thread_main(int argc, char *argv[]) starttime = hrt_absolute_time(); - /* track skipping */ - int skip_count = 0; + /* track changes in sensor_combined topic */ + uint16_t gyro_counter = 0; + uint16_t accelerometer_counter = 0; + uint16_t magnetometer_counter = 0; + uint16_t baro_counter = 0; + uint16_t differential_pressure_counter = 0; while (!thread_should_exit) { /* poll all topics */ - int poll_ret = poll(fds, 4, 1000); + int poll_ret = poll(fds, fdsc, poll_timeout); /* handle the poll result */ if (poll_ret == 0) { @@ -666,11 +649,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ - log_msg.msg_type = LOG_TS_MSG; - log_msg.body.log_TS.t = hrt_absolute_time(); - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(TS)); + log_msg.msg_type = LOG_TIME_MSG; + log_msg.body.log_TIME.t = hrt_absolute_time(); + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(TIME)); - /* --- VEHICLE COMMAND VALUE --- */ + /* --- VEHICLE COMMAND --- */ if (fds[ifds++].revents & POLLIN) { /* copy command into local buffer */ orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -680,19 +663,58 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet } - /* --- SENSORS RAW VALUE --- */ + /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - log_msg.msg_type = LOG_RAW_MSG; - log_msg.body.log_RAW.accX = buf.raw.accelerometer_m_s2[0]; - log_msg.body.log_RAW.accY = buf.raw.accelerometer_m_s2[1]; - log_msg.body.log_RAW.accZ = buf.raw.accelerometer_m_s2[2]; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(RAW)); + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + bool write_IMU = false; + bool write_SENS = false; + if (buf.sensor.gyro_counter != gyro_counter) { + gyro_counter = buf.sensor.gyro_counter; + write_IMU = true; + } + if (buf.sensor.accelerometer_counter != accelerometer_counter) { + accelerometer_counter = buf.sensor.accelerometer_counter; + write_IMU = true; + } + if (buf.sensor.magnetometer_counter != magnetometer_counter) { + magnetometer_counter = buf.sensor.magnetometer_counter; + write_IMU = true; + } + if (buf.sensor.baro_counter != baro_counter) { + baro_counter = buf.sensor.baro_counter; + write_SENS = true; + } + if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { + differential_pressure_counter = buf.sensor.differential_pressure_counter; + write_SENS = true; + } + if (write_IMU) { + log_msg.msg_type = LOG_IMU_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(IMU)); + } + if (write_SENS) { + log_msg.msg_type = LOG_SENS_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(SENS)); + } } - /* --- ATTITUDE VALUE --- */ + /* --- ATTITUDE --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); log_msg.msg_type = LOG_ATT_MSG; @@ -702,69 +724,72 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATT)); } - /* --- ATTITUDE SETPOINT VALUE --- */ + /* --- ATTITUDE SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); - // TODO not implemented yet + log_msg.msg_type = LOG_ATSP_MSG; + log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; + log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; + log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATSP)); } /* --- ACTUATOR OUTPUTS --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); // TODO not implemented yet } - /* --- ACTUATOR CONTROL VALUE --- */ + /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); // TODO not implemented yet } - /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + /* --- ACTUATOR CONTROL EFFECTIVE --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); // TODO not implemented yet } /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + log_msg.msg_type = LOG_LPOS_MSG; + log_msg.body.log_LPOS.x = buf.local_pos.x; + log_msg.body.log_LPOS.y = buf.local_pos.y; + log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.vx = buf.local_pos.vx; + log_msg.body.log_LPOS.vy = buf.local_pos.vy; + log_msg.body.log_LPOS.vz = buf.local_pos.vz; + log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; + log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; + log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; + log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(LPOS)); } /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); // TODO not implemented yet } /* --- VICON POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); // TODO not implemented yet } - /* --- FLOW measurements --- */ + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); // TODO not implemented yet } /* --- BATTERY STATUS --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet - } - - /* --- DIFFERENTIAL PRESSURE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet - } - - /* --- AIRSPEED --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); // TODO not implemented yet } diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index bbf8b6f12f..59b91d90db 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -93,6 +93,6 @@ struct log_format_s { #define LOG_FORMAT_MSG 0x80 -#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(log_msg.body.log_##_name) +#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) #endif /* SDLOG2_FORMAT_H_ */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index ae98ea0381..0bc999e243 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -47,13 +47,13 @@ /* define message formats */ -/* === 1: TS - TIME STAMP === */ -#define LOG_TS_MSG 1 -struct log_TS_s { +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 1 +struct log_TIME_s { uint64_t t; }; -/* === 2: ATT - ATTITUDE === */ +/* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { float roll; @@ -61,20 +61,71 @@ struct log_ATT_s { float yaw; }; -/* === 3: RAW - SENSORS === */ -#define LOG_RAW_MSG 3 -struct log_RAW_s { - float accX; - float accY; - float accZ; +/* --- 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; +}; + +/* --- LPOS - LOCAL POSITION SETPOINT --- */ +#define LOG_LPSP_MSG 7 +struct log_LPSP_s { + float x; + float y; + float z; + float yaw; }; /* construct list of all message formats */ static const struct log_format_s log_formats[] = { - LOG_FORMAT(TS, "Q", "t"), - LOG_FORMAT(ATT, "fff", "roll,pitch,yaw"), - LOG_FORMAT(RAW, "fff", "accX,accY,accZ"), + LOG_FORMAT(TIME, "Q", "t"), + 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"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From d6ae0461ab5c89f87ac10a2304d55a893d0f72f9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 May 2013 12:28:05 +0400 Subject: [PATCH 08/17] sdlog2: GPS message added --- src/modules/sdlog2/sdlog2.c | 15 ++++++++++++++- src/modules/sdlog2/sdlog2_messages.h | 21 +++++++++++++++++++-- 2 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 48d107945b..8aa4db9348 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -504,6 +504,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_GPS_s log_GPS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -663,7 +664,19 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPS_MSG; + log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; + log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; + log_msg.body.log_GPS.satellites_visible = buf.gps_pos.satellites_visible; + log_msg.body.log_GPS.lat = buf.gps_pos.lat; + log_msg.body.log_GPS.lon = buf.gps_pos.lon; + log_msg.body.log_GPS.alt = buf.gps_pos.alt; + log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; + log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; + log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; + log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad; + log_msg.body.log_GPS.vel_valid = (uint8_t) buf.gps_pos.vel_ned_valid; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(GPS)); } /* --- SENSOR COMBINED --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0bc999e243..3ff5978152 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -107,7 +107,7 @@ struct log_LPOS_s { float home_alt; }; -/* --- LPOS - LOCAL POSITION SETPOINT --- */ +/* --- LPSP - LOCAL POSITION SETPOINT --- */ #define LOG_LPSP_MSG 7 struct log_LPSP_s { float x; @@ -116,16 +116,33 @@ struct log_LPSP_s { float yaw; }; +/* --- GPS - GPS POSITION --- */ +#define LOG_GPS_MSG 8 +struct log_GPS_s { + uint64_t gps_time; + uint8_t fix_type; + uint8_t satellites_visible; + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; + float cog; + uint8_t vel_valid; +}; + /* construct list of all message formats */ static const struct log_format_s log_formats[] = { - LOG_FORMAT(TIME, "Q", "t"), + 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, "QBBLLfffffB", "GPSTime,FixType,Sats,Lat,Lon,Alt,VelN,VelE,VelD,Cog,VelValid"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 9952fef645a04ca3b0d86dcb7bae203f27c77a03 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 May 2013 21:27:55 +0400 Subject: [PATCH 09/17] sdlog2 messages packing fixed, sdlog2_dump.py now produces much more compressed output. --- Tools/sdlog2_dump.py | 51 ++++++++++++++++++++-------- src/modules/sdlog2/sdlog2_messages.h | 2 ++ 2 files changed, 38 insertions(+), 15 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index fa02a37237..e4a4e24250 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -50,6 +50,7 @@ class SDLog2Parser: __csv_delim = "," __csv_null = "" __msg_filter = [] + __time_msg = None __debug_out = False def __init__(self): @@ -63,6 +64,7 @@ class SDLog2Parser: 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): @@ -74,6 +76,9 @@ class SDLog2Parser: 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 @@ -115,7 +120,8 @@ class SDLog2Parser: self.__initCSV() first_data_msg = False self.__parseMsg(msg_descr) - + if not self.__debug_out and self.__time_msg != None and self.__csv_updated: + self.__printCSVRow() f.close() def __bytesLeft(self): @@ -140,7 +146,18 @@ class SDLog2Parser: 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] @@ -171,6 +188,9 @@ class SDLog2Parser: 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])) @@ -193,31 +213,27 @@ class SDLog2Parser: label = msg_labels[i] if label in show_fields: self.__csv_data[msg_name + "." + label] = data[i] - # format and print CSV row - 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) + 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 [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]\n" + print "Usage: python sdlog2_dump.py [-v] [-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-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 msg_filter = [] csv_null = "" csv_delim = "," + time_msg = None opt = None for arg in sys.argv[2:]: if opt != None: @@ -225,7 +241,9 @@ def _main(): csv_delim = arg elif opt == "n": csv_null = arg - if opt == "m": + elif opt == "t": + time_msg = arg + elif opt == "m": show_fields = "*" a = arg.split(".") if len(a) > 1: @@ -241,13 +259,16 @@ def _main(): 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.process(fn) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3ff5978152..2baccc106e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -47,6 +47,7 @@ /* define message formats */ +#pragma pack(push, 1) /* --- TIME - TIME STAMP --- */ #define LOG_TIME_MSG 1 struct log_TIME_s { @@ -131,6 +132,7 @@ struct log_GPS_s { float cog; uint8_t vel_valid; }; +#pragma pack(pop) /* construct list of all message formats */ From b614d2f1eb3b3ddd26fa22a1a0761a5aef52c1a8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 May 2013 23:41:06 +0400 Subject: [PATCH 10/17] adlog2: added options cleanup, updates rate limit added --- src/modules/sdlog2/sdlog2.c | 75 +++++++++++++++++++++++-------------- 1 file changed, 47 insertions(+), 28 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8aa4db9348..2422a15f4c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -152,14 +152,19 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-s ]\n\n"); + errx(1, "usage: sdlog2 {start|stop|status} [-r ] -e -a\n" + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n\n"); } unsigned long log_bytes_written = 0; -uint64_t starttime = 0; +uint64_t start_time = 0; /* logging on or off, default to true */ -bool logging_enabled = true; +bool logging_enabled = false; +bool log_when_armed = false; +useconds_t poll_delay = 0; /** * The sd log deamon app only briefly exists to start @@ -384,24 +389,26 @@ int sdlog2_thread_main(int argc, char *argv[]) argv += 2; int ch; - while ((ch = getopt(argc, argv, "s:r")) != EOF) { + while ((ch = getopt(argc, argv, "r:ea")) != EOF) { switch (ch) { - case 's': { - /* log only every n'th (gyro clocked) value */ - unsigned s = strtoul(optarg, NULL, 10); + case 'r': { + unsigned r = strtoul(optarg, NULL, 10); - if (s < 1 || s > 250) { - errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); + if (r == 0) { + poll_delay = 0; } else { - skip_value = s; + poll_delay = 1000000 / r; } } break; - case 'r': - /* log only on request, disable logging per default */ - logging_enabled = false; + case 'e': + logging_enabled = true; + break; + + case 'a': + log_when_armed = true; break; case '?': @@ -493,7 +500,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } subs; /* log message buffer: header + body */ - #pragma pack(push, 1) +#pragma pack(push, 1) struct { LOG_PACKET_HEADER; union { @@ -509,7 +516,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } log_msg = { LOG_PACKET_HEADER_INIT(0) }; - #pragma pack(pop) +#pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -619,7 +626,9 @@ int sdlog2_thread_main(int argc, char *argv[]) /* start logbuffer emptying thread */ pthread_t logwriter_pthread = sdlog2_logwriter_start(&lb); - starttime = hrt_absolute_time(); + /* initialize statistics counter */ + log_bytes_written = 0; + start_time = hrt_absolute_time(); /* track changes in sensor_combined topic */ uint16_t gyro_counter = 0; @@ -629,24 +638,23 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t differential_pressure_counter = 0; while (!thread_should_exit) { + if (!logging_enabled) { + usleep(100000); + continue; + } /* poll all topics */ - int poll_ret = poll(fds, fdsc, poll_timeout); + int poll_ret = poll(fds, fdsc, poll_delay == 0 ? poll_timeout : 0); /* handle the poll result */ - if (poll_ret == 0) { - /* XXX this means none of our providers is giving us data - might be an error? */ - } else if (poll_ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else { + if (poll_ret < 0) { + printf("ERROR: Poll error, stop logging\n"); + thread_should_exit = false; + + } else if (poll_ret > 0) { int ifds = 0; - if (!logging_enabled) { - usleep(100000); - continue; - } - pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ @@ -684,26 +692,32 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); bool write_IMU = false; bool write_SENS = false; + if (buf.sensor.gyro_counter != gyro_counter) { gyro_counter = buf.sensor.gyro_counter; write_IMU = true; } + if (buf.sensor.accelerometer_counter != accelerometer_counter) { accelerometer_counter = buf.sensor.accelerometer_counter; write_IMU = true; } + if (buf.sensor.magnetometer_counter != magnetometer_counter) { magnetometer_counter = buf.sensor.magnetometer_counter; write_IMU = true; } + if (buf.sensor.baro_counter != baro_counter) { baro_counter = buf.sensor.baro_counter; write_SENS = true; } + if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { differential_pressure_counter = buf.sensor.differential_pressure_counter; write_SENS = true; } + if (write_IMU) { log_msg.msg_type = LOG_IMU_MSG; log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; @@ -717,6 +731,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(IMU)); } + if (write_SENS) { log_msg.msg_type = LOG_SENS_MSG; log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; @@ -815,6 +830,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* unlock, now the writer thread may run */ pthread_mutex_unlock(&logbuffer_mutex); } + + if (poll_delay > 0) { + usleep(poll_delay); + } } print_sdlog2_status(); @@ -841,7 +860,7 @@ int sdlog2_thread_main(int argc, char *argv[]) void print_sdlog2_status() { float mebibytes = log_bytes_written / 1024.0f / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); } From 1bf8f7b47ec8dd8f2f494fe40f193b3d1712e025 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Jun 2013 13:18:03 +0400 Subject: [PATCH 11/17] sdlog2 performance increased, fixes and cleanup --- .../{sdlog2_ringbuffer.c => logbuffer.c} | 25 +- .../{sdlog2_ringbuffer.h => logbuffer.h} | 22 +- src/modules/sdlog2/module.mk | 2 +- src/modules/sdlog2/sdlog2.c | 490 ++++++++++++------ 4 files changed, 357 insertions(+), 182 deletions(-) rename src/modules/sdlog2/{sdlog2_ringbuffer.c => logbuffer.c} (85%) rename src/modules/sdlog2/{sdlog2_ringbuffer.h => logbuffer.h} (78%) diff --git a/src/modules/sdlog2/sdlog2_ringbuffer.c b/src/modules/sdlog2/logbuffer.c similarity index 85% rename from src/modules/sdlog2/sdlog2_ringbuffer.c rename to src/modules/sdlog2/logbuffer.c index 022a183ba3..52eda649e0 100644 --- a/src/modules/sdlog2/sdlog2_ringbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -33,9 +33,9 @@ ****************************************************************************/ /** - * @file sdlog2_ringbuffer.c + * @file logbuffer.c * - * Ring FIFO buffer for binary data. + * Ring FIFO buffer for binary log data. * * @author Anton Babushkin */ @@ -43,9 +43,9 @@ #include #include -#include "sdlog2_ringbuffer.h" +#include "logbuffer.h" -void sdlog2_logbuffer_init(struct sdlog2_logbuffer *lb, int size) +void logbuffer_init(struct logbuffer_s *lb, int size) { lb->size = size; lb->write_ptr = 0; @@ -53,7 +53,7 @@ void sdlog2_logbuffer_init(struct sdlog2_logbuffer *lb, int size) lb->data = malloc(lb->size); } -int sdlog2_logbuffer_free(struct sdlog2_logbuffer *lb) +int logbuffer_free(struct logbuffer_s *lb) { int n = lb->read_ptr - lb->write_ptr - 1; @@ -64,7 +64,7 @@ int sdlog2_logbuffer_free(struct sdlog2_logbuffer *lb) return n; } -int sdlog2_logbuffer_count(struct sdlog2_logbuffer *lb) +int logbuffer_count(struct logbuffer_s *lb) { int n = lb->write_ptr - lb->read_ptr; @@ -75,12 +75,12 @@ int sdlog2_logbuffer_count(struct sdlog2_logbuffer *lb) return n; } -int sdlog2_logbuffer_is_empty(struct sdlog2_logbuffer *lb) +int logbuffer_is_empty(struct logbuffer_s *lb) { return lb->read_ptr == lb->write_ptr; } -void sdlog2_logbuffer_write(struct sdlog2_logbuffer *lb, void *ptr, int size) +bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) { // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; @@ -90,7 +90,7 @@ void sdlog2_logbuffer_write(struct sdlog2_logbuffer *lb, void *ptr, int size) if (size > available) { // buffer overflow - return; + return false; } char *c = (char *) ptr; @@ -109,9 +109,10 @@ void sdlog2_logbuffer_write(struct sdlog2_logbuffer *lb, void *ptr, int size) 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 sdlog2_logbuffer_get_ptr(struct sdlog2_logbuffer *lb, void **ptr) +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; @@ -125,17 +126,19 @@ int sdlog2_logbuffer_get_ptr(struct sdlog2_logbuffer *lb, void **ptr) if (available > 0) { // read pointer is before write pointer, write all available bytes n = available; + *is_part = false; } else { // read pointer is after write pointer, write bytes from read_ptr to end n = lb->size - lb->read_ptr; + *is_part = true; } *ptr = &(lb->data[lb->read_ptr]); return n; } -void sdlog2_logbuffer_mark_read(struct sdlog2_logbuffer *lb, int n) +void logbuffer_mark_read(struct logbuffer_s *lb, int n) { lb->read_ptr = (lb->read_ptr + n) % lb->size; } diff --git a/src/modules/sdlog2/sdlog2_ringbuffer.h b/src/modules/sdlog2/logbuffer.h similarity index 78% rename from src/modules/sdlog2/sdlog2_ringbuffer.h rename to src/modules/sdlog2/logbuffer.h index 287f56c34f..a1d29d33da 100644 --- a/src/modules/sdlog2/sdlog2_ringbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -33,9 +33,9 @@ ****************************************************************************/ /** - * @file sdlog2_ringbuffer.h + * @file logbuffer.h * - * Ring FIFO buffer for binary data. + * Ring FIFO buffer for binary log data. * * @author Anton Babushkin */ @@ -43,7 +43,9 @@ #ifndef SDLOG2_RINGBUFFER_H_ #define SDLOG2_RINGBUFFER_H_ -struct sdlog2_logbuffer { +#include + +struct logbuffer_s { // all pointers are in bytes int write_ptr; int read_ptr; @@ -51,18 +53,18 @@ struct sdlog2_logbuffer { char *data; }; -void sdlog2_logbuffer_init(struct sdlog2_logbuffer *lb, int size); +void logbuffer_init(struct logbuffer_s *lb, int size); -int sdlog2_logbuffer_free(struct sdlog2_logbuffer *lb); +int logbuffer_free(struct logbuffer_s *lb); -int sdlog2_logbuffer_count(struct sdlog2_logbuffer *lb); +int logbuffer_count(struct logbuffer_s *lb); -int sdlog2_logbuffer_is_empty(struct sdlog2_logbuffer *lb); +int logbuffer_is_empty(struct logbuffer_s *lb); -void sdlog2_logbuffer_write(struct sdlog2_logbuffer *lb, void *ptr, int size); +bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size); -int sdlog2_logbuffer_get_ptr(struct sdlog2_logbuffer *lb, void **ptr); +int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part); -void sdlog2_logbuffer_mark_read(struct sdlog2_logbuffer *lb, int n); +void logbuffer_mark_read(struct logbuffer_s *lb, int n); #endif diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 5a9936635a..f531296880 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -40,4 +40,4 @@ MODULE_COMMAND = sdlog2 MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ - sdlog2_ringbuffer.c + logbuffer.c diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2422a15f4c..6f509b917a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -68,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -80,40 +82,64 @@ #include -#include "sdlog2_ringbuffer.h" +#include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" +#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ + log_msgs_written++; \ + } else { \ + log_msgs_skipped++; \ + /*printf("skip\n");*/ \ + } + +//#define SDLOG2_DEBUG + 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 */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ -static const int LOG_BUFFER_SIZE = 2048; -static const int MAX_WRITE_CHUNK = 1024; +static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +static const int LOG_BUFFER_SIZE = 8192; +static const int MAX_WRITE_CHUNK = 512; +static const int MIN_BYTES_TO_WRITE = 512; static const char *mountpoint = "/fs/microsd"; int log_file = -1; int mavlink_fd = -1; -struct sdlog2_logbuffer lb; +struct logbuffer_s lb; /* mutex / condition to synchronize threads */ pthread_mutex_t logbuffer_mutex; pthread_cond_t logbuffer_cond; -/** - * System state vector log buffer writing - */ -static void *sdlog2_logbuffer_write_thread(void *arg); +char folder_path[64]; + +/* statistics counters */ +unsigned long log_bytes_written = 0; +uint64_t start_time = 0; +unsigned long log_msgs_written = 0; +unsigned long log_msgs_skipped = 0; + +/* current state of logging */ +bool logging_enabled = false; +/* enable logging on start (-e option) */ +bool log_on_start = false; +/* enable logging when armed (-a option) */ +bool log_when_armed = false; +/* delay = 1 / rate (rate defined by -r option) */ +useconds_t poll_delay = 0; + +/* helper flag to track system state changes */ +bool flag_system_armed = false; + +pthread_t logwriter_pthread = 0; /** - * Create the thread to write the system vector + * Log buffer writing thread. Open and close file here. */ -pthread_t sdlog2_logwriter_start(struct sdlog2_logbuffer *logbuf); - -/** - * Write a header to log file: list of message formats - */ -void sdlog2_write_formats(int fd); +static void *logwriter_thread(void *arg); /** * SD log management function. @@ -128,26 +154,49 @@ int sdlog2_thread_main(int argc, char *argv[]); /** * Print the correct usage. */ -static void usage(const char *reason); +static void sdlog2_usage(const char *reason); -static int file_exist(const char *filename); +/** + * Print the current status. + */ +static void sdlog2_status(void); + +/** + * Start logging: create new file and start log writer thread. + */ +void sdlog2_start_log(); + +/** + * Stop logging: stop log writer thread and close log file. + */ +void sdlog2_stop_log(); + +/** + * Write a header to log file: list of message formats. + */ +void write_formats(int fd); + + +static bool file_exist(const char *filename); static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -/** - * Print the current status. - */ -static void print_sdlog2_status(void); +static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. + * Create folder for current logging session. Store folder name in 'log_folder'. */ -static int create_logfolder(char *folder_path); +static int create_logfolder(); + +/** + * Select first free log file name and open it. + */ +static int open_logfile(); static void -usage(const char *reason) +sdlog2_usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -158,16 +207,8 @@ usage(const char *reason) "\t-a\tLog only when armed (can be still overriden by command)\n\n"); } -unsigned long log_bytes_written = 0; -uint64_t start_time = 0; - -/* logging on or off, default to true */ -bool logging_enabled = false; -bool log_when_armed = false; -useconds_t poll_delay = 0; - /** - * The sd log deamon app only briefly exists to start + * The logger deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. * @@ -177,7 +218,7 @@ useconds_t poll_delay = 0; int sdlog2_main(int argc, char *argv[]) { if (argc < 1) - usage("missing command"); + sdlog2_usage("missing command"); if (!strcmp(argv[1], "start")) { @@ -191,7 +232,7 @@ int sdlog2_main(int argc, char *argv[]) deamon_task = task_spawn("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, - 4096, + 2048, sdlog2_thread_main, (const char **)argv); exit(0); @@ -208,7 +249,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - print_sdlog2_status(); + sdlog2_status(); } else { printf("\tsdlog2 not started\n"); @@ -217,20 +258,20 @@ int sdlog2_main(int argc, char *argv[]) exit(0); } - usage("unrecognized command"); + sdlog2_usage("unrecognized command"); exit(1); } -int create_logfolder(char *folder_path) +int create_logfolder() { /* make folder on sdcard */ - uint16_t foldernumber = 1; // start with folder 0001 + uint16_t folder_number = 1; // start with folder sess001 int mkdir_ret; /* look for the next folder that does not exist */ - while (foldernumber < MAX_NO_LOGFOLDER) { - /* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */ - sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber); + while (folder_number <= MAX_NO_LOGFOLDER) { + /* set up folder path: e.g. /fs/microsd/sess001 */ + sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); /* the result is -1 if the folder exists */ @@ -256,7 +297,7 @@ int create_logfolder(char *folder_path) } else if (mkdir_ret == -1) { /* folder exists already */ - foldernumber++; + folder_number++; continue; } else { @@ -265,63 +306,126 @@ int create_logfolder(char *folder_path) } } - if (foldernumber >= MAX_NO_LOGFOLDER) { + if (folder_number >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); + warnx("all %d possible folders exist already.\n", MAX_NO_LOGFOLDER); return -1; } return 0; } -static void * -sdlog2_logbuffer_write_thread(void *arg) +int open_logfile() +{ + /* make folder on sdcard */ + uint16_t file_number = 1; // start with file log001 + + /* string to hold the path to the log */ + char path_buf[64] = ""; + + int fd = 0; + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ + sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); + + if (file_exist(path_buf)) { + file_number++; + continue; + } + + fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd == 0) { + errx(1, "opening %s failed.\n", path_buf); + } + + warnx("logging to: %s\n", path_buf); + + return fd; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warn("all %d possible files exist already\n", MAX_NO_LOGFILE); + return -1; + } + + return 0; +} + +static void *logwriter_thread(void *arg) { /* set name */ - prctl(PR_SET_NAME, "sdlog2 microSD I/O", 0); + prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct sdlog2_logbuffer *logbuf = (struct sdlog2_logbuffer *)arg; + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + + int log_file = open_logfile(); + + /* write log messages formats */ + write_formats(log_file); int poll_count = 0; void *read_ptr; int n = 0; + bool should_wait = false; + bool is_part = false; - while (!thread_should_exit) { + while (!thread_should_exit && !logwriter_should_exit) { /* make sure threads are synchronized */ pthread_mutex_lock(&logbuffer_mutex); /* update read pointer if needed */ if (n > 0) { - sdlog2_logbuffer_mark_read(&lb, n); + logbuffer_mark_read(&lb, n); } /* only wait if no data is available to process */ - if (sdlog2_logbuffer_is_empty(logbuf)) { + if (should_wait) { /* blocking wait for new data at this line */ pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex); } /* only get pointer to thread-safe data, do heavy I/O a few lines down */ - n = sdlog2_logbuffer_get_ptr(logbuf, &read_ptr); + int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); /* continue */ pthread_mutex_unlock(&logbuffer_mutex); - if (n > 0) { + if (available > 0) { /* do heavy IO here */ - if (n > MAX_WRITE_CHUNK) + if (available > MAX_WRITE_CHUNK) { n = MAX_WRITE_CHUNK; + } else { + n = available; + } + n = write(log_file, read_ptr, n); + should_wait = (n == available) && !is_part; +#ifdef SDLOG2_DEBUG + printf("%i wrote: %i of %i, is_part=%i, should_wait=%i\n", poll_count, n, available, (int)is_part, (int)should_wait); +#endif + + if (n < 0) { + thread_should_exit = true; + err(1, "error writing log file"); + } + if (n > 0) { log_bytes_written += n; } + + } else { + should_wait = true; } - if (poll_count % 100 == 0) { + if (poll_count % 10 == 0) { fsync(log_file); } @@ -329,12 +433,22 @@ sdlog2_logbuffer_write_thread(void *arg) } fsync(log_file); + close(log_file); return OK; } -pthread_t sdlog2_logwriter_start(struct sdlog2_logbuffer *logbuf) +void sdlog2_start_log() { + warnx("start logging.\n"); + + /* initialize statistics counter */ + log_bytes_written = 0; + start_time = hrt_absolute_time(); + log_msgs_written = 0; + log_msgs_skipped = 0; + + /* initialize log buffer emptying thread */ pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); @@ -345,14 +459,39 @@ pthread_t sdlog2_logwriter_start(struct sdlog2_logbuffer *logbuf) pthread_attr_setstacksize(&receiveloop_attr, 2048); + logwriter_should_exit = false; pthread_t thread; - pthread_create(&thread, &receiveloop_attr, sdlog2_logbuffer_write_thread, logbuf); - return thread; + /* start log buffer emptying thread */ + if (0 != pthread_create(&thread, &receiveloop_attr, logwriter_thread, &lb)) { + errx(1, "error creating logwriter thread\n"); + } + + logging_enabled = true; // XXX we have to destroy the attr at some point } -void sdlog2_write_formats(int fd) +void sdlog2_stop_log() +{ + warnx("stop logging.\n"); + + logging_enabled = true; + logwriter_should_exit = true; + + /* wake up write thread one last time */ + pthread_mutex_lock(&logbuffer_mutex); + pthread_cond_signal(&logbuffer_cond); + /* unlock, now the writer thread may return */ + pthread_mutex_unlock(&logbuffer_mutex); + + /* wait for write thread to return */ + (void)pthread_join(logwriter_pthread, NULL); + + sdlog2_status(); +} + + +void write_formats(int fd) { /* construct message format packet */ struct { @@ -378,7 +517,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("failed to open MAVLink log stream, start mavlink app first.\n"); } /* log every n'th value (skip three per default) */ @@ -404,7 +543,7 @@ int sdlog2_thread_main(int argc, char *argv[]) break; case 'e': - logging_enabled = true; + log_on_start = true; break; case 'a': @@ -423,39 +562,48 @@ int sdlog2_thread_main(int argc, char *argv[]) } default: - usage("unrecognized flag"); - errx(1, "exiting."); + sdlog2_usage("unrecognized flag"); + errx(1, "exiting\n"); } } - if (file_exist(mountpoint) != OK) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); + if (!file_exist(mountpoint)) { + errx(1, "logging mount point %s not present, exiting.\n", mountpoint); } - char folder_path[64]; - - if (create_logfolder(folder_path)) - errx(1, "unable to create logging folder, exiting."); - - /* string to hold the path to the sensorfile */ - char path_buf[64] = ""; + if (create_logfolder()) + errx(1, "unable to create logging folder, exiting.\n"); /* only print logging path, important to find log file later */ - warnx("logging to directory %s\n", folder_path); + warnx("logging to directory %s.\n", folder_path); - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "log"); + /* logging control buffers and subscriptions */ + struct { + struct vehicle_command_s cmd; + struct vehicle_status_s status; + } buf_control; + memset(&buf_control, 0, sizeof(buf_control)); - if (0 == (log_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } + struct { + int cmd_sub; + int status_sub; + } subs_control; - /* write log messages formats */ - sdlog2_write_formats(log_file); + /* file descriptors to wait for */ + struct pollfd fds_control[2]; + /* --- VEHICLE COMMAND --- */ + subs_control.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + fds_control[0].fd = subs_control.cmd_sub; + fds_control[0].events = POLLIN; + + /* --- VEHICLE STATUS --- */ + subs_control.status_sub = orb_subscribe(ORB_ID(vehicle_status)); + fds_control[1].fd = subs_control.status_sub; + fds_control[1].events = POLLIN; /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 13; + const ssize_t fdsc = 12; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -469,8 +617,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; struct actuator_controls_effective_s act_controls_effective; - struct vehicle_command_s cmd; struct vehicle_local_position_s local_pos; + struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; @@ -482,7 +630,6 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { - int cmd_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -490,6 +637,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int act_controls_sub; int act_controls_effective_sub; int local_pos_sub; + int local_pos_sp_sub; int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; @@ -519,33 +667,25 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); - /* --- MANAGEMENT - LOGGING COMMAND --- */ - /* subscribe to ORB for vehicle command */ - subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds[fdsc_count].fd = subs.cmd_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- GPS POSITION --- */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SENSORS RAW VALUE --- */ + /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; - /* do not rate limit, instead use skip counter (aliasing on rate limit) */ fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ATTITUDE VALUE --- */ + /* --- ATTITUDE --- */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); fds[fdsc_count].fd = subs.att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ATTITUDE SETPOINT VALUE --- */ + /* --- ATTITUDE SETPOINT --- */ subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); fds[fdsc_count].fd = subs.att_sp_sub; fds[fdsc_count].events = POLLIN; @@ -557,13 +697,13 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR CONTROL VALUE --- */ + /* --- ACTUATOR CONTROL --- */ subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); fds[fdsc_count].fd = subs.act_controls_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + /* --- ACTUATOR CONTROL EFFECTIVE --- */ subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); fds[fdsc_count].fd = subs.act_controls_effective_sub; fds[fdsc_count].events = POLLIN; @@ -575,6 +715,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- LOCAL POSITION SETPOINT --- */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + fds[fdsc_count].fd = subs.local_pos_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- GLOBAL POSITION --- */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; @@ -587,7 +733,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- FLOW measurements --- */ + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; fds[fdsc_count].events = POLLIN; @@ -610,26 +756,19 @@ int sdlog2_thread_main(int argc, char *argv[]) /* * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) + * wait for a maximum of 1000 ms */ const int poll_timeout = 1000; thread_running = true; /* initialize log buffer with specified size */ - sdlog2_logbuffer_init(&lb, LOG_BUFFER_SIZE); + logbuffer_init(&lb, LOG_BUFFER_SIZE); /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); - /* start logbuffer emptying thread */ - pthread_t logwriter_pthread = sdlog2_logwriter_start(&lb); - - /* initialize statistics counter */ - log_bytes_written = 0; - start_time = hrt_absolute_time(); - /* track changes in sensor_combined topic */ uint16_t gyro_counter = 0; uint16_t accelerometer_counter = 0; @@ -637,19 +776,44 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t baro_counter = 0; uint16_t differential_pressure_counter = 0; + /* enable logging on start if needed */ + if (log_on_start) + sdlog2_start_log(); + while (!thread_should_exit) { - if (!logging_enabled) { - usleep(100000); + /* poll control topics */ + int poll_control_ret = poll(fds_control, sizeof(fds_control) / sizeof(fds_control[0]), logging_enabled ? 0 : 500); + + /* handle the poll result */ + if (poll_control_ret < 0) { + warnx("ERROR: poll control topics error, stop logging.\n"); + thread_should_exit = true; continue; + + } else if (poll_control_ret > 0) { + /* --- VEHICLE COMMAND --- */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), subs_control.cmd_sub, &buf_control.cmd); + handle_command(&buf_control.cmd); + } + + /* --- VEHICLE STATUS --- */ + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), subs_control.status_sub, &buf_control.status); + handle_status(&buf_control.status); + } } - /* poll all topics */ + if (!logging_enabled) + continue; + + /* poll data topics */ int poll_ret = poll(fds, fdsc, poll_delay == 0 ? poll_timeout : 0); /* handle the poll result */ if (poll_ret < 0) { - printf("ERROR: Poll error, stop logging\n"); - thread_should_exit = false; + warnx("ERROR: poll error, stop logging.\n"); + thread_should_exit = true; } else if (poll_ret > 0) { @@ -660,14 +824,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* write time stamp message */ log_msg.msg_type = LOG_TIME_MSG; log_msg.body.log_TIME.t = hrt_absolute_time(); - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(TIME)); - - /* --- VEHICLE COMMAND --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy command into local buffer */ - orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - handle_command(&buf.cmd); - } + LOGBUFFER_WRITE_AND_COUNT(TIME); /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { @@ -684,7 +841,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad; log_msg.body.log_GPS.vel_valid = (uint8_t) buf.gps_pos.vel_ned_valid; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(GPS)); + LOGBUFFER_WRITE_AND_COUNT(GPS); } /* --- SENSOR COMBINED --- */ @@ -729,7 +886,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(IMU)); + LOGBUFFER_WRITE_AND_COUNT(IMU); } if (write_SENS) { @@ -738,7 +895,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(SENS)); + LOGBUFFER_WRITE_AND_COUNT(SENS); } } @@ -749,7 +906,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATT)); + LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ @@ -759,7 +916,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATSP)); + LOGBUFFER_WRITE_AND_COUNT(ATSP); } /* --- ACTUATOR OUTPUTS --- */ @@ -794,7 +951,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(LPOS)); + LOGBUFFER_WRITE_AND_COUNT(LPOS); + } + + /* --- LOCAL POSITION SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp); + log_msg.msg_type = LOG_LPSP_MSG; + log_msg.body.log_LPSP.x = buf.local_pos_sp.x; + log_msg.body.log_LPSP.y = buf.local_pos_sp.y; + log_msg.body.log_LPSP.z = buf.local_pos_sp.z; + log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(LPSP); } /* --- GLOBAL POSITION --- */ @@ -822,7 +990,10 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* signal the other thread new data, but not yet unlock */ - if (sdlog2_logbuffer_count(&lb) > (lb.size / 2)) { + if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { +#ifdef SDLOG2_DEBUG + printf("signal %i\n", logbuffer_count(&lb)); +#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } @@ -836,42 +1007,34 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - print_sdlog2_status(); - - /* wake up write thread one last time */ - pthread_mutex_lock(&logbuffer_mutex); - pthread_cond_signal(&logbuffer_cond); - /* unlock, now the writer thread may return */ - pthread_mutex_unlock(&logbuffer_mutex); - - /* wait for write thread to return */ - (void)pthread_join(logwriter_pthread, NULL); + if (logging_enabled) + sdlog2_stop_log(); pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); - warnx("exiting.\n\n"); + warnx("exiting.\n"); thread_running = false; return 0; } -void print_sdlog2_status() +void sdlog2_status() { float mebibytes = log_bytes_written / 1024.0f / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f MiB/s), skipped %lu msgs.\n", log_msgs_written, (double)mebibytes, (double)(mebibytes / seconds), log_msgs_skipped); } /** * @return 0 if file exists */ -int file_exist(const char *filename) +bool file_exist(const char *filename) { struct stat buffer; - return stat(filename, &buffer); + return stat(filename, &buffer) == 0; } int file_copy(const char *file_old, const char *file_new) @@ -881,7 +1044,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy"); + warnx("failed opening input file to copy.\n"); return 1; } @@ -889,7 +1052,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy"); + warnx("failed to open output file to copy.\n"); return 1; } @@ -900,7 +1063,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file"); + warnx("error writing file.\n"); ret = 1; break; } @@ -918,25 +1081,19 @@ void handle_command(struct vehicle_command_s *cmd) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + int param; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_PREFLIGHT_STORAGE: + param = (int)(cmd->param3); - if (((int)(cmd->param3)) == 1) { + if (param == 1) { + sdlog2_start_log(); - /* enable logging */ - mavlink_log_info(mavlink_fd, "[log] file:"); - mavlink_log_info(mavlink_fd, "logdir"); - logging_enabled = true; - } - - if (((int)(cmd->param3)) == 0) { - - /* disable logging */ - mavlink_log_info(mavlink_fd, "[log] stopped."); - logging_enabled = false; + } else if (param == 0) { + sdlog2_stop_log(); } break; @@ -945,5 +1102,18 @@ void handle_command(struct vehicle_command_s *cmd) /* silently ignore */ break; } - +} + +void handle_status(struct vehicle_status_s *status) +{ + if (log_when_armed && (status->flag_system_armed != flag_system_armed)) { + flag_system_armed = status->flag_system_armed; + + if (flag_system_armed) { + sdlog2_start_log(); + + } else { + sdlog2_stop_log(); + } + } } From 34d4d62acc132b8a28395c4ab943f6e424b999c6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Jun 2013 15:59:42 +0400 Subject: [PATCH 12/17] sdlog2 messages cleanup, fixes --- src/modules/sdlog2/sdlog2.c | 166 ++++++++++++++++++------------------ 1 file changed, 84 insertions(+), 82 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6f509b917a..27efe2c62f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -93,6 +93,12 @@ /*printf("skip\n");*/ \ } +#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ + fds[fdsc_count].fd = subs.##_var##_sub; \ + fds[fdsc_count].events = POLLIN; \ + fdsc_count++; + + //#define SDLOG2_DEBUG static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -129,7 +135,7 @@ bool log_on_start = false; /* enable logging when armed (-a option) */ bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ -useconds_t poll_delay = 0; +useconds_t sleep_delay = 0; /* helper flag to track system state changes */ bool flag_system_armed = false; @@ -204,7 +210,7 @@ sdlog2_usage(const char *reason) errx(1, "usage: sdlog2 {start|stop|status} [-r ] -e -a\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n\n"); + "\t-a\tLog only when armed (can be still overriden by command)\n"); } /** @@ -217,7 +223,7 @@ sdlog2_usage(const char *reason) */ int sdlog2_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) sdlog2_usage("missing command"); if (!strcmp(argv[1], "start")) { @@ -308,7 +314,7 @@ int create_logfolder() if (folder_number >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible folders exist already.\n", MAX_NO_LOGFOLDER); + warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); return -1; } @@ -338,17 +344,18 @@ int open_logfile() fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); if (fd == 0) { - errx(1, "opening %s failed.\n", path_buf); + errx(1, "opening %s failed.", path_buf); } - warnx("logging to: %s\n", path_buf); + warnx("logging to: %s", path_buf); + mavlink_log_info(mavlink_fd, "[sdlog2] logging to: %s", path_buf); return fd; } if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warn("all %d possible files exist already\n", MAX_NO_LOGFILE); + warn("all %d possible files exist already", MAX_NO_LOGFILE); return -1; } @@ -409,7 +416,7 @@ static void *logwriter_thread(void *arg) should_wait = (n == available) && !is_part; #ifdef SDLOG2_DEBUG - printf("%i wrote: %i of %i, is_part=%i, should_wait=%i\n", poll_count, n, available, (int)is_part, (int)should_wait); + printf("%i wrote: %i of %i, is_part=%i, should_wait=%i", poll_count, n, available, (int)is_part, (int)should_wait); #endif if (n < 0) { @@ -440,7 +447,8 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging.\n"); + warnx("start logging."); + mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); /* initialize statistics counter */ log_bytes_written = 0; @@ -464,7 +472,7 @@ void sdlog2_start_log() /* start log buffer emptying thread */ if (0 != pthread_create(&thread, &receiveloop_attr, logwriter_thread, &lb)) { - errx(1, "error creating logwriter thread\n"); + errx(1, "error creating logwriter thread"); } logging_enabled = true; @@ -473,7 +481,8 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging.\n"); + warnx("stop logging."); + mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = true; logwriter_should_exit = true; @@ -517,7 +526,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("failed to open MAVLink log stream, start mavlink app first."); } /* log every n'th value (skip three per default) */ @@ -534,10 +543,10 @@ int sdlog2_thread_main(int argc, char *argv[]) unsigned r = strtoul(optarg, NULL, 10); if (r == 0) { - poll_delay = 0; + sleep_delay = 0; } else { - poll_delay = 1000000 / r; + sleep_delay = 1000000 / r; } } break; @@ -552,58 +561,37 @@ int sdlog2_thread_main(int argc, char *argv[]) case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.\n", optopt); + warnx("Option -%c requires an argument.", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.\n", optopt); + warnx("Unknown option `-%c'.", optopt); } else { - warnx("Unknown option character `\\x%x'.\n", optopt); + warnx("Unknown option character `\\x%x'.", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting\n"); + errx(1, "exiting"); } } if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.\n", mountpoint); + errx(1, "logging mount point %s not present, exiting.", mountpoint); } if (create_logfolder()) - errx(1, "unable to create logging folder, exiting.\n"); + errx(1, "unable to create logging folder, exiting."); /* only print logging path, important to find log file later */ - warnx("logging to directory %s.\n", folder_path); - - /* logging control buffers and subscriptions */ - struct { - struct vehicle_command_s cmd; - struct vehicle_status_s status; - } buf_control; - memset(&buf_control, 0, sizeof(buf_control)); - - struct { - int cmd_sub; - int status_sub; - } subs_control; + warnx("logging to directory: %s", folder_path); /* file descriptors to wait for */ struct pollfd fds_control[2]; - /* --- VEHICLE COMMAND --- */ - subs_control.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds_control[0].fd = subs_control.cmd_sub; - fds_control[0].events = POLLIN; - - /* --- VEHICLE STATUS --- */ - subs_control.status_sub = orb_subscribe(ORB_ID(vehicle_status)); - fds_control[1].fd = subs_control.status_sub; - fds_control[1].events = POLLIN; /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 12; + const ssize_t fdsc = 15; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -611,6 +599,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { + struct vehicle_command_s cmd; + struct vehicle_status_s status; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -630,6 +620,8 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { + int cmd_sub; + int status_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -667,6 +659,18 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- VEHICLE COMMAND --- */ + subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + fds[fdsc_count].fd = subs.cmd_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- VEHICLE STATUS --- */ + subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); + fds[fdsc_count].fd = subs.status_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- GPS POSITION --- */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; @@ -716,7 +720,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- LOCAL POSITION SETPOINT --- */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); fds[fdsc_count].fd = subs.local_pos_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -750,7 +754,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -781,44 +785,42 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_start_log(); while (!thread_should_exit) { - /* poll control topics */ - int poll_control_ret = poll(fds_control, sizeof(fds_control) / sizeof(fds_control[0]), logging_enabled ? 0 : 500); + /* decide use usleep() or blocking poll() */ + bool use_sleep = sleep_delay > 0 && logging_enabled; - /* handle the poll result */ - if (poll_control_ret < 0) { - warnx("ERROR: poll control topics error, stop logging.\n"); - thread_should_exit = true; - continue; - - } else if (poll_control_ret > 0) { - /* --- VEHICLE COMMAND --- */ - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_command), subs_control.cmd_sub, &buf_control.cmd); - handle_command(&buf_control.cmd); - } - - /* --- VEHICLE STATUS --- */ - if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs_control.status_sub, &buf_control.status); - handle_status(&buf_control.status); - } - } - - if (!logging_enabled) - continue; - - /* poll data topics */ - int poll_ret = poll(fds, fdsc, poll_delay == 0 ? poll_timeout : 0); + /* poll all topics if logging enabled or only management (first 2) if not */ + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging.\n"); + warnx("ERROR: poll error, stop logging."); thread_should_exit = true; } else if (poll_ret > 0) { + /* check all data subscriptions only if logging enabled, + * logging_enabled can be changed while checking vehicle_command and vehicle_status */ + bool check_data = logging_enabled; int ifds = 0; + /* --- VEHICLE COMMAND --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + handle_command(&buf.cmd); + } + + /* --- VEHICLE STATUS --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf.status); + if (log_when_armed) { + handle_status(&buf.status); + } + } + + if (!logging_enabled || !check_data) { + continue; + } + pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ @@ -992,7 +994,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { #ifdef SDLOG2_DEBUG - printf("signal %i\n", logbuffer_count(&lb)); + printf("signal %i", logbuffer_count(&lb)); #endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); @@ -1002,8 +1004,8 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_unlock(&logbuffer_mutex); } - if (poll_delay > 0) { - usleep(poll_delay); + if (use_sleep) { + usleep(sleep_delay); } } @@ -1013,7 +1015,7 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); - warnx("exiting.\n"); + warnx("exiting."); thread_running = false; @@ -1025,7 +1027,7 @@ void sdlog2_status() float mebibytes = log_bytes_written / 1024.0f / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f MiB/s), skipped %lu msgs.\n", log_msgs_written, (double)mebibytes, (double)(mebibytes / seconds), log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f MiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(mebibytes / seconds), log_msgs_skipped); } /** @@ -1044,7 +1046,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy.\n"); + warnx("failed opening input file to copy."); return 1; } @@ -1052,7 +1054,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy.\n"); + warnx("failed to open output file to copy."); return 1; } @@ -1063,7 +1065,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file.\n"); + warnx("error writing file."); ret = 1; break; } @@ -1106,7 +1108,7 @@ void handle_command(struct vehicle_command_s *cmd) void handle_status(struct vehicle_status_s *status) { - if (log_when_armed && (status->flag_system_armed != flag_system_armed)) { + if (status->flag_system_armed != flag_system_armed) { flag_system_armed = status->flag_system_armed; if (flag_system_armed) { From 9f895d87cd159205cf1e66be49cc4b1f787b54ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Jun 2013 17:16:12 +0400 Subject: [PATCH 13/17] sdlog2 mavlink msg fix --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 27efe2c62f..83bc338e2e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -348,7 +348,7 @@ int open_logfile() } warnx("logging to: %s", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] logging to: %s", path_buf); + mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); return fd; } From 606f68c890dfc15ca29262f6c7c2eff29c9dfec7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Jun 2013 20:40:56 +0400 Subject: [PATCH 14/17] sdlog2 GPS message changes --- src/modules/sdlog2/sdlog2.c | 6 +++--- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 83bc338e2e..b5821098ff 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -834,15 +834,15 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; - log_msg.body.log_GPS.satellites_visible = buf.gps_pos.satellites_visible; + log_msg.body.log_GPS.eph = buf.gps_pos.eph_m; + log_msg.body.log_GPS.epv = buf.gps_pos.epv_m; log_msg.body.log_GPS.lat = buf.gps_pos.lat; log_msg.body.log_GPS.lon = buf.gps_pos.lon; - log_msg.body.log_GPS.alt = buf.gps_pos.alt; + log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001; log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad; - log_msg.body.log_GPS.vel_valid = (uint8_t) buf.gps_pos.vel_ned_valid; LOGBUFFER_WRITE_AND_COUNT(GPS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2baccc106e..316459b1e9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -122,7 +122,8 @@ struct log_LPSP_s { struct log_GPS_s { uint64_t gps_time; uint8_t fix_type; - uint8_t satellites_visible; + float eph; + float epv; int32_t lat; int32_t lon; float alt; @@ -130,7 +131,6 @@ struct log_GPS_s { float vel_e; float vel_d; float cog; - uint8_t vel_valid; }; #pragma pack(pop) @@ -144,7 +144,7 @@ static const struct log_format_s log_formats[] = { 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, "QBBLLfffffB", "GPSTime,FixType,Sats,Lat,Lon,Alt,VelN,VelE,VelD,Cog,VelValid"), + 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); From bd8bafb347e9b020ef78ffc2b534161d3c3b71bd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Jun 2013 10:43:39 +0400 Subject: [PATCH 15/17] sdlog2_dump.py: "recover from errors" option --- Tools/sdlog2_dump.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index e4a4e24250..e40d5c1b51 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -52,6 +52,7 @@ class SDLog2Parser: __msg_filter = [] __time_msg = None __debug_out = False + __correct_errors = False def __init__(self): return @@ -81,6 +82,9 @@ class SDLog2Parser: 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() @@ -90,6 +94,7 @@ class SDLog2Parser: 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: @@ -100,7 +105,11 @@ class SDLog2Parser: head1 = ord(self.__buffer[self.__ptr]) head2 = ord(self.__buffer[self.__ptr+1]) if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2): - raise Exception("Invalid header: %02X %02X, must be %02X %02X" % (head1, head2, self.MSG_HEAD1, 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 @@ -120,6 +129,7 @@ class SDLog2Parser: 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() @@ -221,8 +231,9 @@ class SDLog2Parser: def _main(): if len(sys.argv) < 2: - print "Usage: python sdlog2_dump.py [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n" + print "Usage: python sdlog2_dump.py [-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." @@ -230,6 +241,7 @@ def _main(): return fn = sys.argv[1] debug_out = False + correct_errors = False msg_filter = [] csv_null = "" csv_delim = "," @@ -253,6 +265,8 @@ def _main(): else: if arg == "-v": debug_out = True + elif arg == "-e": + correct_errors = True elif arg == "-d": opt = "d" elif arg == "-n": @@ -270,6 +284,7 @@ def _main(): parser.setMsgFilter(msg_filter) parser.setTimeMsg(time_msg) parser.setDebugOut(debug_out) + parser.setCorrectErrors(correct_errors) parser.process(fn) if __name__ == "__main__": From 6e5e1ff817d11d84b55b63dd482f3205e0899c55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Jun 2013 12:22:43 +0400 Subject: [PATCH 16/17] sdlog2_dump.py comments and version updated --- Tools/sdlog2_dump.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index e40d5c1b51..318f72971f 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -2,10 +2,12 @@ """Dump binary log generated by sdlog2 or APM as CSV -Usage: python sdlog2_dump.py [-v] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] +Usage: python sdlog2_dump.py [-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. @@ -15,7 +17,7 @@ Usage: python sdlog2_dump.py [-v] [-d delimiter] [-n null] [-m MSG[.fi Multiple -m options allowed.""" __author__ = "Anton Babushkin" -__version__ = "1.1" +__version__ = "1.2" import struct, sys From 7ae2cf9d2de9a07249eccdcae10d3ac84794d0fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Jun 2013 16:48:55 +0400 Subject: [PATCH 17/17] Minor sdlog2/logbuffer cleanup --- src/modules/sdlog2/logbuffer.c | 15 ++------------- src/modules/sdlog2/logbuffer.h | 2 -- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 52eda649e0..4205bcf20d 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -53,17 +53,6 @@ void logbuffer_init(struct logbuffer_s *lb, int size) lb->data = malloc(lb->size); } -int logbuffer_free(struct logbuffer_s *lb) -{ - int n = lb->read_ptr - lb->write_ptr - 1; - - if (n < 0) { - n += lb->size; - } - - return n; -} - int logbuffer_count(struct logbuffer_s *lb) { int n = lb->write_ptr - lb->read_ptr; @@ -124,12 +113,12 @@ int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part) int n = 0; if (available > 0) { - // read pointer is before write pointer, write all available bytes + // read pointer is before write pointer, all available bytes can be read n = available; *is_part = false; } else { - // read pointer is after write pointer, write bytes from read_ptr to end + // 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; } diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h index a1d29d33da..e9635e5e73 100644 --- a/src/modules/sdlog2/logbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -55,8 +55,6 @@ struct logbuffer_s { void logbuffer_init(struct logbuffer_s *lb, int size); -int logbuffer_free(struct logbuffer_s *lb); - int logbuffer_count(struct logbuffer_s *lb); int logbuffer_is_empty(struct logbuffer_s *lb);