From 0ee48db90fd1df3bdc8863246057500e3f86562e Mon Sep 17 00:00:00 2001 From: daregger Date: Tue, 6 Nov 2012 11:25:31 +0100 Subject: [PATCH] add attitude + rotation Matrix to logging --- ROMFS/logging/logconv.m | 372 ++++++++++++++++++++++++---------------- apps/sdlog/sdlog.c | 39 +++-- 2 files changed, 243 insertions(+), 168 deletions(-) diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 579a582d3e..88bcfec96a 100644 --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -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 \ No newline at end of file + 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 + + diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 7d2f6afba3..cf6edcbe62 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -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; } +