Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 21:23:57 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

Rewrote SD logging app, simpler, but effective. Pending testing

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-10 23:04:31 +02:00
parent 0019f65b10
commit e440fc4027
11 changed files with 267 additions and 964 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/px4_deamon_app/px4_deamon_app.c
+1 -2
View File
@@ -64,10 +64,9 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n"); printf("[deamon] starting\n");
while (true) { while (!thread_should_exit) {
printf("Hello Deamon!\n"); printf("Hello Deamon!\n");
sleep(10); sleep(10);
if (thread_should_exit) break;
} }
printf("[deamon] exiting.\n"); printf("[deamon] exiting.\n");
apps/mavlink/mavlink.c
+13 -34
View File
@@ -61,7 +61,6 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_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 sensor_combined_s raw;
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
struct vehicle_gps_position_s gps; struct vehicle_gps_position_s gps;
struct ardrone_control_s ar_control;
struct vehicle_local_position_setpoint_s local_sp; struct vehicle_local_position_setpoint_s local_sp;
struct vehicle_global_position_setpoint_s global_sp; struct vehicle_global_position_setpoint_s global_sp;
struct vehicle_attitude_setpoint_s att_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); orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
/* send raw imu data */ /* 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]); mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0],
/* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */ buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0],
//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); buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0],
/* send scaled imu data */ buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
/* mark individual fields as changed */ /* mark individual fields as changed */
uint16_t fields_updated = 0; uint16_t fields_updated = 0;
@@ -847,26 +845,6 @@ static void *uorb_receiveloop(void *arg)
gps_counter++; 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 --- */ /* --- SYSTEM STATUS --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
/* immediately communicate state changes back to user */ /* immediately communicate state changes back to user */
@@ -1539,18 +1517,18 @@ int mavlink_thread_main(int argc, char *argv[])
if (baudrate >= 921600) { if (baudrate >= 921600) {
/* 200 Hz / 5 ms */ /* 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_HIGHRES_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
/* 200 Hz / 5 ms */ /* 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_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_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); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 5 Hz */ /* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 460800) { } else if (baudrate >= 460800) {
/* 200 Hz / 5 ms */ /* 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_HIGHRES_IMU, 5);
/* 50 Hz / 10 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20); /* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
/* 20 Hz / 50 ms */ /* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); 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); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 115200) { } else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */ /* 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_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
/* 20 Hz / 50 ms */ /* 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); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 10 Hz / 100 ms */ /* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100); 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); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) { } else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */ /* 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_RAW_IMU, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_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 */ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */ /* 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); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
} else { } else {
/* very low baud rate, limit to 1 Hz / 1000 ms */ /* 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_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 1 Hz / 1000 ms */ /* 1 Hz / 1000 ms */
apps/multirotor_att_control/multirotor_att_control_main.c
-1
View File
@@ -57,7 +57,6 @@
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h> #include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
apps/multirotor_att_control/multirotor_attitude_control.h
-3
View File
@@ -46,11 +46,8 @@
#define MULTIROTOR_ATTITUDE_CONTROL_H_ #define MULTIROTOR_ATTITUDE_CONTROL_H_
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.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> #include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
apps/sdlog/Makefile
+1 -3
View File
@@ -37,8 +37,6 @@
APPNAME = sdlog APPNAME = sdlog
PRIORITY = SCHED_PRIORITY_DEFAULT - 1 PRIORITY = SCHED_PRIORITY_DEFAULT - 1
STACKSIZE = 3000 STACKSIZE = 2048
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk include $(APPDIR)/mk/app.mk
apps/sdlog/mfile.template
-136
View File
@@ -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
apps/sdlog/sdlog.c
+247 -326
View File
File diff suppressed because it is too large Load Diff
apps/sdlog/sdlog.h
-17
View File
@@ -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_ */
apps/sdlog/sdlog_generated.h
-238
View File
@@ -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
apps/sdlog/updatesdlog.py
-199
View File
@@ -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!'
nuttx/configs/px4fmu/nsh/appconfig
+1 -1
View File
@@ -69,7 +69,7 @@ CONFIGURED_APPS += uORB
CONFIGURED_APPS += mavlink CONFIGURED_APPS += mavlink
CONFIGURED_APPS += gps CONFIGURED_APPS += gps
CONFIGURED_APPS += commander CONFIGURED_APPS += commander
#CONFIGURED_APPS += sdlog CONFIGURED_APPS += sdlog
CONFIGURED_APPS += sensors CONFIGURED_APPS += sensors
CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_att_control