forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:pixhawk/Firmware
This commit is contained in:
Lorenz Meier
commit
4338dd1e48
|
|
@ -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
|
||||
% //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('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');
|
||||
if exist(filePath, 'file')
|
||||
fileInfo = dir(filePath);
|
||||
fileSize = fileInfo.bytes;
|
||||
|
||||
fid = fopen('sensor_combined.bin', 'r');
|
||||
|
||||
for i=1:elements
|
||||
|
||||
fid = fopen(filePath, 'r');
|
||||
elements = int64(fileSize./(16*4+8))/4
|
||||
|
||||
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
|
||||
|
||||
% gyro (3 channels)
|
||||
sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||
|
||||
% accelerometer (3 channels)
|
||||
sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||
|
||||
% mag (3 channels)
|
||||
sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||
|
||||
% baro pressure
|
||||
sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
|
||||
|
||||
% baro alt
|
||||
sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
|
||||
|
||||
% baro temp
|
||||
sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
|
||||
|
||||
% actuator control (4 channels)
|
||||
sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
|
||||
|
||||
% actuator outputs (8 channels)
|
||||
sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
|
||||
|
||||
% vbat
|
||||
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');
|
||||
|
||||
sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||
|
||||
% local position (3 channels)
|
||||
sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||
|
||||
% gps_raw_position (3 channels)
|
||||
sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
|
||||
|
||||
% attitude (3 channels)
|
||||
sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||
|
||||
% RotMatrix (9 channels)
|
||||
sensors(i,39:47) = fread(fid, 9, 'float', 0, '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
|
||||
|
||||
|
||||
%% 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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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[]) {
|
|||
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)
|
|||
return ret;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue