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
Unified 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
%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%
% SYSTEM VECTOR % SYSTEM VECTOR
% %
% All measurements in NED frame % //All measurements in NED frame
% %
% uint64_t timestamp; % uint64_t timestamp; //[us]
% float gyro[3]; in rad/s % float gyro[3]; //[rad/s]
% float accel[3]; in m/s^2 % float accel[3]; //[m/s^2]
% float mag[3]; in Gauss % float mag[3]; //[gauss]
% float baro; pressure in millibar % float baro; //pressure [millibar]
% float baro_alt; altitude above MSL in meters % float baro_alt; //altitude above MSL [meter]
% float baro_temp; in degrees celcius % float baro_temp; //[degree celcius]
% float control[4]; roll, pitch, yaw [-1..1], thrust [0..1] % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000, % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
% AR.Drone: 0-512 % float vbat; //battery voltage in [volt]
% float vbat; battery voltage in volt % float adc[3]; //remaining auxiliary ADC ports [volt]
% float adc[3]; remaining auxiliary ADC ports in volt % float local_position[3]; //tangent plane mapping into x,y,z [m]
% float local_position % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
% int32 gps_raw_position % float attitude[3]; //pitch, roll, yaw [rad]
% float rotMatrix[9]; //unitvectors
if exist('sysvector.bin', 'file') %myPath = '..\LOG30102012\session0002\'; %set relative path here
% Read actuators file myPath = '.\';
myFile = java.io.File('sysvector.bin') myFile = 'sysvector.bin';
fileSize = length(myFile) filePath = strcat(myPath,myFile);
fid = fopen('sysvector.bin', 'r'); if exist(filePath, 'file')
elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4)); fileInfo = dir(filePath);
fileSize = fileInfo.bytes;
fid = fopen(filePath, 'r');
elements = int64(fileSize./(16*4+8))/4
for i=1:elements for i=1:elements
% timestamp % timestamp
sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64')); sensors(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 % gyro (3 channels)
sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000; 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 % mag (3 channels)
gps = sysvector(:,33:35); sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
gps(~any(gps,2), :) = [];
all_data = figure('Name', 'GPS RAW'); % baro pressure
gps_position = plot3(gps(:,1), gps(:,2), gps(:,3)); 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)'); % baro temp
plot(sysvector(:,1), sysvector(:,2:32)); sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
actuator_inputs = figure('Name', 'Attitude controller outputs'); % actuator control (4 channels)
plot(sysvector(:,1), sysvector(:,14:17)); sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
actuator_outputs = figure('Name', 'Actuator outputs'); % actuator outputs (8 channels)
plot(sysvector(:,1), sysvector(:,18:25)); sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
end % vbat
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
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'); % adc voltage (3 channels)
elements = int64(fileSize./(16*4+8)) sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
for i=1:elements % local position (3 channels)
% timestamp sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
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') % gps_raw_position (3 channels)
% Read actuators file sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
myFile = java.io.File('actuator_controls0.bin')
fileSize = length(myFile)
fid = fopen('actuator_controls0.bin', 'r'); % attitude (3 channels)
elements = int64(fileSize./(8*4+8)) sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
for i=1:elements % RotMatrix (9 channels)
% timestamp sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
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
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 end
if exist('sensor_combined.bin', 'file') %% old version of reading in different files from sdlog.c
% Read sensor combined file % if exist('sysvector.bin', 'file')
% Type definition: Firmware/apps/uORB/topics/sensor_combined.h % % Read actuators file
% Struct: sensor_combined_s % myFile = java.io.File('sysvector.bin')
fileInfo = dir('sensor_combined.bin'); % fileSize = length(myFile)
fileSize = fileInfo.bytes; %
% fid = fopen('sysvector.bin', 'r');
fid = fopen('sensor_combined.bin', 'r'); % elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
%
for i=1:elements % for i=1:elements
% timestamp % % timestamp
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64')); % sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% gyro raw % % actuators 1-16
sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le'); % % quadrotor: motor 1-4 on the first four positions
% gyro counter % sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le'); % sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
% gyro in rad/s % end
sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le'); %
% sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
% accelerometer raw % sysvector_minutes = sysvector_interval_seconds / 60
sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le'); %
% padding bytes % % Normalize time
fread(fid, 1, 'int16', 0, 'ieee-le'); % sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
% accelerometer counter %
sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le'); % % Create some basic plots
% accel in m/s2 %
sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le'); % % Remove zero rows from GPS
% accel mode % gps = sysvector(:,33:35);
sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le'); % gps(~any(gps,2), :) = [];
% accel range %
sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le'); % all_data = figure('Name', 'GPS RAW');
% gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
% mag raw %
sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le'); %
% padding bytes % all_data = figure('Name', 'Complete Log Data (exc. GPS)');
fread(fid, 1, 'int16', 0, 'ieee-le'); % plot(sysvector(:,1), sysvector(:,2:32));
% mag in Gauss %
sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le'); % actuator_inputs = figure('Name', 'Attitude controller outputs');
% mag mode % plot(sysvector(:,1), sysvector(:,14:17));
sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le'); % legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
% mag range %
sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le'); % actuator_outputs = figure('Name', 'Actuator outputs');
% mag cuttoff freq % plot(sysvector(:,1), sysvector(:,18:25));
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le'); % legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
% mag counter %
sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le'); % 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[]) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); 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_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_global_position), subs.global_pos_sub, &buf.global_pos);
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
#pragma pack(push, 1) #pragma pack(push, 1)
struct { struct {
uint64_t timestamp; uint64_t timestamp; //[us]
float gyro[3]; float gyro[3]; //[rad/s]
float accel[3]; float accel[3]; //[m/s^2]
float mag[3]; float mag[3]; //[gauss]
float baro; float baro; //pressure [millibar]
float baro_alt; float baro_alt; //altitude above MSL [meter]
float baro_temp; float baro_temp; //[degree celcius]
float control[4]; 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 actuators[8]; float vbat; //battery voltage in [volt]
float vbat; float adc[3]; //remaining auxiliary ADC ports [volt]
float adc[3]; float local_position[3]; //tangent plane mapping into x,y,z [m]
float local_pos[3]; int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
int32_t gps_pos[3]; float attitude[3]; //pitch, roll, yaw [rad]
float rotMatrix[9]; //unitvectors
} sysvector = { } sysvector = {
.timestamp = buf.raw.timestamp, .timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, .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[]) {
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, 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, .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]}, .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}, .local_position = {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} .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) #pragma pack(pop)
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); 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); fsync(sysvector_file);
@ -602,3 +606,4 @@ int file_copy(const char* file_old, const char* file_new)
return ret; return ret;
} }

Write Preview
Loading…
Cancel
Save