fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 13:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2: parameters logging implemented (APM-compatible)

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-23 18:57:06 +02:00
parent 49ff143551
commit 3c6f438691
3 changed files with 110 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+87 -35
View File
@@ -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));
}
fsync(fd);
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;
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;
src/modules/sdlog2/sdlog2_format.h
+4 -4
View File
@@ -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
src/modules/sdlog2/sdlog2_messages.h
+19 -8
View File
@@ -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);