forked from Archive/PX4-Autopilot
Improved reporting / logging a lot, first usable version of SD card logger
This commit is contained in:
parent
fb397c8dc4
commit
e5950ad498
|
@ -331,6 +331,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
|
|||
static uint64_t last_motor_time = 0;
|
||||
|
||||
static struct actuator_outputs_s outputs;
|
||||
outputs.timestamp = hrt_absolute_time();
|
||||
outputs.output[0] = motor1;
|
||||
outputs.output[1] = motor2;
|
||||
outputs.output[2] = motor3;
|
||||
|
|
|
@ -777,10 +777,10 @@ static void *uorb_receiveloop(void *arg)
|
|||
last_sensor_timestamp = buf.raw.timestamp;
|
||||
|
||||
/* send raw imu data */
|
||||
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]);
|
||||
// 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]);
|
||||
|
||||
/* mark individual fields as changed */
|
||||
uint16_t fields_updated = 0;
|
||||
|
@ -835,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, last_sensor_timestamp, 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 / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
|
||||
|
||||
attitude_counter++;
|
||||
}
|
||||
|
@ -1073,7 +1073,7 @@ static void *uorb_receiveloop(void *arg)
|
|||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
|
||||
mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
|
||||
buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1);
|
||||
buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0);
|
||||
}
|
||||
|
||||
/* --- ACTUATOR ARMED --- */
|
||||
|
@ -1320,23 +1320,23 @@ void handleMessage(mavlink_message_t *msg)
|
|||
memset(&rc_hil, 0, sizeof(rc_hil));
|
||||
static orb_advert_t rc_pub = 0;
|
||||
|
||||
rc_hil.chan[0].raw = 1510 + man.roll * 500;
|
||||
rc_hil.chan[1].raw = 1520 + man.pitch * 500;
|
||||
rc_hil.chan[2].raw = 1590 + man.yaw * 500;
|
||||
rc_hil.chan[3].raw = 1420 + man.thrust * 500;
|
||||
rc_hil.chan[0].raw = 1510 + man.x * 500;
|
||||
rc_hil.chan[1].raw = 1520 + man.y * 500;
|
||||
rc_hil.chan[2].raw = 1590 + man.r * 500;
|
||||
rc_hil.chan[3].raw = 1420 + man.z * 500;
|
||||
|
||||
rc_hil.chan[0].scaled = man.roll;
|
||||
rc_hil.chan[1].scaled = man.pitch;
|
||||
rc_hil.chan[2].scaled = man.yaw;
|
||||
rc_hil.chan[3].scaled = man.thrust;
|
||||
rc_hil.chan[0].scaled = man.x;
|
||||
rc_hil.chan[1].scaled = man.y;
|
||||
rc_hil.chan[2].scaled = man.r;
|
||||
rc_hil.chan[3].scaled = man.z;
|
||||
|
||||
struct manual_control_setpoint_s mc;
|
||||
static orb_advert_t mc_pub = 0;
|
||||
|
||||
mc.roll = man.roll;
|
||||
mc.pitch = man.roll;
|
||||
mc.yaw = man.roll;
|
||||
mc.roll = man.roll;
|
||||
mc.roll = man.x;
|
||||
mc.pitch = man.y;
|
||||
mc.yaw = man.r;
|
||||
mc.roll = man.z;
|
||||
|
||||
/* fake RC channels with manual control input from simulator */
|
||||
|
||||
|
@ -1578,15 +1578,15 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
|
||||
} else if (baudrate >= 230400) {
|
||||
/* 200 Hz / 5 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
|
||||
} else if (baudrate >= 115200) {
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
|
|
|
@ -195,10 +195,12 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
errx(1, "unable to create logging folder, exiting");
|
||||
|
||||
/* create sensorfile */
|
||||
int sensorfile;
|
||||
int sensorfile = -1;
|
||||
unsigned sensor_combined_bytes = 0;
|
||||
int actuator_outputs_file;
|
||||
int actuator_outputs_file = -1;
|
||||
unsigned actuator_outputs_bytes = 0;
|
||||
int actuator_controls_file = -1;
|
||||
unsigned actuator_controls_bytes = 0;
|
||||
FILE *gpsfile;
|
||||
unsigned blackbox_file_bytes = 0;
|
||||
FILE *blackbox_file;
|
||||
|
@ -208,18 +210,23 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
|
||||
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");
|
||||
/* 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))) {
|
||||
// /* 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, "actuator_controls0");
|
||||
if (0 == (actuator_controls_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");
|
||||
|
@ -249,7 +256,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
struct vehicle_attitude_s att;
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
struct actuator_outputs_s act_outputs;
|
||||
struct actuator_controls_s actuators;
|
||||
struct actuator_controls_s act_controls;
|
||||
struct vehicle_command_s cmd;
|
||||
} buf;
|
||||
|
||||
|
@ -259,7 +266,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
int att_sub;
|
||||
int spa_sub;
|
||||
int act_0_sub;
|
||||
int actuators_sub;
|
||||
int controls0_sub;
|
||||
} subs;
|
||||
|
||||
/* --- MANAGEMENT - LOGGING COMMAND --- */
|
||||
|
@ -299,8 +306,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
|
||||
/* --- ACTUATOR CONTROL VALUE --- */
|
||||
/* subscribe to ORB for actuator control */
|
||||
subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
fds[fdsc_count].fd = subs.actuators_sub;
|
||||
subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
fds[fdsc_count].fd = subs.controls0_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
|
@ -338,9 +345,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
|
||||
int ifds = 0;
|
||||
|
||||
if (poll_count % 2000 == 0) {
|
||||
if (poll_count % 5000 == 0) {
|
||||
fsync(sensorfile);
|
||||
fsync(actuator_outputs_file_no);
|
||||
fsync(actuator_outputs_file);
|
||||
fsync(actuator_controls_file);
|
||||
fsync(blackbox_file_no);
|
||||
}
|
||||
poll_count++;
|
||||
|
@ -349,7 +357,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
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,
|
||||
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);
|
||||
}
|
||||
|
@ -360,7 +368,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
/* 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));
|
||||
sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
|
||||
}
|
||||
|
||||
/* --- ATTITUDE VALUE --- */
|
||||
|
@ -384,18 +392,19 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
/* 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_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.actuators_sub, &buf.actuators);
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes;
|
||||
unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
|
||||
float mebibytes = bytes / 1024.0f / 1024.0f;
|
||||
float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
|
||||
|
||||
|
@ -404,7 +413,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
|
|||
printf("[sdlog] exiting.\n");
|
||||
|
||||
close(sensorfile);
|
||||
close(gpsfile);
|
||||
close(actuator_outputs_file);
|
||||
close(actuator_controls_file);
|
||||
fclose(gpsfile);
|
||||
fclose(blackbox_file);
|
||||
|
||||
|
|
|
@ -53,6 +53,7 @@
|
|||
#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
|
||||
|
||||
struct actuator_controls_s {
|
||||
uint64_t timestamp;
|
||||
float control[NUM_ACTUATOR_CONTROLS];
|
||||
};
|
||||
|
||||
|
|
|
@ -53,6 +53,7 @@
|
|||
#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */
|
||||
|
||||
struct actuator_outputs_s {
|
||||
uint64_t timestamp;
|
||||
float output[NUM_ACTUATOR_OUTPUTS];
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue