mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 20:19:06 +08:00
Rewrote SD logging app, simpler, but effective. Pending testing
This commit is contained in:
Lorenz Meier
parent
0019f65b10
commit
e440fc4027
@ -64,10 +64,9 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
|
||||
|
||||
printf("[deamon] starting\n");
|
||||
|
||||
while (true) {
|
||||
while (!thread_should_exit) {
|
||||
printf("Hello Deamon!\n");
|
||||
sleep(10);
|
||||
if (thread_should_exit) break;
|
||||
}
|
||||
|
||||
printf("[deamon] exiting.\n");
|
||||
|
||||
@ -61,7 +61,6 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/ardrone_control.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
@ -593,7 +592,6 @@ static void *uorb_receiveloop(void *arg)
|
||||
struct sensor_combined_s raw;
|
||||
struct vehicle_attitude_s att;
|
||||
struct vehicle_gps_position_s gps;
|
||||
struct ardrone_control_s ar_control;
|
||||
struct vehicle_local_position_setpoint_s local_sp;
|
||||
struct vehicle_global_position_setpoint_s global_sp;
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
@ -770,10 +768,10 @@ static void *uorb_receiveloop(void *arg)
|
||||
orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
|
||||
|
||||
/* send raw imu data */
|
||||
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
|
||||
/* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */
|
||||
//mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
|
||||
/* send scaled imu data */
|
||||
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0],
|
||||
buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0],
|
||||
buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0],
|
||||
buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
|
||||
|
||||
/* mark individual fields as changed */
|
||||
uint16_t fields_updated = 0;
|
||||
@ -847,26 +845,6 @@ static void *uorb_receiveloop(void *arg)
|
||||
gps_counter++;
|
||||
}
|
||||
|
||||
// /* --- ARDRONE CONTROL OUTPUTS --- */
|
||||
// if (fds[ifds++].revents & POLLIN) {
|
||||
// /* copy ardrone control data into local buffer */
|
||||
// orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
|
||||
// uint64_t timestamp = buf.ar_control.timestamp;
|
||||
// float setpoint_roll = buf.ar_control.setpoint_attitude[0];
|
||||
// float setpoint_pitch = buf.ar_control.setpoint_attitude[1];
|
||||
// float setpoint_yaw = buf.ar_control.setpoint_attitude[2];
|
||||
// float setpoint_thrust = buf.ar_control.setpoint_thrust_cast;
|
||||
|
||||
// float control_roll = buf.ar_control.attitude_control_output[0];
|
||||
// float control_pitch = buf.ar_control.attitude_control_output[1];
|
||||
// float control_yaw = buf.ar_control.attitude_control_output[2];
|
||||
|
||||
// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust);
|
||||
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll);
|
||||
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch);
|
||||
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw);
|
||||
// }
|
||||
|
||||
/* --- SYSTEM STATUS --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
/* immediately communicate state changes back to user */
|
||||
@ -1539,18 +1517,18 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
if (baudrate >= 921600) {
|
||||
/* 200 Hz / 5 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
|
||||
/* 200 Hz / 5 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
|
||||
/* 5 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
|
||||
} else if (baudrate >= 460800) {
|
||||
/* 200 Hz / 5 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
|
||||
/* 50 Hz / 10 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
@ -1559,10 +1537,10 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
} else if (baudrate >= 115200) {
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
/* 10 Hz / 100 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
|
||||
@ -1570,9 +1548,10 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
|
||||
} else if (baudrate >= 57600) {
|
||||
/* 10 Hz / 100 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
|
||||
/* 10 Hz / 100 ms ATTITUDE */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
@ -1581,7 +1560,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
|
||||
} else {
|
||||
/* very low baud rate, limit to 1 Hz / 1000 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
|
||||
/* 1 Hz / 1000 ms */
|
||||
|
||||
@ -57,7 +57,6 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/ardrone_control.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/ardrone_motors_setpoint.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
||||
@ -46,11 +46,8 @@
|
||||
#define MULTIROTOR_ATTITUDE_CONTROL_H_
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/ardrone_control.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
@ -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
|
||||
****************************************************************************/
|
||||
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 */
|
||||
|
||||
static const char *mountpoint = "/fs/microsd";
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
__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];
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int sdlog_thread_main(int argc, char *argv[]);
|
||||
|
||||
#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
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
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 int file_exist(const char *filename);
|
||||
|
||||
static pthread_t logbuffer_thread; // thread to copy log values to the buffer
|
||||
static void *logbuffer_loop(void *arg);
|
||||
/**
|
||||
* Create folder for current logging session.
|
||||
*/
|
||||
static int create_logfolder(char* folder_path);
|
||||
|
||||
|
||||
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)
|
||||
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);
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
sdlog_sigusr1_rcvd = false;
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
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;
|
||||
}
|
||||
if (thread_running) {
|
||||
printf("sdlog already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
printf("[sdlog] Mount created at %s...\n", trgt);
|
||||
global_data_send_led_request(&amber_off_request);
|
||||
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);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tsdlog is running\n");
|
||||
} else {
|
||||
printf("\tsdlog not started\n");
|
||||
}
|
||||
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;
|
||||
|
||||
/* create the ringbuffer */
|
||||
uint8_t buffer[BUFFER_BYTES];
|
||||
buffer_start = buffer;
|
||||
buffer_cursor_start = buffer;
|
||||
buffer_cursor_end = buffer;
|
||||
buffer_end = buffer + BUFFER_BYTES;
|
||||
char path_buf[64] = ""; // string to hold the path to the sensorfile
|
||||
|
||||
/* 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()));
|
||||
/* 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);
|
||||
}
|
||||
|
||||
// else
|
||||
// {
|
||||
// printf("[sdlog] opening %s was successful\n",logfile_path);
|
||||
// }
|
||||
/* 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);
|
||||
|
||||
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;
|
||||
// }
|
||||
}
|
||||
/* --- 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];
|
||||
|
||||
|
||||
/* 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);
|
||||
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;
|
||||
|
||||
if (error_count > 100) {
|
||||
return ERROR;
|
||||
struct {
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
int spa_sub;
|
||||
int act_0_sub;
|
||||
int actuators_sub;
|
||||
} subs;
|
||||
|
||||
} else {
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
/* --- 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++;
|
||||
|
||||
/* 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()));
|
||||
}
|
||||
}
|
||||
/* --- 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++;
|
||||
|
||||
// else if(bytes_recv > 2*SAVE_EVERY_BYTES)
|
||||
// {
|
||||
// goto finished;
|
||||
// }
|
||||
/* sleep, not to block everybody else */
|
||||
usleep(1000);
|
||||
/* --- 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;
|
||||
}
|
||||
|
||||
//finished:
|
||||
//
|
||||
// /* at the moment we should not end here */
|
||||
// fclose(logfile);
|
||||
// printf("[sdlog] logfile saved\n");
|
||||
// umount(trgt);
|
||||
// printf("[sdlog] ending...\n");
|
||||
// fflush(stdout);
|
||||
/*
|
||||
* set up poll to block for new data,
|
||||
* wait for a maximum of 1000 ms (1 second)
|
||||
*/
|
||||
const int timeout = 1000;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
int poll_ret = poll(fds, fdsc_count, timeout);
|
||||
|
||||
/* handle the poll result */
|
||||
if (poll_ret == 0) {
|
||||
/* XXX this means none of our providers is giving us data - might be an error? */
|
||||
} else if (poll_ret < 0) {
|
||||
/* XXX this is seriously bad - should be an emergency */
|
||||
} else {
|
||||
|
||||
int ifds = 0;
|
||||
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
|
||||
/* copy sensors raw data into local buffer */
|
||||
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
/* write out */
|
||||
write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw));
|
||||
}
|
||||
|
||||
/* --- ATTITUDE VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
|
||||
/* copy attitude data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
|
||||
|
||||
|
||||
}
|
||||
|
||||
/* --- VEHICLE ATTITUDE SETPOINT --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
|
||||
|
||||
}
|
||||
|
||||
/* --- 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);
|
||||
|
||||
}
|
||||
|
||||
/* --- ACTUATOR CONTROL --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators);
|
||||
|
||||
}
|
||||
|
||||
/* enforce write to disk */
|
||||
fsync(sensorfile);
|
||||
fsync(gpsfile_no);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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_ */
|
||||
@ -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
|
||||
@ -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!'
|
||||
@ -69,7 +69,7 @@ CONFIGURED_APPS += uORB
|
||||
CONFIGURED_APPS += mavlink
|
||||
CONFIGURED_APPS += gps
|
||||
CONFIGURED_APPS += commander
|
||||
#CONFIGURED_APPS += sdlog
|
||||
CONFIGURED_APPS += sdlog
|
||||
CONFIGURED_APPS += sensors
|
||||
CONFIGURED_APPS += ardrone_interface
|
||||
CONFIGURED_APPS += multirotor_att_control
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user