mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-04 21:23:57 +08:00
Rewrote SD logging app, simpler, but effective. Pending testing
This commit is contained in:
Lorenz Meier
@@ -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");
|
||||
|
||||
+13
-34
@@ -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,
|
||||
|
||||
+1
-3
@@ -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
|
||||
+251
-330
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user