rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

AHRS: Added Matlab code to analyze the INSGPS friendly data dump format and plot the important variables. This requires "DUMP_FRIENDLY" be enabled in the file.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1560 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-08 06:15:21 +00:00 committed by peabody124
parent e499f33fd5
commit 6cdb9b0b48
4 changed files with 68 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/AHRS/ahrs.c
View File

@ -413,10 +413,11 @@ int main()
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "a: %d %d %d\r\n", (int16_t)(accel_data.filtered.x * 100), (int16_t)(accel_data.filtered.y * 100), (int16_t)(accel_data.filtered.z * 100));
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "g: %d %d %d\r\n", (int16_t)(gyro_data.filtered.x * 100), (int16_t)(gyro_data.filtered.y * 100), (int16_t)(gyro_data.filtered.z * 100));
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]);
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 100), (int16_t)(Nav.q[1] * 100), (int16_t)(Nav.q[2] * 100), (int16_t)(Nav.q[3] * 100));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "a: %d %d %d\r\n", (int16_t)(accel_data.filtered.x * 1000), (int16_t)(accel_data.filtered.y * 1000), (int16_t)(accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n", (int16_t)(gyro_data.filtered.x * 1000), (int16_t)(gyro_data.filtered.y * 1000), (int16_t)(gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 1000), (int16_t)(Nav.q[1] * 1000), (int16_t)(Nav.q[2] * 1000), (int16_t)(Nav.q[3] * 1000));
#endif
process_spi_request();

61
ground/src/experimental/SerialLogger/analyzeINSGPS.m Normal file
View File

@ -0,0 +1,61 @@
function [q gyro accel time] = analyzeINSGPS(fn)
% Analyzes data collected from SerialLogger while DUMP_FRIENDLY
% enabled in AHRS
%
% [q gyro accel time] = analyzeINSGPS(fn)
fid = fopen(fn);
i = 1;
data(i).block = -1;
tline = fgetl(fid);
while ischar(tline)
switch(tline(1))
case 'q'
c = textscan(tline,'q: %f %f %f %f');
data(i).q = [c{:}] / 1000;
case 'b'
c = textscan(tline,'b: %f');
i = i+1;
data(i).block = c{1};
case 'm'
c = textscan(tline,'m: %f %f %f');
data(i).mag = [c{:}];
case 'a'
c = textscan(tline,'a: %f %f %f %f');
data(i).accel = [c{:}] / 1000;
case 'g'
c = textscan(tline,'g: %f %f %f %f');
data(i).gyro = [c{:}] / 1000;
end
tline = fgetl(fid);
end
fclose(fid);
b = [data.block]; % get block counts
gaps = find(diff(b) ~= 1);
if(gaps) % get biggest contiguous chunk
lengths = diff(gaps);
[foo idx] = max(lengths);
idx = gaps(idx):gaps(idx+1);
data = data(idx);
end
q = cat(1,data.q);
accel = cat(1,data.accel);
gyro = cat(1,data.gyro);
time = (1:size(q,1)) / 50;
h(1) = subplot(311);
plot(time,q)
ylabel('Quaternion');
h(2) = subplot(312);
plot(time,accel);
ylabel('m/s')
h(3) = subplot(313);
plot(time,gyro);
ylabel('rad/sec');
xlabel('Time (s)')
linkaxes(h,'x');

2
ground/src/experimental/SerialLogger/read.m → ground/src/experimental/SerialLogger/analyzeRaw.m
View File

@ -1,4 +1,4 @@
function read(device)
function analyzeRaw(device)
downsample = 12; % relevant for knowing block size
Fs = 512; % need to verify, close

2
ground/src/experimental/SerialLogger/main.cpp
View File

@ -23,7 +23,7 @@ public:
QByteArray framing(framingRaw,16);
PortSettings Settings;
Settings.BaudRate=BAUD9600;
Settings.BaudRate=BAUD115200;
Settings.DataBits=DATA_8;
Settings.Parity=PAR_NONE;
Settings.StopBits=STOP_1;