diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-13 23:35:20 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-13 23:35:20 +0200 |
commit | a294ee2b870b8c8555e694015a37f4942082472a (patch) | |
tree | 5959ad780ac90fe4dbe27c4cfe5c5682c1eb7cfe | |
parent | e20c2541c6c8023d577d6599263a6c16cf0f6cfa (diff) | |
download | px4-firmware-a294ee2b870b8c8555e694015a37f4942082472a.tar.gz px4-firmware-a294ee2b870b8c8555e694015a37f4942082472a.tar.bz2 px4-firmware-a294ee2b870b8c8555e694015a37f4942082472a.zip |
Fixed mavlink timestamps, fixed SD logger, ready for flight tests
-rw-r--r-- | apps/mavlink/mavlink.c | 61 | ||||
-rw-r--r-- | apps/sdlog/Makefile | 2 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 79 |
3 files changed, 105 insertions, 37 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 004c9a5fe..78267d9eb 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -748,6 +748,13 @@ static void *uorb_receiveloop(void *arg) */ const int timeout = 1000; + /* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ + uint64_t last_sensor_timestamp = 0; + while (!thread_should_exit) { int poll_ret = poll(fds, fdsc_count, timeout); @@ -767,8 +774,10 @@ static void *uorb_receiveloop(void *arg) /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw); + last_sensor_timestamp = buf.raw.timestamp; + /* send raw imu data */ - mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], + mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); @@ -804,7 +813,7 @@ static void *uorb_receiveloop(void *arg) baro_counter = buf.raw.baro_counter; } - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2], @@ -826,7 +835,7 @@ static void *uorb_receiveloop(void *arg) orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); + mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); attitude_counter++; } @@ -933,17 +942,17 @@ static void *uorb_receiveloop(void *arg) /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ - buf.att_sp.pitch_body, - buf.att_sp.yaw_body, - buf.att_sp.thrust, - 0, - 0, - 0, - 0, - mavlink_mode, - 0); + hrt_absolute_time(), + buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ + buf.att_sp.pitch_body, + buf.att_sp.yaw_body, + buf.att_sp.thrust, + 0, + 0, + 0, + 0, + mavlink_mode, + 0); } } @@ -951,7 +960,7 @@ static void *uorb_receiveloop(void *arg) 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); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, 0 /* port 0 */, buf.act_outputs.output[0], buf.act_outputs.output[1], @@ -979,7 +988,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/, buf.act_outputs.output[0], buf.act_outputs.output[1], @@ -990,7 +999,7 @@ static void *uorb_receiveloop(void *arg) buf.act_outputs.output[6], buf.act_outputs.output[7]); if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, 3 /* port 3 */, buf.act_outputs.output[ 8], buf.act_outputs.output[ 9], @@ -1007,7 +1016,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */, buf.act_outputs.output[0], buf.act_outputs.output[1], @@ -1018,7 +1027,7 @@ static void *uorb_receiveloop(void *arg) buf.act_outputs.output[6], buf.act_outputs.output[7]); if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, 5 /* port 5 */, buf.act_outputs.output[ 8], buf.act_outputs.output[ 9], @@ -1035,7 +1044,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */, buf.act_outputs.output[0], buf.act_outputs.output[1], @@ -1046,7 +1055,7 @@ static void *uorb_receiveloop(void *arg) buf.act_outputs.output[6], buf.act_outputs.output[7]); if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, 7 /* port 7 */, buf.act_outputs.output[ 8], buf.act_outputs.output[ 9], @@ -1075,16 +1084,16 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators); /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0 ", buf.actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1 ", buf.actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2 ", buf.actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3 ", buf.actuators.control[3]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0 ", buf.actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); } /* --- DEBUG KEY/VALUE --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value); } } } diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile index 73490f6e5..61f224e99 100644 --- a/apps/sdlog/Makefile +++ b/apps/sdlog/Makefile @@ -36,7 +36,7 @@ # APPNAME = sdlog -PRIORITY = SCHED_PRIORITY_DEFAULT - 1 +PRIORITY = SCHED_PRIORITY_DEFAULT - 30 STACKSIZE = 2048 include $(APPDIR)/mk/app.mk diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index ab5f5c2a8..1ef838850 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -50,6 +50,8 @@ #include <stdlib.h> #include <string.h> #include <systemlib/err.h> +#include <unistd.h> +#include <arch/board/up_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -57,6 +59,7 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_command.h> static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -117,12 +120,15 @@ int sdlog_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 10, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } if (!strcmp(argv[1], "stop")) { + if (!thread_running) { + printf("\tsdlog is not started\n"); + } thread_should_exit = true; exit(0); } @@ -190,17 +196,31 @@ int sdlog_thread_main(int argc, char *argv[]) { /* create sensorfile */ int sensorfile; + unsigned sensor_combined_bytes = 0; + int actuator_outputs_file; + unsigned actuator_outputs_bytes = 0; FILE *gpsfile; + unsigned blackbox_file_bytes = 0; + FILE *blackbox_file; // FILE *vehiclefile; char path_buf[64] = ""; // string to hold the path to the sensorfile + printf("[sdlog] logging to directory %s\n", folder_path); + /* set up file path: e.g. /mnt/sdcard/session0001/sensors_combined.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sensors_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); + } + int actuator_outputs_file_no = actuator_outputs_file; + /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); if (NULL == (gpsfile = fopen(path_buf, "w"))) { @@ -208,6 +228,13 @@ int sdlog_thread_main(int argc, char *argv[]) { } int gpsfile_no = fileno(gpsfile); + /* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */ + sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox"); + if (NULL == (blackbox_file = fopen(path_buf, "w"))) { + errx(1, "opening %s failed.\n", path_buf); + } + int blackbox_file_no = fileno(blackbox_file); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ const ssize_t fdsc = 25; @@ -223,9 +250,11 @@ int sdlog_thread_main(int argc, char *argv[]) { struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s actuators; + struct vehicle_command_s cmd; } buf; struct { + int cmd_sub; int sensor_sub; int att_sub; int spa_sub; @@ -233,6 +262,13 @@ int sdlog_thread_main(int argc, char *argv[]) { int actuators_sub; } subs; + /* --- MANAGEMENT - LOGGING COMMAND --- */ + /* subscribe to ORB for sensors raw */ + subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + fds[fdsc_count].fd = subs.cmd_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)); @@ -243,7 +279,6 @@ int sdlog_thread_main(int argc, char *argv[]) { /* --- ATTITUDE VALUE --- */ /* subscribe to ORB for attitude */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(subs.att_sub, 100); fds[fdsc_count].fd = subs.att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -252,7 +287,6 @@ int sdlog_thread_main(int argc, char *argv[]) { /* subscribe to ORB for attitude setpoint */ /* struct already allocated */ subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(subs.spa_sub, 2000); /* 0.5 Hz updates */ fds[fdsc_count].fd = subs.spa_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -287,6 +321,10 @@ int sdlog_thread_main(int argc, char *argv[]) { thread_running = true; + int poll_count = 0; + + uint64_t starttime = hrt_absolute_time(); + while (!thread_should_exit) { int poll_ret = poll(fds, fdsc_count, timeout); @@ -300,13 +338,29 @@ int sdlog_thread_main(int argc, char *argv[]) { int ifds = 0; + if (poll_count % 2000 == 0) { + fsync(sensorfile); + fsync(actuator_outputs_file_no); + fsync(blackbox_file_no); + } + poll_count++; + + /* --- 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-%f-%f-%f-%f-%f-%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) { /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); /* write out */ - write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw)); + sensor_combined_bytes += write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw)); } /* --- ATTITUDE VALUE --- */ @@ -329,7 +383,8 @@ int sdlog_thread_main(int argc, char *argv[]) { 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.raw, sizeof(buf.raw)); } /* --- ACTUATOR CONTROL --- */ @@ -337,17 +392,21 @@ int sdlog_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators); } - - /* enforce write to disk */ - fsync(sensorfile); - fsync(gpsfile_no); } } - warn("exiting."); + unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes; + float mebibytes = bytes / 1024.0f / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + + printf("[sdlog] wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); + + printf("[sdlog] exiting.\n"); close(sensorfile); + close(gpsfile); fclose(gpsfile); + fclose(blackbox_file); thread_running = false; |