From 4d7e053e065921d68c751f083e359f0439e6a36f Mon Sep 17 00:00:00 2001
From: peabody124 <peabody124>
Date: Wed, 27 Oct 2010 14:31:34 +0000
Subject: [PATCH] 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
---
 flight/AHRS/ahrs.c                            | 58 +++++--------------
 .../experimental/SerialLogger/analyzeEKF.m    | 22 +++++++
 2 files changed, 37 insertions(+), 43 deletions(-)
 create mode 100644 ground/src/experimental/SerialLogger/analyzeEKF.m

diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 1cda21991..453aa1818 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -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  
 
 	}
 
diff --git a/ground/src/experimental/SerialLogger/analyzeEKF.m b/ground/src/experimental/SerialLogger/analyzeEKF.m
new file mode 100644
index 000000000..555ad1254
--- /dev/null
+++ b/ground/src/experimental/SerialLogger/analyzeEKF.m
@@ -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,[])';
\ No newline at end of file