forked from Archive/PX4-Autopilot
Enable / disable logging while running, enabled black box logging (ringbuffer needed), enabled GPS KML logging (does not yet write outputs)
This commit is contained in:
Lorenz Meier
parent
3128529c3b
commit
d463c94ea1
|
|
@ -73,6 +73,8 @@
|
|||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "sdlog_ringbuffer.h"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
|
|
@ -83,6 +85,7 @@ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
|
|||
static const char *mountpoint = "/fs/microsd";
|
||||
static const char *mfile_in = "/etc/logging/logconv.m";
|
||||
int sysvector_file = -1;
|
||||
int mavlink_fd = -1;
|
||||
struct sdlog_logbuffer lb;
|
||||
|
||||
/* mutex / condition to synchronize threads */
|
||||
|
|
@ -118,6 +121,8 @@ 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.
|
||||
*/
|
||||
|
|
@ -134,7 +139,7 @@ usage(const char *reason)
|
|||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
|
||||
errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n");
|
||||
}
|
||||
|
||||
// XXX turn this into a C++ class
|
||||
|
|
@ -145,6 +150,9 @@ unsigned sysvector_bytes = 0;
|
|||
unsigned blackbox_file_bytes = 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
|
||||
|
|
@ -318,25 +326,44 @@ sysvector_write_start(struct sdlog_logbuffer *logbuf)
|
|||
|
||||
int sdlog_thread_main(int argc, char *argv[])
|
||||
{
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
/* log every 2nd value (skip one) */
|
||||
if (mavlink_fd < 0) {
|
||||
warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
|
||||
}
|
||||
|
||||
/* log every n'th value (skip one per default) */
|
||||
int skip_value = 1;
|
||||
|
||||
if (argc > 1) {
|
||||
if (!strcmp(argv[1], "-s") && argc > 2) {
|
||||
int s = atoi(argv[2]);
|
||||
/* work around some stupidity in task_create's argv handling */
|
||||
argc -= 2;
|
||||
argv += 2;
|
||||
int ch;
|
||||
|
||||
if (s > 0 && s < 250) {
|
||||
skip_value = s;
|
||||
while ((ch = getopt(argc, argv, "sr")) != 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 {
|
||||
warnx("Ignoring skip value of %d, out of range (1..250)\n", s);
|
||||
}
|
||||
skip_value = s;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
warnx("starting\n");
|
||||
case 'r':
|
||||
/* log only on request, disable logging per default */
|
||||
logging_enabled = false;
|
||||
break;
|
||||
|
||||
warnx("skipping %d sensor packets between logged packets.\n", skip_value);
|
||||
default:
|
||||
usage("unrecognized flag");
|
||||
}
|
||||
}
|
||||
|
||||
if (file_exist(mountpoint) != OK) {
|
||||
errx(1, "logging mount point %s not present, exiting.", mountpoint);
|
||||
|
|
@ -347,31 +374,15 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
if (create_logfolder(folder_path))
|
||||
errx(1, "unable to create logging folder, exiting.");
|
||||
|
||||
/* create sensorfile */
|
||||
int sensorfile = -1;
|
||||
int actuator_outputs_file = -1;
|
||||
int actuator_controls_file = -1;
|
||||
FILE *gpsfile;
|
||||
FILE *blackbox_file;
|
||||
// FILE *vehiclefile;
|
||||
|
||||
char path_buf[64] = ""; // string to hold the path to the sensorfile
|
||||
/* 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/sensor_combined.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined");
|
||||
|
||||
if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
// /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */
|
||||
// sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0");
|
||||
// if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
// errx(1, "opening %s failed.\n", path_buf);
|
||||
// }
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
|
||||
|
||||
|
|
@ -379,13 +390,6 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0");
|
||||
|
||||
if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
|
||||
sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
|
||||
|
||||
|
|
@ -402,6 +406,7 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
// XXX for fsync() calls
|
||||
int blackbox_file_no = fileno(blackbox_file);
|
||||
|
||||
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
|
||||
|
|
@ -455,12 +460,18 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
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;
|
||||
/* rate-limit raw data updates to 200Hz */
|
||||
orb_set_interval(subs.sensor_sub, 5);
|
||||
/* do not rate limit, instead use skip counter (aliasing on rate limit) */
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
|
|
@ -513,13 +524,6 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
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++;
|
||||
|
||||
/* --- VICON POSITION --- */
|
||||
/* subscribe to ORB for vicon position */
|
||||
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
|
||||
|
|
@ -577,18 +581,13 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
|
||||
starttime = hrt_absolute_time();
|
||||
|
||||
// XXX clock the log for now with the gyro output rate / 2
|
||||
struct pollfd gyro_fd;
|
||||
gyro_fd.fd = subs.sensor_sub;
|
||||
gyro_fd.events = POLLIN;
|
||||
|
||||
/* track skipping */
|
||||
int skip_count = 0;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
// XXX only use gyro for now
|
||||
int poll_ret = poll(&gyro_fd, 1, 1000);
|
||||
int poll_ret = poll(fds, 2, 1000);
|
||||
|
||||
// int poll_ret = poll(fds, fdsc_count, timeout);
|
||||
|
||||
|
|
@ -599,95 +598,45 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
/* XXX this is seriously bad - should be an emergency */
|
||||
} else {
|
||||
|
||||
/* always copy sensors raw data into local buffer, since poll flags won't clear else */
|
||||
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
|
||||
/* copy actuator data into local buffer */
|
||||
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
|
||||
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
|
||||
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
|
||||
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
|
||||
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
|
||||
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
|
||||
int ifds;
|
||||
|
||||
if (skip_count < skip_value) {
|
||||
skip_count++;
|
||||
/* do not log data */
|
||||
continue;
|
||||
} else {
|
||||
/* log data, reset */
|
||||
skip_count = 0;
|
||||
/* --- VEHICLE COMMAND VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
/* copy command into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
|
||||
|
||||
/* always log to blackbox, even when logging disabled */
|
||||
blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
|
||||
buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
|
||||
(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
|
||||
|
||||
handle_command(&buf.cmd);
|
||||
}
|
||||
|
||||
// int ifds = 0;
|
||||
/* --- VEHICLE GPS VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
/* copy gps position into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
|
||||
|
||||
// if (poll_count % 5000 == 0) {
|
||||
// fsync(sensorfile);
|
||||
// fsync(actuator_outputs_file);
|
||||
// fsync(actuator_controls_file);
|
||||
// fsync(blackbox_file_no);
|
||||
// }
|
||||
/* if logging disabled, continue */
|
||||
if (logging_enabled) {
|
||||
|
||||
/* write KML line */
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// /* --- VEHICLE COMMAND VALUE --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
// /* copy command into local buffer */
|
||||
// orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
|
||||
// blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
|
||||
// buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
|
||||
// (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
|
||||
// }
|
||||
|
||||
// /* --- SENSORS RAW VALUE --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
|
||||
// /* copy sensors raw data into local buffer */
|
||||
// orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
// /* write out */
|
||||
// sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
|
||||
// }
|
||||
|
||||
// /* --- ATTITUDE VALUE --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
|
||||
// /* copy attitude data into local buffer */
|
||||
// orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
|
||||
|
||||
|
||||
// }
|
||||
|
||||
// /* --- VEHICLE ATTITUDE SETPOINT --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
// /* copy local position data into local buffer */
|
||||
// orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
|
||||
|
||||
// }
|
||||
|
||||
// /* --- ACTUATOR OUTPUTS 0 --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
// /* copy actuator data into local buffer */
|
||||
// orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
|
||||
// /* write out */
|
||||
// // actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs));
|
||||
// }
|
||||
|
||||
// /* --- ACTUATOR CONTROL --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls);
|
||||
// /* write out */
|
||||
// actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls));
|
||||
// }
|
||||
|
||||
|
||||
/* copy sensors raw data into local buffer */
|
||||
/* always copy sensors raw data into local buffer, since poll flags won't clear else */
|
||||
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
|
||||
/* copy actuator data into local buffer */
|
||||
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
|
||||
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
|
||||
|
|
@ -699,6 +648,16 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
|
||||
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
|
||||
|
||||
/* if skipping is on or logging is disabled, ignore */
|
||||
if (skip_count < skip_value || !logging_enabled) {
|
||||
skip_count++;
|
||||
/* do not log data */
|
||||
continue;
|
||||
} else {
|
||||
/* log data, reset */
|
||||
skip_count = 0;
|
||||
}
|
||||
|
||||
struct sdlog_sysvector sysvect = {
|
||||
.timestamp = buf.raw.timestamp,
|
||||
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
|
||||
|
|
@ -732,7 +691,7 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
pthread_mutex_lock(&sysvector_mutex);
|
||||
sdlog_logbuffer_write(&lb, &sysvect);
|
||||
/* signal the other thread new data, but not yet unlock */
|
||||
if (lb.count > lb.size / 3) {
|
||||
if ((unsigned)lb.count > lb.size / 3) {
|
||||
/* only request write if several packets can be written at once */
|
||||
pthread_cond_signal(&sysvector_cond);
|
||||
}
|
||||
|
|
@ -742,6 +701,8 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
print_sdlog_status();
|
||||
|
||||
/* wait for write thread to return */
|
||||
|
|
@ -752,9 +713,8 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
|
||||
warnx("exiting.\n");
|
||||
|
||||
close(sensorfile);
|
||||
close(actuator_outputs_file);
|
||||
close(actuator_controls_file);
|
||||
/* finish KML file */
|
||||
// XXX
|
||||
fclose(gpsfile);
|
||||
fclose(blackbox_file);
|
||||
|
||||
|
|
@ -821,4 +781,34 @@ int file_copy(const char *file_old, const char *file_new)
|
|||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue