diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-23 14:18:46 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-23 14:18:46 -0700 |
commit | c4901579e7a11d654482ae509d36e346d1b6dec4 (patch) | |
tree | 6e470f7203d959e94b624e3b58a0520b5c31f140 | |
parent | bdf7d515613151127a401a4da2300d0ecc9a3806 (diff) | |
parent | bb8a2c3631a518f822d1e7e3d40c28a281901a3f (diff) | |
download | px4-firmware-c4901579e7a11d654482ae509d36e346d1b6dec4.tar.gz px4-firmware-c4901579e7a11d654482ae509d36e346d1b6dec4.tar.bz2 px4-firmware-c4901579e7a11d654482ae509d36e346d1b6dec4.zip |
Merge pull request #489 from PX4/sdlog2_params
sdlog2: parameters logging
-rw-r--r-- | Tools/sdlog2_dump.py | 25 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 122 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_format.h | 8 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 27 |
4 files changed, 122 insertions, 60 deletions
diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 7fefc5908..5b1e55e22 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -25,8 +25,12 @@ import struct, sys if sys.hexversion >= 0x030000F0: runningPython3 = True + def _parseCString(cstr): + return str(cstr, 'ascii').split('\0')[0] else: runningPython3 = False + def _parseCString(cstr): + return str(cstr).split('\0')[0] class SDLog2Parser: BLOCK_SIZE = 8192 @@ -175,9 +179,9 @@ class SDLog2Parser: self.__csv_columns.append(full_label) self.__csv_data[full_label] = None if self.__file != None: - print(self.__csv_delim.join(self.__csv_columns), file=self.__file) + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) else: - print(self.__csv_delim.join(self.__csv_columns)) + print(self.__csv_delim.join(self.__csv_columns)) def __printCSVRow(self): s = [] @@ -190,9 +194,9 @@ class SDLog2Parser: s.append(v) if self.__file != None: - print(self.__csv_delim.join(s), file=self.__file) + print(self.__csv_delim.join(s), file=self.__file) else: - print(self.__csv_delim.join(s)) + print(self.__csv_delim.join(s)) def __parseMsgDescr(self): if runningPython3: @@ -202,14 +206,9 @@ class SDLog2Parser: msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - if runningPython3: - msg_name = str(data[2], 'ascii').strip("\0") - msg_format = str(data[3], 'ascii').strip("\0") - msg_labels = str(data[4], 'ascii').strip("\0").split(",") - else: - msg_name = str(data[2]).strip("\0") - msg_format = str(data[3]).strip("\0") - msg_labels = str(data[4]).strip("\0").split(",") + msg_name = _parseCString(data[2]) + msg_format = _parseCString(data[3]) + msg_labels = _parseCString(data[4]).split(",") # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -243,7 +242,7 @@ class SDLog2Parser: data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))) for i in range(len(data)): if type(data[i]) is str: - data[i] = data[i].strip("\0") + data[i] = _parseCString(data[i]) m = msg_mults[i] if m != None: data[i] = data[i] * m diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 61a54170a..f94875d5b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,7 @@ #include <systemlib/err.h> #include <unistd.h> #include <drivers/drv_hrt.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -84,6 +85,7 @@ #include <uORB/topics/esc_status.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> #include <mavlink/mavlink_log.h> @@ -181,12 +183,17 @@ static void sdlog2_stop_log(void); /** * Write a header to log file: list of message formats. */ -static void write_formats(int fd); +static int write_formats(int fd); /** * Write version message to log file. */ -static void write_version(int fd); +static int write_version(int fd); + +/** + * Write parameters to log file. + */ +static int write_parameters(int fd); static bool file_exist(const char *filename); @@ -242,11 +249,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -359,13 +366,13 @@ static void *logwriter_thread(void *arg) struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_file = open_logfile(); + int log_fd = open_logfile(); - /* write log messages formats */ - write_formats(log_file); - - /* write version */ - write_version(log_file); + /* write log messages formats, version and parameters */ + log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; @@ -404,7 +411,7 @@ static void *logwriter_thread(void *arg) n = available; } - n = write(log_file, read_ptr, n); + n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; @@ -419,21 +426,23 @@ static void *logwriter_thread(void *arg) } else { n = 0; + /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { break; } + should_wait = true; } if (++poll_count == 10) { - fsync(log_file); + fsync(log_fd); poll_count = 0; } } - fsync(log_file); - close(log_file); + fsync(log_fd); + close(log_fd); return OK; } @@ -487,15 +496,17 @@ void sdlog2_stop_log() /* wait for write thread to return */ int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { warnx("error joining logwriter thread: %i", ret); } + logwriter_pthread = 0; sdlog2_status(); } -void write_formats(int fd) +int write_formats(int fd) { /* construct message format packet */ struct { @@ -505,35 +516,72 @@ void write_formats(int fd) LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; - /* fill message format packet for each format and write to log */ - int i; + int written = 0; - for (i = 0; i < log_formats_num; i++) { + /* fill message format packet for each format and write it */ + for (int i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; - log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format)); + written += write(fd, &log_msg_format, sizeof(log_msg_format)); } - fsync(fd); + return written; } -void write_version(int fd) +int write_version(int fd) { /* construct version message */ struct { LOG_PACKET_HEADER; struct log_VER_s body; } log_msg_VER = { - LOG_PACKET_HEADER_INIT(127), + LOG_PACKET_HEADER_INIT(LOG_VER_MSG), }; - /* fill message format packet for each format and write to log */ - int i; - + /* fill version message and write it */ strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); - log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER)); + return write(fd, &log_msg_VER, sizeof(log_msg_VER)); +} + +int write_parameters(int fd) +{ + /* construct parameter message */ + struct { + LOG_PACKET_HEADER; + struct log_PARM_s body; + } log_msg_PARM = { + LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), + }; + + int written = 0; + param_t params_cnt = param_count(); + + for (param_t param = 0; param < params_cnt; param++) { + /* fill parameter message and write it */ + strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); + float value = NAN; - fsync(fd); + switch (param_type(param)) { + case PARAM_TYPE_INT32: { + int32_t i; + param_get(param, &i); + value = i; // cast integer to float + break; + } + + case PARAM_TYPE_FLOAT: + param_get(param, &value); + break; + + default: + break; + } + + log_msg_PARM.body.value = value; + written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); + } + + return written; } int sdlog2_thread_main(int argc, char *argv[]) @@ -611,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } const char *converter_in = "/etc/logging/conv.zip"; - char* converter_out = malloc(120); + char *converter_out = malloc(120); sprintf(converter_out, "%s/conv.zip", folder_path); if (file_copy(converter_in, converter_out)) { errx(1, "unable to copy conversion scripts, exiting."); } + free(converter_out); /* only print logging path, important to find log file later */ @@ -630,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); /* warning! using union here to save memory, elements should be used separately! */ @@ -655,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; + memset(&buf, 0, sizeof(buf)); struct { @@ -825,7 +876,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); fds[fdsc_count].fd = subs.airspeed_sub; fds[fdsc_count].events = POLLIN; - fdsc_count++; + fdsc_count++; /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); @@ -913,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } - ifds = 1; // Begin from fds[1] again + ifds = 1; // begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); @@ -1172,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESCs --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - for (uint8_t i=0; i<buf.esc.esc_count; i++) - { + + for (uint8_t i = 0; i < buf.esc.esc_count; i++) { log_msg.msg_type = LOG_ESC_MSG; log_msg.body.log_ESC.counter = buf.esc.counter; log_msg.body.log_ESC.esc_count = buf.esc.esc_count; @@ -1321,6 +1372,7 @@ void handle_status(struct vehicle_status_s *status) { // TODO use flag from actuator_armed here? bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + if (armed != flag_system_armed) { flag_system_armed = armed; diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 5c175ef7e..dc5e6c8bd 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -85,10 +85,10 @@ struct log_format_s { #define LOG_FORMAT(_name, _format, _labels) { \ .type = LOG_##_name##_MSG, \ - .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ - .name = #_name, \ - .format = _format, \ - .labels = _labels \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ } #define LOG_FORMAT_MSG 0x80 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9f709e459..90093a407 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -48,12 +48,6 @@ /* define message formats */ #pragma pack(push, 1) -/* --- TIME - TIME STAMP --- */ -#define LOG_TIME_MSG 1 -struct log_TIME_s { - uint64_t t; -}; - /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { @@ -253,18 +247,31 @@ struct log_GVSP_s { float vz; }; +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 129 +struct log_TIME_s { + uint64_t t; +}; + /* --- VER - VERSION --- */ -#define LOG_VER_MSG 127 +#define LOG_VER_MSG 130 struct log_VER_s { char arch[16]; char fw_git[64]; }; +/* --- PARM - PARAMETER --- */ +#define LOG_PARM_MSG 131 +struct log_PARM_s { + char name[16]; + float value; +}; + #pragma pack(pop) /* construct list of all message formats */ static const struct log_format_s log_formats[] = { - LOG_FORMAT(TIME, "Q", "StartTime"), + /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), @@ -283,7 +290,11 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + /* system-level messages, ID >= 0x80 */ + // FMT: don't write format of format message, it's useless + LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), + LOG_FORMAT(PARM, "Nf", "Name,Value"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); |