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.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1560 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/AHRS/ahrs.c

@@ -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_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks);
-        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_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_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_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_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, "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();

ground/src/experimental/SerialLogger/analyzeINSGPS.m

@@ -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');

ground/src/experimental/SerialLogger/analyzeRaw.m

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

ground/src/experimental/SerialLogger/main.cpp

@@ -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;