diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-17 11:51:33 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-17 11:51:33 +0200 |
commit | 3816327977166cbb68ba94aae4b316651cd70ba3 (patch) | |
tree | b3977de00f62ce889fc3a8f06cf8a7c2ad71f6bc /apps/sdlog | |
parent | 7a375ad670b7d0df5edf34fbe02d555c451da8d9 (diff) | |
download | px4-firmware-3816327977166cbb68ba94aae4b316651cd70ba3.tar.gz px4-firmware-3816327977166cbb68ba94aae4b316651cd70ba3.tar.bz2 px4-firmware-3816327977166cbb68ba94aae4b316651cd70ba3.zip |
SD log WIP, currently logs everything to one packet
Diffstat (limited to 'apps/sdlog')
-rw-r--r-- | apps/sdlog/sdlog.c | 170 |
1 files changed, 114 insertions, 56 deletions
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index eaf609090..b99c6652c 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -201,6 +201,8 @@ int sdlog_thread_main(int argc, char *argv[]) { unsigned actuator_outputs_bytes = 0; int actuator_controls_file = -1; unsigned actuator_controls_bytes = 0; + int sysvector_file = -1; + unsigned sysvector_bytes = 0; FILE *gpsfile; unsigned blackbox_file_bytes = 0; FILE *blackbox_file; @@ -223,6 +225,12 @@ int sdlog_thread_main(int argc, char *argv[]) { // } /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ + sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); + if (0 == (sysvector_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); @@ -251,7 +259,7 @@ int sdlog_thread_main(int argc, char *argv[]) { struct pollfd fds[fdsc]; - union { + struct { struct sensor_combined_s raw; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -259,6 +267,7 @@ int sdlog_thread_main(int argc, char *argv[]) { struct actuator_controls_s act_controls; struct vehicle_command_s cmd; } buf; + memset(&buf, 0, sizeof(buf)); struct { int cmd_sub; @@ -334,74 +343,123 @@ int sdlog_thread_main(int argc, char *argv[]) { while (!thread_should_exit) { - int poll_ret = poll(fds, fdsc_count, timeout); + // int poll_ret = poll(fds, fdsc_count, timeout); - /* handle the poll result */ - if (poll_ret == 0) { - /* XXX this means none of our providers is giving us data - might be an error? */ - } else if (poll_ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else { + // /* handle the poll result */ + // if (poll_ret == 0) { + // /* XXX this means none of our providers is giving us data - might be an error? */ + // } else if (poll_ret < 0) { + // /* XXX this is seriously bad - should be an emergency */ + // } else { - int ifds = 0; + // int ifds = 0; - if (poll_count % 5000 == 0) { - fsync(sensorfile); - fsync(actuator_outputs_file); - fsync(actuator_controls_file); - fsync(blackbox_file_no); - } - poll_count++; + // if (poll_count % 5000 == 0) { + // fsync(sensorfile); + // fsync(actuator_outputs_file); + // fsync(actuator_controls_file); + // fsync(blackbox_file_no); + // } - /* --- 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) { + // /* --- 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); + // } - /* 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)); - } + // /* --- SENSORS RAW VALUE --- */ + // if (fds[ifds++].revents & POLLIN) { - /* --- ATTITUDE 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)); + // } - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + // /* --- 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); + // /* --- 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)); - } + // } + + // /* --- 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)); + // } + // } + + if (poll_count % 100 == 0) { + fsync(sysvector_file); } + + poll_count++; + + + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); + /* 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); + + #pragma pack(push, 1) + struct { + uint64_t timestamp; + float gyro[3]; + float accel[3]; + float mag[3]; + float baro; + float baro_alt; + float baro_temp; + float control[4]; + + float actuators[8]; + float vbat; + float adc[3]; + } sysvector = { + .timestamp = buf.raw.timestamp, + .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, + .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, + .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, + .baro = buf.raw.baro_pres_mbar, + .baro_alt = buf.raw.baro_alt_meter, + .baro_temp = buf.raw.baro_temp_celcius, + .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, + .actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], + buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, + .vbat = buf.raw.battery_voltage_v, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]} + }; + #pragma pack(pop) + + sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); + + usleep(4900); } unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; |