forked from Archive/PX4-Autopilot
238 lines
12 KiB
C
238 lines
12 KiB
C
/* 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 |