zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge branch 'master' of github.com:pixhawk/Firmware

sbg
Lorenz Meier 12 years ago
parent
0245d7be86 847ff7c5e2
commit
4338dd1e48
2 changed files with 242 additions and 167 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 342
      ROMFS/logging/logconv.m
  2. 39
      apps/sdlog/sdlog.c

342
ROMFS/logging/logconv.m
Unescape View File

@ -4,162 +4,232 @@ close all @@ -4,162 +4,232 @@ close all
%%%%%%%%%%%%%%%%%%%%%%%
% SYSTEM VECTOR
%
% All measurements in NED frame
%
% uint64_t timestamp;
% float gyro[3]; in rad/s
% float accel[3]; in m/s^2
% float mag[3]; in Gauss
% float baro; pressure in millibar
% float baro_alt; altitude above MSL in meters
% float baro_temp; in degrees celcius
% float control[4]; roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000,
% AR.Drone: 0-512
% float vbat; battery voltage in volt
% float adc[3]; remaining auxiliary ADC ports in volt
% float local_position
% int32 gps_raw_position
if exist('sysvector.bin', 'file')
% Read actuators file
myFile = java.io.File('sysvector.bin')
fileSize = length(myFile)
fid = fopen('sysvector.bin', 'r');
elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
% //All measurements in NED frame
%
% uint64_t timestamp; //[us]
% float gyro[3]; //[rad/s]
% float accel[3]; //[m/s^2]
% float mag[3]; //[gauss]
% float baro; //pressure [millibar]
% float baro_alt; //altitude above MSL [meter]
% float baro_temp; //[degree celcius]
% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
% float vbat; //battery voltage in [volt]
% float adc[3]; //remaining auxiliary ADC ports [volt]
% float local_position[3]; //tangent plane mapping into x,y,z [m]
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
% float attitude[3]; //pitch, roll, yaw [rad]
% float rotMatrix[9]; //unitvectors
%myPath = '..\LOG30102012\session0002\'; %set relative path here
myPath = '.\';
myFile = 'sysvector.bin';
filePath = strcat(myPath,myFile);
if exist(filePath, 'file')
fileInfo = dir(filePath);
fileSize = fileInfo.bytes;
fid = fopen(filePath, 'r');
elements = int64(fileSize./(16*4+8))/4
for i=1:elements
% timestamp
sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
end
sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
sysvector_minutes = sysvector_interval_seconds / 60
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% Normalize time
sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
% gyro (3 channels)
sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
% Create some basic plots
% accelerometer (3 channels)
sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
% Remove zero rows from GPS
gps = sysvector(:,33:35);
gps(~any(gps,2), :) = [];
% mag (3 channels)
sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
all_data = figure('Name', 'GPS RAW');
gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
% baro pressure
sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
% baro alt
sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
all_data = figure('Name', 'Complete Log Data (exc. GPS)');
plot(sysvector(:,1), sysvector(:,2:32));
% baro temp
sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
actuator_inputs = figure('Name', 'Attitude controller outputs');
plot(sysvector(:,1), sysvector(:,14:17));
legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
% actuator control (4 channels)
sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
actuator_outputs = figure('Name', 'Actuator outputs');
plot(sysvector(:,1), sysvector(:,18:25));
legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
% actuator outputs (8 channels)
sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
end
if exist('actuator_outputs0.bin', 'file')
% Read actuators file
myFile = java.io.File('actuator_outputs0.bin')
fileSize = length(myFile)
% vbat
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
fid = fopen('actuator_outputs0.bin', 'r');
elements = int64(fileSize./(16*4+8))
% adc voltage (3 channels)
sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
for i=1:elements
% timestamp
actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
end
end
% local position (3 channels)
sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
if exist('actuator_controls0.bin', 'file')
% Read actuators file
myFile = java.io.File('actuator_controls0.bin')
fileSize = length(myFile)
% gps_raw_position (3 channels)
sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
fid = fopen('actuator_controls0.bin', 'r');
elements = int64(fileSize./(8*4+8))
% attitude (3 channels)
sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
for i=1:elements
% timestamp
actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
% RotMatrix (9 channels)
sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
end
time_us = sensors(elements,1) - sensors(1,1);
time_s = time_us*1e-6
time_m = time_s/60
disp(['end log2matlab conversion' char(10)]);
else
disp(['file: ' filePath ' does not exist' char(10)]);
end
if exist('sensor_combined.bin', 'file')
% Read sensor combined file
% Type definition: Firmware/apps/uORB/topics/sensor_combined.h
% Struct: sensor_combined_s
fileInfo = dir('sensor_combined.bin');
fileSize = fileInfo.bytes;
fid = fopen('sensor_combined.bin', 'r');
for i=1:elements
% timestamp
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% gyro raw
sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
% gyro counter
sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
% gyro in rad/s
sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
% accelerometer raw
sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
% padding bytes
fread(fid, 1, 'int16', 0, 'ieee-le');
% accelerometer counter
sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
% accel in m/s2
sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
% accel mode
sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
% accel range
sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag raw
sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
% padding bytes
fread(fid, 1, 'int16', 0, 'ieee-le');
% mag in Gauss
sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
% mag mode
sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
% mag range
sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag cuttoff freq
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag counter
sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
%% old version of reading in different files from sdlog.c
% if exist('sysvector.bin', 'file')
% % Read actuators file
% myFile = java.io.File('sysvector.bin')
% fileSize = length(myFile)
%
% fid = fopen('sysvector.bin', 'r');
% elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
%
% for i=1:elements
% % timestamp
% sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
% sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
% end
%
% sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
% sysvector_minutes = sysvector_interval_seconds / 60
%
% % Normalize time
% sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
%
% % Create some basic plots
%
% % Remove zero rows from GPS
% gps = sysvector(:,33:35);
% gps(~any(gps,2), :) = [];
%
% all_data = figure('Name', 'GPS RAW');
% gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
%
%
% all_data = figure('Name', 'Complete Log Data (exc. GPS)');
% plot(sysvector(:,1), sysvector(:,2:32));
%
% actuator_inputs = figure('Name', 'Attitude controller outputs');
% plot(sysvector(:,1), sysvector(:,14:17));
% legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
%
% actuator_outputs = figure('Name', 'Actuator outputs');
% plot(sysvector(:,1), sysvector(:,18:25));
% legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
%
% end
%
% if exist('actuator_outputs0.bin', 'file')
% % Read actuators file
% myFile = java.io.File('actuator_outputs0.bin')
% fileSize = length(myFile)
%
% fid = fopen('actuator_outputs0.bin', 'r');
% elements = int64(fileSize./(16*4+8))
%
% for i=1:elements
% % timestamp
% actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
% end
% end
%
% if exist('actuator_controls0.bin', 'file')
% % Read actuators file
% myFile = java.io.File('actuator_controls0.bin')
% fileSize = length(myFile)
%
% fid = fopen('actuator_controls0.bin', 'r');
% elements = int64(fileSize./(8*4+8))
%
% for i=1:elements
% % timestamp
% actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
% end
% end
%
%
% if exist('sensor_combined.bin', 'file')
% % Read sensor combined file
% % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
% % Struct: sensor_combined_s
% fileInfo = dir('sensor_combined.bin');
% fileSize = fileInfo.bytes;
%
% fid = fopen('sensor_combined.bin', 'r');
%
% for i=1:elements
% % timestamp
% sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % gyro raw
% sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % gyro counter
% sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
% % gyro in rad/s
% sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
%
% % accelerometer raw
% sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % padding bytes
% fread(fid, 1, 'int16', 0, 'ieee-le');
% % accelerometer counter
% sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
% % accel in m/s2
% sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
% % accel mode
% sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
% % accel range
% sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
%
% % mag raw
% sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % padding bytes
% fread(fid, 1, 'int16', 0, 'ieee-le');
% % mag in Gauss
% sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
% % mag mode
% sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
% % mag range
% sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
% % mag cuttoff freq
% sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% % mag counter
% sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
%
% % baro pressure millibar
% % baro alt meter
% % baro temp celcius
% % battery voltage
% % adc voltage (3 channels)
% sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
% % baro counter and battery counter
% sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
% % battery voltage valid flag
% sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
%
% end
% end
% baro pressure millibar
% baro alt meter
% baro temp celcius
% battery voltage
% adc voltage (3 channels)
sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
% baro counter and battery counter
sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
% battery voltage valid flag
sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
end
end

39
apps/sdlog/sdlog.c
Unescape View File

@ -488,23 +488,25 @@ int sdlog_thread_main(int argc, char *argv[]) { @@ -488,23 +488,25 @@ int sdlog_thread_main(int argc, char *argv[]) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
#pragma pack(push, 1)
struct {
uint64_t timestamp;
float gyro[3];
float accel[3];
float mag[3];
float baro;
float baro_alt;
float baro_temp;
float control[4];
float actuators[8];
float vbat;
float adc[3];
float local_pos[3];
int32_t gps_pos[3];
uint64_t timestamp; //[us]
float gyro[3]; //[rad/s]
float accel[3]; //[m/s^2]
float mag[3]; //[gauss]
float baro; //pressure [millibar]
float baro_alt; //altitude above MSL [meter]
float baro_temp; //[degree celcius]
float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
float vbat; //battery voltage in [volt]
float adc[3]; //remaining auxiliary ADC ports [volt]
float local_position[3]; //tangent plane mapping into x,y,z [m]
int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
float attitude[3]; //pitch, roll, yaw [rad]
float rotMatrix[9]; //unitvectors
} sysvector = {
.timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@ -518,14 +520,16 @@ int sdlog_thread_main(int argc, char *argv[]) { @@ -518,14 +520,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
.vbat = buf.raw.battery_voltage_v,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
.rotMatrix = {buf.att.R[1][1], buf.att.R[1][2], buf.att.R[1][3], buf.att.R[2][1], buf.att.R[2][2], buf.att.R[2][3], buf.att.R[3][1], buf.att.R[3][2], buf.att.R[3][3]}
};
#pragma pack(pop)
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
usleep(10000);
usleep(10000); //10000 corresponds in reality to ca. 76 Hz
}
fsync(sysvector_file);
@ -602,3 +606,4 @@ int file_copy(const char* file_old, const char* file_new) @@ -602,3 +606,4 @@ int file_copy(const char* file_old, const char* file_new)
return ret;
}

Write Preview
Loading…
Cancel
Save