mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
AHRS Debugign code for dumping the EKF and importing into Matlab
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2013 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
2019edd612
commit
4d7e053e06
@ -523,50 +523,22 @@ for all data to be up to date before doing anything*/
|
||||
(int16_t) (Nav.q[3] * 1000));
|
||||
#endif
|
||||
#ifdef DUMP_EKF
|
||||
uint8_t framing[16] =
|
||||
{ 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
|
||||
0 };
|
||||
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
|
||||
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
|
||||
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
extern float K[NUMX][NUMV]; // feedback gain matrix
|
||||
|
||||
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
|
||||
// Dump raw buffer
|
||||
int8_t result;
|
||||
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
|
||||
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX,
|
||||
(uint8_t *) & mag_data,
|
||||
sizeof(mag_data));
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX,
|
||||
(uint8_t *) & gps_data,
|
||||
sizeof(gps_data));
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX,
|
||||
(uint8_t *) & accel_data,
|
||||
sizeof(accel_data));
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX,
|
||||
(uint8_t *) & gyro_data,
|
||||
sizeof(gyro_data));
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Q,
|
||||
sizeof(float) * NUMX * NUMX);
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & K,
|
||||
sizeof(float) * NUMX * NUMV);
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X,
|
||||
sizeof(float) * NUMX * NUMX);
|
||||
|
||||
if (result == 0)
|
||||
PIOS_LED_Off(LED1);
|
||||
else {
|
||||
PIOS_LED_On(LED1);
|
||||
}
|
||||
#endif
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20)
|
||||
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32)
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:82)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (83:134)
|
||||
for(uint8_t i = 0; i < NUMX; i++)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (135:186)
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
22
ground/src/experimental/SerialLogger/analyzeEKF.m
Normal file
22
ground/src/experimental/SerialLogger/analyzeEKF.m
Normal file
@ -0,0 +1,22 @@
|
||||
function analyzeEKF(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
|
||||
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,[])';
|
Loading…
x
Reference in New Issue
Block a user