diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-10 23:04:31 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-10 23:04:31 +0200 |
commit | e440fc40275fd61a76d40aae73ed9e1cb3e813b2 (patch) | |
tree | 200ff7c9b500e86dd973e0febad629590d2765e3 /apps/sdlog | |
parent | 0019f65b10dfdaa962283c916798df7b209ba9fc (diff) | |
download | px4-firmware-e440fc40275fd61a76d40aae73ed9e1cb3e813b2.tar.gz px4-firmware-e440fc40275fd61a76d40aae73ed9e1cb3e813b2.tar.bz2 px4-firmware-e440fc40275fd61a76d40aae73ed9e1cb3e813b2.zip |
Rewrote SD logging app, simpler, but effective. Pending testing
Diffstat (limited to 'apps/sdlog')
-rw-r--r-- | apps/sdlog/Makefile | 4 | ||||
-rw-r--r-- | apps/sdlog/mfile.template | 136 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 563 | ||||
-rw-r--r-- | apps/sdlog/sdlog.h | 17 | ||||
-rw-r--r-- | apps/sdlog/sdlog_generated.h | 238 | ||||
-rw-r--r-- | apps/sdlog/updatesdlog.py | 199 |
6 files changed, 243 insertions, 914 deletions
diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile index 1e5abdb55..73490f6e5 100644 --- a/apps/sdlog/Makefile +++ b/apps/sdlog/Makefile @@ -37,8 +37,6 @@ APPNAME = sdlog PRIORITY = SCHED_PRIORITY_DEFAULT - 1 -STACKSIZE = 3000 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink +STACKSIZE = 2048 include $(APPDIR)/mk/app.mk diff --git a/apps/sdlog/mfile.template b/apps/sdlog/mfile.template deleted file mode 100644 index 0f7ebd045..000000000 --- a/apps/sdlog/mfile.template +++ /dev/null @@ -1,136 +0,0 @@ -%% Import logfiles - -if (exist ('OCTAVE_VERSION', 'builtin')) - % Octave - graphics_toolkit ("fltk") -else - % Matlab -end - -% define log file and GPSs -logfileFolder = 'logfiles'; -fileName = 'all'; -fileEnding = 'px4log'; - -numberOfLogTypes = length(logTypes); - -path = [fileName,'.', fileEnding]; -fid = fopen(path, 'r'); - -% get file length -fseek(fid, 0,'eof'); -fileLength = ftell(fid); -fseek(fid, 0,'bof'); - -% get length of one block -blockLength = 4; % check: $$$$ -for i=1:numberOfLogTypes - blockLength = blockLength + logTypes{i}.type_bytes*logTypes{i}.number_of_array; -end - -% determine number of entries -entries = fileLength / blockLength; - - -% import data -offset = 0; -for i=1:numberOfLogTypes - - data.(genvarname(logTypes{i}.data)).(genvarname(logTypes{i}.variable_name)) = transpose(fread(fid,[logTypes{i}.number_of_array,entries],[num2str(logTypes{i}.number_of_array),'*',logTypes{i}.type_name,'=>',logTypes{i}.type_name],blockLength-logTypes{i}.type_bytes*logTypes{i}.number_of_array)); - offset = offset + logTypes{i}.type_bytes*logTypes{i}.number_of_array; - fseek(fid, offset,'bof'); - - progressPercentage = i/numberOfLogTypes*100; - fprintf('%3.0f%%',progressPercentage); -end - - -%% Plots - -figure -plot(data.sensors_raw.timestamp,data.sensors_raw.gyro_raw) - -figure -plot(data.sensors_raw.timestamp,data.sensors_raw.accelerometer_raw) - -%% Check for lost data - -% to detect lost frames (either when logging to sd card or if no new data is -% data is available for some time) -diff_counter = diff(data.sensors_raw.gyro_raw_counter); -figure -plot(diff_counter) - -% to detect how accurate the timing was -diff_timestamp = diff(data.sensors_raw.timestamp); - -figure -plot(diff_timestamp) - -%% Export to file for google earth - - -if(isfield(data.gps,'lat') && isfield(data.gps,'lon') && isfield(data.gps,'alt')) - - % extract coordinates and height where they are not zero - maskWhereNotZero = ((data.gps.lon ~= 0 & data.gps.lat ~= 0 ) & data.gps.alt ~= 0); - - % plot - figure - plot3(data.gps.lon(maskWhereNotZero),data.gps.lat(maskWhereNotZero),data.gps.alt(maskWhereNotZero)) - - - % create a kml file according to https://developers.google.com/kml/documentation/kml_tut - % also see https://support.google.com/earth/bin/answer.py?hl=en&answer=148072&topic=2376756&ctx=topic - - % open file and overwrite content - fileId = fopen('gps_path.kml','w+'); - - % define strings that should be written to file - fileStartDocumentString = ['<?xml version="1.0" encoding="UTF-8"?><kml xmlns="http://www.opengis.net/kml/2.2"><Document><name>Paths</name><description>Path</description>']; - - fileStyleString = '<Style id="blueLinebluePoly"><LineStyle><color>7fff0000</color><width>4</width></LineStyle><PolyStyle><color>7fff0000</color></PolyStyle></Style>'; - - filePlacemarkString = '<Placemark><name>Absolute Extruded</name><description>Transparent blue wall with blue outlines</description><styleUrl>#blueLinebluePoly</styleUrl><LineString><extrude>1</extrude><tessellate>1</tessellate><altitudeMode>absolute</altitudeMode><coordinates>'; - - fileEndPlacemarkString = '</coordinates></LineString></Placemark>'; - fileEndDocumentString = '</Document></kml>'; - - % start writing to file - fprintf(fileId,fileStartDocumentString); - - fprintf(fileId,fileStyleString); - fprintf(fileId,filePlacemarkString); - - - lonTemp = double(data.gps.lon(maskWhereNotZero))/1E7; - latTemp = double(data.gps.lat(maskWhereNotZero))/1E7; - altTemp = double(data.gps.alt(maskWhereNotZero))/1E3 + 100.0; % in order to see the lines above ground - - % write coordinates to file - for k=1:length(data.gps.lat(maskWhereNotZero)) - if(mod(k,10)==0) - fprintf(fileId,'%.10f,%.10f,%.10f\\n',lonTemp(k),latTemp(k),altTemp(k)); - end - end - - % write end placemark - fprintf(fileId,fileEndPlacemarkString); - - % write end of file - fprintf(fileId,fileEndDocumentString); - - % close file, it should now be readable in Google Earth using File -> Open - fclose(fileId); - -end - -if(isfield(data.position,'lat') && isfield(data.position,'lon') && isfield(data.position,'alt')) - - % extract coordinates and height where they are not zero - maskWhereNotZero = ((data.position.lon ~= 0 & data.position.lat ~= 0 ) & data.position.alt ~= 0); - - % plot - figure - plot3(data.position.lon(maskWhereNotZero),data.position.lat(maskWhereNotZero),data.position.alt(maskWhereNotZero)) -end diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 1b87aab77..3099a21a1 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -1,8 +1,7 @@ /**************************************************************************** - * examples/hello/main.c * - * Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User <mail@example.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -14,7 +13,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -33,152 +32,121 @@ * ****************************************************************************/ +/** + * @file sdlog.c + * Simple SD logger for flight data + */ -#include <time.h> #include <nuttx/config.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> +#include <errno.h> #include <unistd.h> #include <stdio.h> -#include <stdint.h> +#include <poll.h> #include <stdlib.h> #include <string.h> -#include <float.h> -#include <stdbool.h> -#include <sys/mount.h> -#include <sys/stat.h> -#include <time.h> -#include <pthread.h> -#include <fcntl.h> -#include <sys/prctl.h> -#include <errno.h> +#include <systemlib/err.h> -#include "sdlog.h" -#include "sdlog_generated.h" +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls.h> -/**************************************************************************** - * Definitions - ****************************************************************************/ - -__EXPORT int sdlog_main(int argc, char *argv[]); +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 */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ -const char *src = "/dev/mmcsd0"; -const char *trgt = "/fs/microsd"; -const char *type = "vfat"; -const char *logfile_end = ".px4log"; -const char *mfile_end = ".m"; -char folder_path[64]; +static const char *mountpoint = "/fs/microsd"; -#define SDLOG_LED_PRIORITY LED_REQUEST_PRIORITY_MAX -#define BUFFER_BYTES 1000 // length of buffer -#define SAVE_EVERY_BYTES 2000 -#define MAX_MOUNT_TRIES 5 - -static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext); // is executed when SIGUSR1 is received -bool sdlog_sigusr1_rcvd; // if this is set to true through SIGUSR1, sdlog will terminate - -static pthread_t logbuffer_thread; // thread to copy log values to the buffer -static void *logbuffer_loop(void *arg); +/** + * Deamon management function. + */ +__EXPORT int sdlog_main(int argc, char *argv[]); +/** + * Mainloop of deamon. + */ +int sdlog_thread_main(int argc, char *argv[]); -uint8_t *buffer_start; // always points to the very beginning -uint8_t *buffer_end; // always points to the very end -uint8_t *buffer_cursor_start; // points to the start of the current buffer -uint8_t *buffer_cursor_end; // points to the end of the current buffer -size_t buffer_bytes_used; // amount stored in the buffer at the moment +/** + * Print the correct usage. + */ +static void usage(const char *reason); -uint32_t bytes_recv; // to count bytes received and written to the sdcard -uint32_t bytes_sent; // to count the bytes written to the buffer +static int file_exist(const char *filename); -/**************************************************************************** - * user_start - ****************************************************************************/ +/** + * Create folder for current logging session. + */ +static int create_logfolder(char* folder_path); -int file_exist(const char *filename) +static void +usage(const char *reason) { - struct stat buffer; - return (stat(filename, &buffer) == 0); + if (reason) + fprintf(stderr, "%s\n", reason); + errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n"); } - +/** + * The 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_create(). + */ int sdlog_main(int argc, char *argv[]) { - // print text - printf("[sdlog] initialized\n"); - usleep(10000); - - sdlog_sigusr1_rcvd = false; + if (argc < 1) + usage("missing command"); - led_request_t amber_on_request = {.led = LED_AMBER, .priority = SDLOG_LED_PRIORITY, .request_type = LED_REQUEST_ON}; - led_request_t amber_off_request = {.led = LED_AMBER, .priority = SDLOG_LED_PRIORITY, .request_type = LED_REQUEST_OFF}; - amber_on_request.pid = getpid(); - amber_off_request.pid = getpid(); + if (!strcmp(argv[1], "start")) { - /* signal handler to abort when low voltage occurs */ - struct sigaction act; - struct sigaction oact; - act.sa_sigaction = sdlog_sig_handler; - (void)sigemptyset(&act.sa_mask); - (void)sigaddset(&act.sa_mask, SIGUSR1); - (void)sigaction(SIGUSR1, &act, &oact); - - uint8_t mount_counter = 0; - - commander_state_machine_t current_system_state; - - global_data_send_led_request(&amber_on_request); - - if (file_exist(trgt) == 1) { - printf("[sdlog] Mount already available at %s\n", trgt); - global_data_send_led_request(&amber_off_request); - - } else { - printf("[sdlog] Mount not available yet, trying to mount...\n"); + if (thread_running) { + printf("sdlog already running\n"); + /* this is not an error */ + exit(0); + } - /* mostly the first mount fails, sometimes it helps to press the card onto the board! */ - while (mount(src, trgt, type, 0, "") != 0) { - /* abort if kill signal is received */ - if (sdlog_sigusr1_rcvd == true) { - return 0; - } + 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); + thread_running = true; + exit(0); + } - //make sure were not airborne - global_data_trylock(&global_data_sys_status->access_conf); - current_system_state = global_data_sys_status->state_machine; - global_data_unlock(&global_data_sys_status->access_conf); - - if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT || current_system_state == SYSTEM_STATE_GROUND_ERROR) { - usleep(1000000); - printf("[sdlog] ERROR: Failed to mount SD card (attempt %d of %d), reason: %s\n", mount_counter + 1, MAX_MOUNT_TRIES, strerror((int)*get_errno_ptr())); - global_data_send_led_request(&amber_off_request); - mount_counter++; - - } else { - printf("[sdlog] WARNING: Not mounting SD card in flight!\n"); - printf("[sdlog] ending now...\n"); - fflush(stdout); - return 0; - } + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } - if (mount_counter >= MAX_MOUNT_TRIES) { - printf("[sdlog] ERROR: SD card could not be mounted!\n"); - printf("[sdlog] ending now...\n"); - fflush(stdout); - return 0; - } + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tsdlog is running\n"); + } else { + printf("\tsdlog not started\n"); } - - printf("[sdlog] Mount created at %s...\n", trgt); - global_data_send_led_request(&amber_off_request); + exit(0); } + usage("unrecognized command"); + exit(1); +} +int create_logfolder(char* folder_path) { /* make folder on sdcard */ uint16_t foldernumber = 1; // start with folder 0001 int mkdir_ret; /* look for the next folder that does not exist */ while (foldernumber < MAX_NO_LOGFOLDER) { - /* set up file path: e.g. /mnt/sdcard/logfile0001.txt */ - sprintf(folder_path, "%s/session%04u", trgt, foldernumber); + /* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */ + sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber); mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); /* the result is -1 if the folder exists */ @@ -192,251 +160,204 @@ int sdlog_main(int argc, char *argv[]) continue; } else { - printf("[sdlog] ERROR: Failed creating new folder: %s\n", strerror((int)*get_errno_ptr())); + warn("failed creating new folder"); return -1; } } if (foldernumber >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - printf("[sdlog] ERROR: all %d possible folders exist already\n", MAX_NO_LOGFOLDER); + warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); return -1; } + return 0; +} - /* write m-file for evaluation in Matlab */ - FILE *mfile; - char mfile_path[64] = ""; // string to hold the path to the mfile - const char *mfilename = "sdlog_eval"; +int sdlog_thread_main(int argc, char *argv[]) { - sprintf(mfile_path, "%s/%s%s", folder_path, mfilename, mfile_end); + printf("[sdlog] starting\n"); - if (NULL == (mfile = fopen(mfile_path, "w"))) { - printf("[sdlog] ERROR: opening %s failed: %s\n", mfile_path, strerror((int)*get_errno_ptr())); + if (file_exist(mountpoint) != OK) { + errx(1, "logging mount point %s not present, exiting.", mountpoint); } - /* write file contents generated using updatesdlog.sh and mfile.template in nuttx/configs/px4fmu/include */ - fwrite(MFILE_STRING, sizeof(MFILE_STRING), 1, mfile); - fprintf(mfile, "\n"); - fclose(mfile); - printf("[sdlog] m-file written to sdcard\n"); + char folder_path[64]; + if (create_logfolder(folder_path)) + errx(1, "unable to create logging folder, exiting"); + + /* create sensorfile */ + int sensorfile; + FILE *gpsfile; + // FILE *vehiclefile; + + char path_buf[64] = ""; // string to hold the path to the sensorfile + /* 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); + } - /* create the ringbuffer */ - uint8_t buffer[BUFFER_BYTES]; - buffer_start = buffer; - buffer_cursor_start = buffer; - buffer_cursor_end = buffer; - buffer_end = buffer + BUFFER_BYTES; + /* 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); + + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 25; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + + + union { + struct sensor_combined_s raw; + struct vehicle_attitude_s att; + struct vehicle_attitude_setpoint_s att_sp; + struct actuator_outputs_s act_outputs; + struct actuator_controls_s actuators; + } buf; + + struct { + int sensor_sub; + int att_sub; + int spa_sub; + int act_0_sub; + int actuators_sub; + } subs; + + /* --- 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; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- 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++; + + /* --- ATTITUDE SETPOINT VALUE --- */ + /* 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++; + + /** --- ACTUATOR OUTPUTS --- */ + subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + fds[fdsc_count].fd = subs.act_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- 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; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* WARNING: If you get the error message below, + * then the number of registered messages (fdsc) + * differs from the number of messages in the above list. + */ + if (fdsc_count > fdsc) { + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); + fdsc_count = fdsc; + } - /* create loop to write log data in a ringbuffer */ - pthread_attr_t logbuffer_attr; - pthread_attr_init(&logbuffer_attr); - pthread_attr_setstacksize(&logbuffer_attr, 2400); - pthread_create(&logbuffer_thread, &logbuffer_attr, logbuffer_loop, NULL); + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 1000; - /* create logfile */ - FILE *logfile; - char logfile_path[64] = ""; // string to hold the path to the logfile - const char *logfilename = "all"; + thread_running = true; - /* set up file path: e.g. /mnt/sdcard/session0001/gpslog.txt */ - sprintf(logfile_path, "%s/%s%s", folder_path, logfilename, logfile_end); + while (!thread_should_exit) { + int poll_ret = poll(fds, fdsc_count, timeout); - if (NULL == (logfile = fopen(logfile_path, "wb"))) { - printf("[sdlog] opening %s failed: %s\n", logfile_path, strerror((int)*get_errno_ptr())); - } + /* 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 { -// else -// { -// printf("[sdlog] opening %s was successful\n",logfile_path); -// } - - int logfile_no = fileno(logfile); - - bytes_recv = 0; /**< count all bytes that were received and written to the sdcard */ - size_t ret_write; /**< last amount written to sdcard */ - size_t target_write; /**< desired amount to write to sdcard */ - int ret_fsync; /**< return value of fsync() system call */ - int error_count = 0; /**< number of continous errors (one successful write resets it) */ - - /* Start logging */ - while (1) { - /* write as soon as content is in the buffer */ - while (buffer_bytes_used > 1) { - /* case where the data is not wrapped in the buffer */ - if (buffer_cursor_start < buffer_cursor_end) { - /* write all available data */ - target_write = (size_t)(buffer_cursor_end - buffer_cursor_start); - - if (0 <= (ret_write = fwrite(buffer_cursor_start, 1, target_write, logfile))) { - /* decrease the amount stored in the buffer, normally to 0 */ - buffer_bytes_used -= ret_write; - /* set new cursor position: wrap it in case it falls out of the buffer */ - buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES; - bytes_recv += ret_write; - error_count = 0; - - } else { - error_count++; - printf("[sdlog] ERROR: Write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr())); - } - } + int ifds = 0; + + /* --- SENSORS RAW VALUE --- */ + if (fds[ifds++].revents & POLLIN) { - /* case where the content is wrapped around the buffer */ - else if (buffer_cursor_start > buffer_cursor_end) { - /* write data until the end of the buffer */ - target_write = (size_t)(buffer_end - buffer_cursor_start); - - if (0 <= (ret_write = fwrite(buffer_cursor_start, 1, target_write, logfile))) { - /* decrease the amount stored in the buffer */ - buffer_bytes_used -= ret_write; - /* set new cursor position: wrap it in case it falls out of the buffer */ - buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES; - bytes_recv += ret_write; - error_count = 0; - - } else { - error_count++; - printf("[sdlog] ERROR: Write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr())); - } - - /* write the rest of the data at the beginning of the buffer */ - target_write = (size_t)(buffer_cursor_end - buffer_start); - - if (0 <= (ret_write = fwrite(buffer_start, 1, target_write, logfile))) { - /* decrease the amount stored in the buffer, now normally to 0 */ - buffer_bytes_used -= ret_write; - /* set new cursor position: wrap it in case it falls out of the buffer */ - buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES; - bytes_recv += ret_write; - error_count = 0; - - } else { - error_count++; - printf("[sdlog] ERROR: write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr())); - } - - } else { - error_count++; - printf("[sdlog] ERROR: dropped data\n"); + /* 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)); } -// if(bytes_recv>500000) -// { -// goto finished; -// } - } + /* --- ATTITUDE VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy attitude data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - /* save and exit if we received signal 1 or have a permanent error */ - if (sdlog_sigusr1_rcvd == true || error_count > 100) { - fclose(logfile); - umount(trgt); + + } - if (error_count > 100) { - return ERROR; + /* --- 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); + + } - } else { - return OK; + /* --- 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); + } - } - /* save file from time to time */ - else if ((bytes_recv > 0 && bytes_recv % SAVE_EVERY_BYTES == 0)) { - if ((ret_fsync = fsync(logfile_no)) != OK) { - printf("[sdlog] ERROR: sync fail: #%d, %s\n", ret_fsync, strerror((int)*get_errno_ptr())); + /* --- ACTUATOR CONTROL --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators); + } - } -// else if(bytes_recv > 2*SAVE_EVERY_BYTES) -// { -// goto finished; -// } - /* sleep, not to block everybody else */ - usleep(1000); + /* enforce write to disk */ + fsync(sensorfile); + fsync(gpsfile_no); + } } -//finished: -// -// /* at the moment we should not end here */ -// fclose(logfile); -// printf("[sdlog] logfile saved\n"); -// umount(trgt); -// printf("[sdlog] ending...\n"); -// fflush(stdout); + warn("exiting."); + + close(sensorfile); + fclose(gpsfile); + + thread_running = false; return 0; } -/* is executed when SIGUSR1 is raised (to terminate the app) */ -static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext) +/** + * @return 0 if file exists + */ +int file_exist(const char *filename) { - sdlog_sigusr1_rcvd = true; + struct stat buffer; + return stat(filename, &buffer); } - -static void *logbuffer_loop(void *arg) -{ - /* set name for this pthread */ - prctl(PR_SET_NAME, "sdlog logbuffer", getpid()); - - bytes_sent = 0; // count all bytes written to the buffer - - //size_t block_length = sizeof(global_data_sensors_raw_t) - 4*sizeof(access_conf_t); - size_t first_block_length; - size_t second_block_length; - buffer_bytes_used = 0; - - log_block_t log_block = {.check = {'$', '$', '$', '$'}}; - - size_t block_length = sizeof(log_block_t); -// printf("Block length is: %u",(uint16_t)block_length); - -// uint16_t test_counter = 0; - - /* start copying data to buffer */ - while (1) { - copy_block(&log_block); - - /* no more free space in buffer */ - if (buffer_bytes_used + (uint16_t)block_length > BUFFER_BYTES) { - printf("[sdlog] buf full, skipping\n"); - } - - /* data needs to be wrapped around ringbuffer*/ - else if (buffer_cursor_end + block_length >= buffer_end) { - /* first block length is from cursor until the end of the buffer */ - first_block_length = (size_t)(buffer_end - buffer_cursor_end); - /* second block length is the rest */ - second_block_length = block_length - first_block_length; - /* copy the data, not the pointer conversion */ - memcpy(buffer_cursor_end, ((uint8_t *) & (log_block)), first_block_length); - memcpy(buffer_start, ((uint8_t *) & (log_block)) + first_block_length, second_block_length); - /* set the end position of the cursor */ - buffer_cursor_end = buffer_start + second_block_length; - /* increase the amount stored in the buffer */ - buffer_bytes_used += block_length; - bytes_sent += block_length; - } - - /* data does not need to be wrapped around */ - else { - /* copy the whole block in one step */ - memcpy(buffer_cursor_end, ((uint8_t *) & (log_block)), block_length); - /* set the cursor to 0 of the buffer instead of the end */ - buffer_cursor_end = buffer_start + (buffer_cursor_end - buffer_start + block_length) % BUFFER_BYTES; - /* increase the amount stored in the buffer */ - buffer_bytes_used += block_length; - bytes_sent += block_length; - } - - /* also end the pthread when we receive a SIGUSR1 */ - if (sdlog_sigusr1_rcvd == true) { - break; - } - } - - return NULL; -} diff --git a/apps/sdlog/sdlog.h b/apps/sdlog/sdlog.h deleted file mode 100644 index 8012af177..000000000 --- a/apps/sdlog/sdlog.h +++ /dev/null @@ -1,17 +0,0 @@ -/* - * sdlog.h - * - * Created on: Mar 8, 2012 - * Author: romanpatscheider - */ - -#ifndef SENSORS_H_ -#define SENSORS_H_ -/**************************************************************************** - * Included Files - ****************************************************************************/ - - -#define MAX_NO_LOGFOLDER 3000 // tested up to here... - -#endif /* SENSORS_H_ */ diff --git a/apps/sdlog/sdlog_generated.h b/apps/sdlog/sdlog_generated.h deleted file mode 100644 index ca16ef908..000000000 --- a/apps/sdlog/sdlog_generated.h +++ /dev/null @@ -1,238 +0,0 @@ -/* This file is autogenerated in nuttx/configs/px4fmu/include/updatesdlog.py */ - -#ifndef SDLOG_GENERATED_H_ -#define SDLOG_GENERATED_H_ - - -typedef struct { - uint64_t sensors_raw_timestamp; - int16_t sensors_raw_gyro_raw[3]; - uint16_t sensors_raw_gyro_raw_counter; - int16_t sensors_raw_accelerometer_raw[3]; - uint16_t sensors_raw_accelerometer_raw_counter; - float attitude_roll; - float attitude_pitch; - float attitude_yaw; - float position_lat; - float position_lon; - float position_alt; - float position_relative_alt; - float position_vx; - float position_vy; - float position_vz; - int32_t gps_lat; - int32_t gps_lon; - int32_t gps_alt; - uint16_t gps_eph; - float ardrone_control_setpoint_thrust_cast; - float ardrone_control_setpoint_attitude[3]; - float ardrone_control_position_control_output[3]; - float ardrone_control_attitude_setpoint_navigationframe_from_positioncontroller[3]; - char check[4]; -} __attribute__((__packed__)) log_block_t; - -void copy_block(log_block_t *log_block) -{ - if (global_data_wait(&global_data_sensors_raw->access_conf_rate_low) == 0) { - memcpy(&log_block->sensors_raw_timestamp, &global_data_sensors_raw->timestamp, sizeof(uint64_t) * 1); - memcpy(&log_block->sensors_raw_gyro_raw, &global_data_sensors_raw->gyro_raw, sizeof(int16_t) * 3); - memcpy(&log_block->sensors_raw_gyro_raw_counter, &global_data_sensors_raw->gyro_raw_counter, sizeof(uint16_t) * 1); - memcpy(&log_block->sensors_raw_accelerometer_raw, &global_data_sensors_raw->accelerometer_raw, sizeof(int16_t) * 3); - memcpy(&log_block->sensors_raw_accelerometer_raw_counter, &global_data_sensors_raw->accelerometer_raw_counter, sizeof(uint16_t) * 1); - - if (global_data_trylock(&global_data_attitude->access_conf) == 0) { - memcpy(&log_block->attitude_roll, &global_data_attitude->roll, sizeof(float) * 1); - memcpy(&log_block->attitude_pitch, &global_data_attitude->pitch, sizeof(float) * 1); - memcpy(&log_block->attitude_yaw, &global_data_attitude->yaw, sizeof(float) * 1); - global_data_unlock(&global_data_attitude->access_conf); - } - - if (global_data_trylock(&global_data_position->access_conf) == 0) { - memcpy(&log_block->position_lat, &global_data_position->lat, sizeof(float) * 1); - memcpy(&log_block->position_lon, &global_data_position->lon, sizeof(float) * 1); - memcpy(&log_block->position_alt, &global_data_position->alt, sizeof(float) * 1); - memcpy(&log_block->position_relative_alt, &global_data_position->relative_alt, sizeof(float) * 1); - memcpy(&log_block->position_vx, &global_data_position->vx, sizeof(float) * 1); - memcpy(&log_block->position_vy, &global_data_position->vy, sizeof(float) * 1); - memcpy(&log_block->position_vz, &global_data_position->vz, sizeof(float) * 1); - global_data_unlock(&global_data_position->access_conf); - } - - if (global_data_trylock(&global_data_gps->access_conf) == 0) { - memcpy(&log_block->gps_lat, &global_data_gps->lat, sizeof(int32_t) * 1); - memcpy(&log_block->gps_lon, &global_data_gps->lon, sizeof(int32_t) * 1); - memcpy(&log_block->gps_alt, &global_data_gps->alt, sizeof(int32_t) * 1); - memcpy(&log_block->gps_eph, &global_data_gps->eph, sizeof(uint16_t) * 1); - global_data_unlock(&global_data_gps->access_conf); - } - - if (global_data_trylock(&global_data_ardrone_control->access_conf) == 0) { - memcpy(&log_block->ardrone_control_setpoint_thrust_cast, &global_data_ardrone_control->setpoint_thrust_cast, sizeof(float) * 1); - memcpy(&log_block->ardrone_control_setpoint_attitude, &global_data_ardrone_control->setpoint_attitude, sizeof(float) * 3); - memcpy(&log_block->ardrone_control_position_control_output, &global_data_ardrone_control->position_control_output, sizeof(float) * 3); - memcpy(&log_block->ardrone_control_attitude_setpoint_navigationframe_from_positioncontroller, &global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller, sizeof(float) * 3); - global_data_unlock(&global_data_ardrone_control->access_conf); - } - - global_data_unlock(&global_data_sensors_raw->access_conf_rate_low); - } -} -#define MFILE_STRING "% This file is autogenerated in updatesdlog.py and mfile.template in apps/sdlog\n\ -%% Define logged values \n\ -\n\ -logTypes = {};\n\ -logTypes{end+1} = struct('data','sensors_raw','variable_name','timestamp','type_name','uint64','type_bytes',8,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','sensors_raw','variable_name','gyro_raw','type_name','int16','type_bytes',2,'number_of_array',3);\n\ -logTypes{end+1} = struct('data','sensors_raw','variable_name','gyro_raw_counter','type_name','uint16','type_bytes',2,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','sensors_raw','variable_name','accelerometer_raw','type_name','int16','type_bytes',2,'number_of_array',3);\n\ -logTypes{end+1} = struct('data','sensors_raw','variable_name','accelerometer_raw_counter','type_name','uint16','type_bytes',2,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','attitude','variable_name','roll','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','attitude','variable_name','pitch','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','attitude','variable_name','yaw','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','lat','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','lon','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','alt','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','relative_alt','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','vx','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','vy','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','position','variable_name','vz','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','gps','variable_name','lat','type_name','int32','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','gps','variable_name','lon','type_name','int32','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','gps','variable_name','alt','type_name','int32','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','gps','variable_name','eph','type_name','uint16','type_bytes',2,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','ardrone_control','variable_name','setpoint_thrust_cast','type_name','float','type_bytes',4,'number_of_array',1);\n\ -logTypes{end+1} = struct('data','ardrone_control','variable_name','setpoint_attitude','type_name','float','type_bytes',4,'number_of_array',3);\n\ -logTypes{end+1} = struct('data','ardrone_control','variable_name','position_control_output','type_name','float','type_bytes',4,'number_of_array',3);\n\ -logTypes{end+1} = struct('data','ardrone_control','variable_name','attitude_setpoint_navigationframe_from_positioncontroller','type_name','float','type_bytes',4,'number_of_array',3);\n\ -\ -%% Import logfiles\n\ -\n\ -% define log file and GPSs\n\ -logfileFolder = 'logfiles';\n\ -fileName = 'all';\n\ -fileEnding = 'px4log';\n\ -\n\ -numberOfLogTypes = length(logTypes);\n\ -\n\ -path = [fileName,'.', fileEnding];\n\ -fid = fopen(path, 'r');\n\ -\n\ -% get file length\n\ -fseek(fid, 0,'eof');\n\ -fileLength = ftell(fid);\n\ -fseek(fid, 0,'bof');\n\ -\n\ -% get length of one block\n\ -blockLength = 4; % check: $$$$\n\ -for i=1:numberOfLogTypes\n\ - blockLength = blockLength + logTypes{i}.type_bytes*logTypes{i}.number_of_array;\n\ -end\n\ -\n\ -% determine number of entries\n\ -entries = fileLength / blockLength;\n\ -\n\ -\n\ -% import data\n\ -offset = 0;\n\ -for i=1:numberOfLogTypes\n\ - \n\ - data.(genvarname(logTypes{i}.data)).(genvarname(logTypes{i}.variable_name)) = transpose(fread(fid,[logTypes{i}.number_of_array,entries],[num2str(logTypes{i}.number_of_array),'*',logTypes{i}.type_name,'=>',logTypes{i}.type_name],blockLength-logTypes{i}.type_bytes*logTypes{i}.number_of_array));\n\ - offset = offset + logTypes{i}.type_bytes*logTypes{i}.number_of_array;\n\ - fseek(fid, offset,'bof');\n\ - \n\ - progressPercentage = i/numberOfLogTypes*100;\n\ - fprintf('%3.0f%%',progressPercentage);\n\ -end\n\ -\n\ -\n\ -%% Plots\n\ -\n\ -figure\n\ -plot(data.sensors_raw.timestamp,data.sensors_raw.gyro_raw)\n\ -\n\ -figure\n\ -plot(data.sensors_raw.timestamp,data.sensors_raw.accelerometer_raw)\n\ -\n\ -%% Check for lost data\n\ -\n\ -% to detect lost frames (either when logging to sd card or if no new data is\n\ -% data is available for some time)\n\ -diff_counter = diff(data.sensors_raw.gyro_raw_counter);\n\ -figure\n\ -plot(diff_counter)\n\ -\n\ -% to detect how accurate the timing was\n\ -diff_timestamp = diff(data.sensors_raw.timestamp);\n\ -\n\ -figure\n\ -plot(diff_timestamp)\n\ -\n\ -%% Export to file for google earth\n\ -\n\ -\n\ -if(isfield(data.gps,'lat') && isfield(data.gps,'lon') && isfield(data.gps,'alt'))\n\ -\n\ - % extract coordinates and height where they are not zero\n\ - maskWhereNotZero = ((data.gps.lon ~= 0 & data.gps.lat ~= 0 ) & data.gps.alt ~= 0);\n\ -\n\ - % plot\n\ - figure\n\ - plot3(data.gps.lon(maskWhereNotZero),data.gps.lat(maskWhereNotZero),data.gps.alt(maskWhereNotZero))\n\ -\n\ -\n\ - % create a kml file according to https://developers.google.com/kml/documentation/kml_tut\n\ - % also see https://support.google.com/earth/bin/answer.py?hl=en&answer=148072&topic=2376756&ctx=topic\n\ -\n\ - % open file and overwrite content\n\ - fileId = fopen('gps_path.kml','w+');\n\ -\n\ - % define strings that should be written to file\n\ - fileStartDocumentString = ['<?xml version=\"1.0\" encoding=\"UTF-8\"?><kml xmlns=\"http://www.opengis.net/kml/2.2\"><Document><name>Paths</name><description>Path</description>'];\n\ -\n\ - fileStyleString = '<Style id=\"blueLinebluePoly\"><LineStyle><color>7fff0000</color><width>4</width></LineStyle><PolyStyle><color>7fff0000</color></PolyStyle></Style>';\n\ -\n\ - filePlacemarkString = '<Placemark><name>Absolute Extruded</name><description>Transparent blue wall with blue outlines</description><styleUrl>#blueLinebluePoly</styleUrl><LineString><extrude>1</extrude><tessellate>1</tessellate><altitudeMode>absolute</altitudeMode><coordinates>';\n\ -\n\ - fileEndPlacemarkString = '</coordinates></LineString></Placemark>';\n\ - fileEndDocumentString = '</Document></kml>';\n\ -\n\ - % start writing to file\n\ - fprintf(fileId,fileStartDocumentString);\n\ -\n\ - fprintf(fileId,fileStyleString);\n\ - fprintf(fileId,filePlacemarkString);\n\ -\n\ - \n\ - lonTemp = double(data.gps.lon(maskWhereNotZero))/1E7;\n\ - latTemp = double(data.gps.lat(maskWhereNotZero))/1E7;\n\ - altTemp = double(data.gps.alt(maskWhereNotZero))/1E3 + 100.0; % in order to see the lines above ground\n\ -\n\ - % write coordinates to file\n\ - for k=1:length(data.gps.lat(maskWhereNotZero))\n\ - if(mod(k,10)==0)\n\ - fprintf(fileId,'%.10f,%.10f,%.10f\\n',lonTemp(k),latTemp(k),altTemp(k));\n\ - end\n\ - end\n\ -\n\ - % write end placemark\n\ - fprintf(fileId,fileEndPlacemarkString);\n\ -\n\ - % write end of file\n\ - fprintf(fileId,fileEndDocumentString);\n\ -\n\ - % close file, it should now be readable in Google Earth using File -> Open\n\ - fclose(fileId);\n\ -\n\ -end\n\ -\n\ -if(isfield(data.position,'lat') && isfield(data.position,'lon') && isfield(data.position,'alt'))\n\ -\n\ - % extract coordinates and height where they are not zero\n\ - maskWhereNotZero = ((data.position.lon ~= 0 & data.position.lat ~= 0 ) & data.position.alt ~= 0);\n\ - \n\ - % plot\n\ - figure\n\ - plot3(data.position.lon(maskWhereNotZero),data.position.lat(maskWhereNotZero),data.position.alt(maskWhereNotZero))\n\ -end\n\ -" -#endif
\ No newline at end of file diff --git a/apps/sdlog/updatesdlog.py b/apps/sdlog/updatesdlog.py deleted file mode 100644 index 5892bc40a..000000000 --- a/apps/sdlog/updatesdlog.py +++ /dev/null @@ -1,199 +0,0 @@ -import os -import glob - -# path to global data files -base_path = '../orb/' -cfile = './sdlog_generated.h' -mfile_template = './mfile.template' - -# there should be one LOGBROADCAST which gives the timing for the logging -logbroadcast_found = 0 - -# these types can nicely be imported into Matlab -allowed_types = ['uint8_t','int8_t','uint16_t','int16_t','uint32_t','int32_t','uint64_t','int64_t','float','double'] - -log_entries = [] -# loop through global_data_files ending in _t.h and look for LOGME (per variable) and LOGBROADCAST (overall) -for path in glob.glob( os.path.join(base_path, '*_t.h') ): - # filename is supposed to be global_data_bapedibup_t.h - if 'global_data' not in path: - print 'path: ' + path - raise 'wrong filename found' - f = open(path, 'r') - access_conf_found = False; - # strip away ../../../../apps/orb/ and _t.h - data_name = path.lstrip(base_path)[0:-4] - # strip away ../../../../apps/orb/ and global_data_ and _t.h - name = path.lstrip(base_path)[12:-4] - log_entry = {'data_name': data_name,'name':name,'vars': []} - - logbroadcast = False; - - # loop throug lines - for line in f: - - line_parts = line.split() - # access_conf is needed to lock the data - if 'access_conf_t' in line: - - # always use the access_conf which has the LOGBROADCAST flag - if 'LOGBROADCAST' in line: - access_conf_found = True - log_entry['access_conf_name'] = line_parts[1].rstrip(';') - logbroadcast = True - print 'LOGBROADCAST found in ' + data_name - logbroadcast_found += 1 - # but use an access_conf anyway - elif access_conf_found == False: - access_conf_found = True - log_entry['access_conf_name'] = line_parts[1].rstrip(';') - # variables flagged with LOGME should be logged - elif 'LOGME' in line: - var_entry = {'type': line_parts[0]} - - # check that it is an allowed type - if var_entry['type'] not in allowed_types: - print 'file: '+ path + ', type: ' + var_entry['type'] - raise 'unsupported type' - - # save variable name and number for array - if '[' in line_parts[1]: - var_entry['name'] = line_parts[1].split('[')[0] - var_entry['number'] = line_parts[1].split('[')[1].rstrip('];') - else: - var_entry['name'] = line_parts[1].rstrip(';') - var_entry['number'] = 1 - - # add the variable - log_entry['vars'].append(var_entry) - # only use the global data file if any variables have a LOGME - if logbroadcast == True and len(log_entry['vars']) > 0: - logbroadcast_entry = log_entry - elif len(log_entry['vars']) > 0: - print 'added ' + log_entry['data_name'] - log_entries.append(log_entry) - f.close() - -# check that we have one and only one LOGBROADCAST -if logbroadcast_found > 1: - raise 'too many LOGBROADCAST found\n' -elif logbroadcast_found == 0: - raise 'no LOGBROADCAST found\n' - -# write function to c file - -header = '/* This file is autogenerated in nuttx/configs/px4fmu/include/updatesdlog.py */\n\ -\n\ -#ifndef SDLOG_GENERATED_H_\n\ -#define SDLOG_GENERATED_H_\n\ -\n\ -\n\ -' - -cstruct = 'typedef struct\n{\n' - -for j in logbroadcast_entry['vars']: - cstruct += '\t' + j['type'] + ' ' + logbroadcast_entry['name'] + '_' + j['name'] - if j['number'] == 1: - cstruct += ';\n' - else: - cstruct += '[' + j['number'] + '];\n' - -for i in log_entries: - for j in i['vars']: - cstruct += '\t' + j['type'] + ' ' + i['name'] + '_' + j['name'] - if j['number'] == 1: - cstruct += ';\n' - else: - cstruct += '[' + j['number'] + '];\n' - -cstruct += '\tchar check[4];\n} __attribute__((__packed__)) log_block_t;\n\n' - - -copy_function = 'void copy_block(log_block_t* log_block)\n{\n' -copy_function += '\tif(global_data_wait(&' + logbroadcast_entry['data_name'] + '->' + logbroadcast_entry['access_conf_name'] + ') == 0)\n\t{\n' - -for j in logbroadcast_entry['vars']: - copy_function += '\t\tmemcpy(&log_block->' + logbroadcast_entry['name'] + '_' + j['name'] + ',&' + logbroadcast_entry['data_name'] + '->' + j['name'] + ',sizeof(' + j['type'] + ')*' + str(j['number']) + ');\n' - #copy_function += '\t\t}\n' - -# generate logging MACRO - - -for i in log_entries: - copy_function += '\t\tif(global_data_trylock(&' + i['data_name'] + '->' + i['access_conf_name'] + ') == 0)\n\t\t{\n' - - for j in i['vars']: - copy_function += '\t\t\tmemcpy(&log_block->' + i['name'] + '_' + j['name'] + ',&' + i['data_name'] + '->' + j['name'] + ',sizeof(' + j['type'] + ')*' + str(j['number']) + ');\n' - - copy_function += '\t\t\tglobal_data_unlock(&' + i['data_name'] + '->' + i['access_conf_name'] + ');\n' - copy_function += '\t\t}\n' -copy_function += '\t\tglobal_data_unlock(&' + logbroadcast_entry['data_name'] + '->' + logbroadcast_entry['access_conf_name'] + ');\n' -copy_function += '\t}\n' - -copy_function += '}\n' - -footer = '\n#endif' - - - -# generate mfile - -type_bytes = { -'uint8_t' : 1, -'int8_t' : 1, -'uint16_t' : 2, -'int16_t' : 2, -'uint32_t' : 4, -'int32_t' : 4, -'uint64_t' : 8, -'int64_t' : 8, -'float' : 4, -'double' : 8, -} - -type_names_matlab = { -'uint8_t' : 'uint8', -'int8_t' : 'int8', -'uint16_t' : 'uint16', -'int16_t' : 'int16', -'uint32_t' : 'uint32', -'int32_t' : 'int32', -'uint64_t' : 'uint64', -'int64_t' : 'int64', -'float' : 'float', -'double' : 'double', -} - - -# read template mfile -mf = open(mfile_template, 'r') -mfile_template_string = mf.read() - -mfile_define = '#define MFILE_STRING "% This file is autogenerated in updatesdlog.py and mfile.template in apps/sdlog\\n\\\n' -mfile_define += '%% Define logged values \\n\\\n\\n\\\nlogTypes = {};\\n\\\n' - -for j in logbroadcast_entry['vars']: - mfile_define += 'logTypes{end+1} = struct(\'data\',\'' + logbroadcast_entry['name'] + '\',\'variable_name\',\'' + j['name'] + '\',\'type_name\',\'' + type_names_matlab.get(j['type']) + '\',\'type_bytes\',' + str(type_bytes.get(j['type'])) + ',\'number_of_array\',' + str(j['number']) + ');\\n\\\n' - -for i in log_entries: - for j in i['vars']: - mfile_define += 'logTypes{end+1} = struct(\'data\',\'' + i['name'] + '\',\'variable_name\',\'' + j['name'] + '\',\'type_name\',\'' + type_names_matlab.get(j['type']) + '\',\'type_bytes\',' + str(type_bytes.get(j['type'])) + ',\'number_of_array\',' + str(j['number']) + ');\\n\\\n' - - - -mfile_define += '\\\n' + mfile_template_string.replace('\n', '\\n\\\n').replace('\"','\\\"') + '"' - - -# write to c File -cf = open(cfile, 'w') -cf.write(header) -cf.write(cstruct); -cf.write(copy_function) - -cf.write(mfile_define) - -cf.write(footer) -cf.close() - -print 'finished, cleanbuild needed!' |