diff options
Diffstat (limited to 'apps/sdlog/sdlog.c')
-rw-r--r-- | apps/sdlog/sdlog.c | 165 |
1 files changed, 113 insertions, 52 deletions
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 9b4cd1622..407c47811 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -69,6 +69,8 @@ #include <systemlib/systemlib.h> +#include "sdlog_ringbuffer.h" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -76,6 +78,18 @@ 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; +struct sdlog_logbuffer lb; + +/** + * System state vector log buffer writing + */ +static void *sdlog_sysvector_write_thread(void *arg); + +/** + * Create the thread to write the system vector + */ +pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf); /** * SD log management function. @@ -94,7 +108,7 @@ static void usage(const char *reason); static int file_exist(const char *filename); -static int file_copy(const char* file_old, const char* file_new); +static int file_copy(const char *file_old, const char *file_new); /** * Print the current status. @@ -104,13 +118,14 @@ static void print_sdlog_status(void); /** * Create folder for current logging session. */ -static int create_logfolder(char* folder_path); +static int create_logfolder(char *folder_path); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n"); } @@ -126,7 +141,7 @@ uint64_t starttime = 0; * The sd log deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_spawn(). */ @@ -157,6 +172,7 @@ int sdlog_main(int argc, char *argv[]) if (!thread_running) { printf("\tsdlog is not started\n"); } + thread_should_exit = true; exit(0); } @@ -164,9 +180,11 @@ int sdlog_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { print_sdlog_status(); + } else { printf("\tsdlog not started\n"); } + exit(0); } @@ -174,7 +192,8 @@ int sdlog_main(int argc, char *argv[]) exit(1); } -int create_logfolder(char* folder_path) { +int create_logfolder(char *folder_path) +{ /* make folder on sdcard */ uint16_t foldernumber = 1; // start with folder 0001 int mkdir_ret; @@ -193,11 +212,14 @@ int create_logfolder(char* folder_path) { char mfile_out[100]; sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); int ret = file_copy(mfile_in, mfile_out); + if (!ret) { warnx("copied m file to %s", mfile_out); + } else { warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); } + break; } else if (mkdir_ret == -1) { @@ -216,11 +238,60 @@ int create_logfolder(char* folder_path) { warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); return -1; } + return 0; } -int sdlog_thread_main(int argc, char *argv[]) { +static void * +sdlog_sysvector_write_thread(void *arg) +{ + struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg; + + int poll_count = 0; + struct sdlog_sysvector sysvect; + memset(&sysvect, 0, sizeof(sysvect)); + + while (!thread_should_exit) { + //pthread_mutex.. + if (sdlog_logbuffer_read(logbuf, &sysvect) == OK) { + sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect)); + } + + if (poll_count % 100 == 0) { + fsync(sysvector_file); + } + + poll_count++; + usleep(3000); + } + + fsync(sysvector_file); + + return OK; +} + +pthread_t +sysvector_write_start(struct sdlog_logbuffer *logbuf) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + struct sched_param param; + /* low priority, as this is expensive disk I/O */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf); + return thread; +} + + +int sdlog_thread_main(int argc, char *argv[]) +{ warnx("starting\n"); @@ -229,6 +300,7 @@ int sdlog_thread_main(int argc, char *argv[]) { } char folder_path[64]; + if (create_logfolder(folder_path)) errx(1, "unable to create logging folder, exiting."); @@ -236,7 +308,6 @@ int sdlog_thread_main(int argc, char *argv[]) { int sensorfile = -1; int actuator_outputs_file = -1; int actuator_controls_file = -1; - int sysvector_file = -1; FILE *gpsfile; FILE *blackbox_file; // FILE *vehiclefile; @@ -247,6 +318,7 @@ int sdlog_thread_main(int argc, char *argv[]) { /* 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); } @@ -259,28 +331,34 @@ 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); } /* 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"))) { errx(1, "opening %s failed.\n", path_buf); } + 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 --- */ @@ -426,7 +504,10 @@ int sdlog_thread_main(int argc, char *argv[]) { thread_running = true; - int poll_count = 0; + /* Initialize log buffer with a size of 5 */ + sdlog_logbuffer_init(&lb, 5); + /* start logbuffer emptying thread */ + pthread_t sysvector_pthread = sysvector_write_start(&lb); starttime = hrt_absolute_time(); @@ -450,7 +531,7 @@ int sdlog_thread_main(int argc, char *argv[]) { // fsync(blackbox_file_no); // } - + // /* --- VEHICLE COMMAND VALUE --- */ // if (fds[ifds++].revents & POLLIN) { @@ -476,14 +557,14 @@ int sdlog_thread_main(int argc, char *argv[]) { // /* 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 --- */ @@ -502,12 +583,6 @@ int sdlog_thread_main(int argc, char *argv[]) { // } // } - 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); @@ -523,27 +598,7 @@ int sdlog_thread_main(int argc, char *argv[]) { 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); - #pragma pack(push, 1) - struct { - uint64_t timestamp; //[us] - float gyro[3]; //[rad/s] - float accel[3]; //[m/s^2] - float mag[3]; //[gauss] - float baro; //pressure [millibar] - float baro_alt; //altitude above MSL [meter] - float baro_temp; //[degree celcius] - float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) - float vbat; //battery voltage in [volt] - float adc[3]; //remaining auxiliary ADC ports [volt] - float local_position[3]; //tangent plane mapping into x,y,z [m] - int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] - float attitude[3]; //pitch, roll, yaw [rad] - float rotMatrix[9]; //unitvectors - float vicon[6]; - float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality - } sysvector = { + 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]}, .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, @@ -552,8 +607,10 @@ int sdlog_thread_main(int argc, char *argv[]) { .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]}, + .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 = 0.0f, /* XXX use battery_status uORB topic */ .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, @@ -563,18 +620,21 @@ int sdlog_thread_main(int argc, char *argv[]) { .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality} + /* XXX add calculated airspeed */ }; - #pragma pack(pop) - sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); + /* put into buffer for later IO */ + sdlog_logbuffer_write(&lb, &sysvect); + // XXX notify writing thread through pthread mutex usleep(3500); // roughly 150 Hz } - fsync(sysvector_file); - print_sdlog_status(); + /* wait for write thread to return */ + (void)pthread_join(sysvector_pthread, NULL); + warnx("exiting.\n"); close(sensorfile); @@ -606,43 +666,44 @@ int file_exist(const char *filename) return stat(filename, &buffer); } -int file_copy(const char* file_old, const char* file_new) +int file_copy(const char *file_old, const char *file_new) { FILE *source, *target; source = fopen(file_old, "r"); int ret = 0; - - if( source == NULL ) - { + + if (source == NULL) { warnx("failed opening input file to copy"); return 1; } target = fopen(file_new, "w"); - - if( target == NULL ) - { + + if (target == NULL) { fclose(source); warnx("failed to open output file to copy"); return 1; } - + char buf[128]; int nread; + while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { int ret = fwrite(buf, 1, nread, target); + if (ret <= 0) { warnx("error writing file"); ret = 1; break; } } + fsync(fileno(target)); fclose(source); fclose(target); - return ret; + return ret; } |