From 8a365179eafdf3aea98e60ab9f5882b200d4c759 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 4 Aug 2012 15:12:36 -0700 Subject: Fresh import of the PX4 firmware sources. --- apps/sdlog/Makefile | 44 +++++ apps/sdlog/mfile.template | 136 +++++++++++++ apps/sdlog/sdlog.c | 442 +++++++++++++++++++++++++++++++++++++++++++ apps/sdlog/sdlog.h | 17 ++ apps/sdlog/sdlog_generated.h | 238 +++++++++++++++++++++++ apps/sdlog/updatesdlog.py | 199 +++++++++++++++++++ 6 files changed, 1076 insertions(+) create mode 100644 apps/sdlog/Makefile create mode 100644 apps/sdlog/mfile.template create mode 100644 apps/sdlog/sdlog.c create mode 100644 apps/sdlog/sdlog.h create mode 100644 apps/sdlog/sdlog_generated.h create mode 100644 apps/sdlog/updatesdlog.py (limited to 'apps/sdlog') diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile new file mode 100644 index 000000000..1e5abdb55 --- /dev/null +++ b/apps/sdlog/Makefile @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 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. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# sdlog Application +# + +APPNAME = sdlog +PRIORITY = SCHED_PRIORITY_DEFAULT - 1 +STACKSIZE = 3000 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/sdlog/mfile.template b/apps/sdlog/mfile.template new file mode 100644 index 000000000..0f7ebd045 --- /dev/null +++ b/apps/sdlog/mfile.template @@ -0,0 +1,136 @@ +%% 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 = ['PathsPath']; + + fileStyleString = ''; + + filePlacemarkString = 'Absolute ExtrudedTransparent blue wall with blue outlines#blueLinebluePoly11absolute'; + + fileEndPlacemarkString = ''; + fileEndDocumentString = ''; + + % 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 new file mode 100644 index 000000000..1b87aab77 --- /dev/null +++ b/apps/sdlog/sdlog.c @@ -0,0 +1,442 @@ +/**************************************************************************** + * examples/hello/main.c + * + * Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * 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 + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sdlog.h" +#include "sdlog_generated.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +__EXPORT int sdlog_main(int argc, char *argv[]); + +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]; + +#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); + + +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 + +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 + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int file_exist(const char *filename) +{ + struct stat buffer; + return (stat(filename, &buffer) == 0); +} + + +int sdlog_main(int argc, char *argv[]) +{ + // print text + printf("[sdlog] initialized\n"); + usleep(10000); + + sdlog_sigusr1_rcvd = false; + + 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(); + + /* 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"); + + /* 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; + } + + //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 (mount_counter >= MAX_MOUNT_TRIES) { + printf("[sdlog] ERROR: SD card could not be mounted!\n"); + printf("[sdlog] ending now...\n"); + fflush(stdout); + return 0; + } + } + + printf("[sdlog] Mount created at %s...\n", trgt); + global_data_send_led_request(&amber_off_request); + } + + + /* 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); + mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); + /* the result is -1 if the folder exists */ + + if (mkdir_ret == 0) { + /* folder does not exist */ + break; + + } else if (mkdir_ret == -1) { + /* folder exists already */ + foldernumber++; // to try next time + continue; + + } else { + printf("[sdlog] ERROR: Failed creating new folder: %s\n", strerror((int)*get_errno_ptr())); + 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); + return -1; + } + + + /* 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"; + + sprintf(mfile_path, "%s/%s%s", folder_path, mfilename, mfile_end); + + if (NULL == (mfile = fopen(mfile_path, "w"))) { + printf("[sdlog] ERROR: opening %s failed: %s\n", mfile_path, strerror((int)*get_errno_ptr())); + } + + /* 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"); + + + /* create the ringbuffer */ + uint8_t buffer[BUFFER_BYTES]; + buffer_start = buffer; + buffer_cursor_start = buffer; + buffer_cursor_end = buffer; + buffer_end = buffer + BUFFER_BYTES; + + /* 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); + + /* create logfile */ + FILE *logfile; + char logfile_path[64] = ""; // string to hold the path to the logfile + const char *logfilename = "all"; + + /* set up file path: e.g. /mnt/sdcard/session0001/gpslog.txt */ + sprintf(logfile_path, "%s/%s%s", folder_path, logfilename, logfile_end); + + + if (NULL == (logfile = fopen(logfile_path, "wb"))) { + printf("[sdlog] opening %s failed: %s\n", logfile_path, strerror((int)*get_errno_ptr())); + } + +// 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())); + } + } + + /* 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"); + } + +// if(bytes_recv>500000) +// { +// goto finished; +// } + } + + + /* 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; + + } else { + return OK; + } + } + + /* 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())); + } + } + +// else if(bytes_recv > 2*SAVE_EVERY_BYTES) +// { +// goto finished; +// } + /* sleep, not to block everybody else */ + usleep(1000); + } + +//finished: +// +// /* at the moment we should not end here */ +// fclose(logfile); +// printf("[sdlog] logfile saved\n"); +// umount(trgt); +// printf("[sdlog] ending...\n"); +// fflush(stdout); + + return 0; +} + +/* is executed when SIGUSR1 is raised (to terminate the app) */ +static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext) +{ + sdlog_sigusr1_rcvd = true; +} + + +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 new file mode 100644 index 000000000..8012af177 --- /dev/null +++ b/apps/sdlog/sdlog.h @@ -0,0 +1,17 @@ +/* + * 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 new file mode 100644 index 000000000..ca16ef908 --- /dev/null +++ b/apps/sdlog/sdlog_generated.h @@ -0,0 +1,238 @@ +/* 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 = ['PathsPath'];\n\ +\n\ + fileStyleString = '';\n\ +\n\ + filePlacemarkString = 'Absolute ExtrudedTransparent blue wall with blue outlines#blueLinebluePoly11absolute';\n\ +\n\ + fileEndPlacemarkString = '';\n\ + fileEndDocumentString = '';\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 new file mode 100644 index 000000000..5892bc40a --- /dev/null +++ b/apps/sdlog/updatesdlog.py @@ -0,0 +1,199 @@ +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!' -- cgit v1.2.3