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 - fixed time constant of baro altitude offset filter, added the offset to the EKF dump

Browse Source 
Added new EKF data parser (m-file) to ground



git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2178 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
dschin 2010-11-29 23:28:38 +00:00 committed by dschin
parent a4ba7fcbee
commit 18809ce3dc
2 changed files with 84 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/AHRS/ahrs.c
View File

@ -43,7 +43,7 @@
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */
#define INSGPS_MAGLEN 1000
#define INSGPS_MAGLEN 1000
#define INSGPS_MAGTOL 0.5 /* error in magnetic vector length to use */
// For debugging the raw sensors
@ -104,6 +104,9 @@ struct gps_sensor gps_data;
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 20;
//! Offset correction of barometric alt, to match gps data
static float baro_offset = 0;
/**
* @}
*/
@ -117,7 +120,6 @@ void ins_outdoor_update()
{
float gyro[3], accel[3], vel[3];
static uint32_t last_gps_time = 0;
static float baro_offset = 0;
uint16_t sensors;
// format data for INS algo
@ -174,7 +176,7 @@ void ins_outdoor_update()
baro_offset = gps_data.NED[2] + altitude_data.altitude;
} else {
/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
baro_offset = baro_offset * 0.99 + (gps_data.NED[2] + altitude_data.altitude) * 0.01;
baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
}
gps_data.updated = false;
} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
@ -376,6 +378,7 @@ void print_ekf_binary()
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (138:189)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (190:193)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (194:198)
}
#else
void print_ekf_binary() {}

78
ground/src/experimental/SerialLogger/ParseEKFLog.m Normal file
View File

@ -0,0 +1,78 @@
function ParseEKFLog(fn)
fid = fopen(fn);
dat = uint8(fread(fid,inf,'uchar'));
fclose(fid);
mag_data_size = 1 + 4*3;
accel_sensor_size = 4*3;
gyro_sensor_size = 4*3;
raw_framing = 15:-1:0;
starts = strfind(char(dat'),char(raw_framing));
starts(end) = [];
counts = typecast(reshape(dat(bsxfun(@plus,starts,(16:19)')),1,[]),'int32');
accel = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(20:31)')),1,[]),'single'),3,[])';
gyro = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(32:43)')),1,[]),'single'),3,[])';
mag_updated = dat(starts+44);
mag = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(45:56)')),1,[]),'single'),3,[])';
%gps = 28 bytes 57:84
NED = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(57:68)')),1,[]),'single'),3,[])';
heading = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(69:72)')),1,[]),'single'),1,[])';
groundspeed = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(73:76)')),1,[]),'single'),1,[])';
quality = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(77:80)')),1,[]),'single'),1,[])';
updated = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(81:84)')),1,[]),'int32'),1,[])';
X = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(85:85+13*4-1)')),1,[]),'single'),13,[])';
P = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(137:137+13*4-1)')),1,[]),'single'),13,[])';
Baro = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(189:192)')),1,[]),'single'),1,[])';
BaroOffset = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(193:196)')),1,[]),'single'),1,[])';
N=length(accel);
rpy = zeros(3,N);
Vne = zeros(2,N);
dt = 0.01;
t=0:dt:(N-1)*dt;
for i = 1:N
rpy(:,i) = q2rpy(X(i,7:10))*180/pi;
groundTrack = heading(i)*pi/180;
Vne(:,i) = groundspeed(i)*[cos(groundTrack); sin(groundTrack)];
end
figure(1); plot(t,rpy(1,:),t,rpy(2,:),t,rpy(3,:));
title('roll-pitch-yaw');
figure(2); plot(t,NED(:,1),t,NED(:,2),t,NED(:,3),t,X(:,1),t,X(:,2),t,X(:,3),t,BaroOffset-Baro);
title('gpsNED');ylim([-500 500]);
figure(3); plot(t,Vne(1,:),t,Vne(2,:),t,X(:,4),t,X(:,5));
title('gpsVne');ylim([-3 3]);
figure(4); plot(t,accel(:,1),t,accel(:,2),t,accel(:,3));
title('accels');ylim([-10 10]);
figure(5); plot(t,gyro(:,1),t,gyro(:,2),t,gyro(:,3));
title('gyros'); ylim([-pi pi]);
figure(6); plot(t,mag(:,1),t,mag(:,2),t,mag(:,3));
title('mags'); ylim([-1200 1200]);
figure(7); plot(t,Baro,t,BaroOffset,t,BaroOffset-NED(:,3));
title('BaroAlt'); ylim([300 410]);
figure(8); plot(t,X(:,11),t,X(:,12),t,X(:,13));
title('GyroBias'); ylim([-1 1]);
Be = [20595 1363 49068]';
ScaledBe = [Be(1) Be(2) Be(3)]'/Be(3);
ScaledBe = [ScaledBe ScaledBe];
ScaledMag = zeros(3,N);
MagMag = zeros(1,N);
for i = 1:N
ScaledMag(:,i) = mag(i,:)'/mag(i,3);
MagMag(i) = norm(mag(i,:));
end
t2 = [t(1) t(end)];
figure(13); plot(t,ScaledMag(1,:),t,ScaledMag(2,:),t,ScaledMag(3,:),t2,ScaledBe(1,:),t2,ScaledBe(2,:),t2,ScaledBe(3,:));
title('scaledMags'); ylim([-1.01 1.01]);
figure(14); plot(t,MagMag);
title('Mag Magnitude'); ylim([500 1500]);
keyboard