diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 448fbd8c2..3fc3fd6d5 100644 --- a/flight/AHRS/ahrs.c +++ b/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_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(); diff --git a/ground/src/experimental/SerialLogger/analyzeINSGPS.m b/ground/src/experimental/SerialLogger/analyzeINSGPS.m new file mode 100644 index 000000000..943157f9b --- /dev/null +++ b/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'); \ No newline at end of file diff --git a/ground/src/experimental/SerialLogger/read.m b/ground/src/experimental/SerialLogger/analyzeRaw.m similarity index 98% rename from ground/src/experimental/SerialLogger/read.m rename to ground/src/experimental/SerialLogger/analyzeRaw.m index a0c04e646..05a17e01a 100644 --- a/ground/src/experimental/SerialLogger/read.m +++ b/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 diff --git a/ground/src/experimental/SerialLogger/main.cpp b/ground/src/experimental/SerialLogger/main.cpp index 5f580d213..03c91dfed 100644 --- a/ground/src/experimental/SerialLogger/main.cpp +++ b/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;