mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-30 04:06:33 +08:00
Merge branch 'master' of github.com:pixhawk/Firmware
This commit is contained in:
Lorenz Meier
+212
-142
@@ -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
|
||||||
|
|
||||||
|
%myPath = '..\LOG30102012\session0002\'; %set relative path here
|
||||||
|
myPath = '.\';
|
||||||
|
myFile = 'sysvector.bin';
|
||||||
|
filePath = strcat(myPath,myFile);
|
||||||
|
|
||||||
if exist('sysvector.bin', 'file')
|
if exist(filePath, 'file')
|
||||||
% Read actuators file
|
fileInfo = dir(filePath);
|
||||||
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;
|
fileSize = fileInfo.bytes;
|
||||||
|
|
||||||
fid = fopen('sensor_combined.bin', 'r');
|
fid = fopen(filePath, 'r');
|
||||||
|
elements = int64(fileSize./(16*4+8))/4
|
||||||
|
|
||||||
for i=1:elements
|
for i=1:elements
|
||||||
% timestamp
|
% timestamp
|
||||||
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
|
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
|
% gyro (3 channels)
|
||||||
sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
|
sensors(i,2:4) = fread(fid, 3, 'float', 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
|
% accelerometer (3 channels)
|
||||||
sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
|
sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||||
% padding bytes
|
|
||||||
fread(fid, 1, 'int16', 0, 'ieee-le');
|
% mag (3 channels)
|
||||||
% mag in Gauss
|
sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
|
||||||
sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
|
|
||||||
% mag mode
|
% baro pressure
|
||||||
sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
|
sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
|
||||||
% mag range
|
|
||||||
sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
|
% baro alt
|
||||||
% mag cuttoff freq
|
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');
|
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)
|
% adc voltage (3 channels)
|
||||||
sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
|
sensors(i,27:29) = fread(fid, 3, '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');
|
|
||||||
|
|
||||||
|
% 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
|
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
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
+22
-17
@@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user