rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

AHRS: Improved code to stream raw data from AHRS and added a matlab function to acquire and analyze the data. The QT program to analyze it should work but QextSerial seems to have a bug and drops or inserts data

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1559 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-08 06:15:18 +00:00 committed by peabody124
parent b53f19dc03
commit e499f33fd5
4 changed files with 111 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flight/AHRS/ahrs.c
View File

@ -279,12 +279,19 @@ int main()
}
#ifdef DUMP_RAW
int previous_conversion;
while(1) {
int result;
uint8_t framing[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10};
uint8_t framing[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
while( ahrs_state != AHRS_DATA_READY );
ahrs_state = AHRS_PROCESSING;
if(total_conversion_blocks != previous_conversion+1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;

2
flight/AHRS/pios_board.c
View File

@ -157,7 +157,7 @@ void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,

13
ground/src/experimental/SerialLogger/main.cpp
View File

@ -18,15 +18,16 @@ public:
void run()
{
QByteArray dat;
const char framingRaw[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10};
//const char framingRaw[16] = {7,9,3,15,193,130,150,10,7,9,3,15,193,130,150,10};
const char framingRaw[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
QByteArray framing(framingRaw,16);
PortSettings Settings;
Settings.BaudRate=BAUD57600;
Settings.BaudRate=BAUD9600;
Settings.DataBits=DATA_8;
Settings.Parity=PAR_NONE;
Settings.StopBits=STOP_1;
Settings.FlowControl=FLOW_HARDWARE;
Settings.FlowControl=FLOW_OFF;
Settings.Timeout_Millisec=500;
QextSerialPort serialPort(device, Settings);
@ -43,17 +44,19 @@ public:
while(1)
{
dat = serialPort.read(1000);
dat = serialPort.read(500);
if(dat.contains(framing))
{
int start = dat.indexOf(framing);
int count = *((int *) (dat.data() + start+16));
qDebug() << "Found frame start at " << start << " count " << count;
}
else if (dat.size() == 0)
qDebug() << "No data";
else
qDebug() << "No frame start";
ts << dat;
usleep(50000);
usleep(100000);
}
};

93
ground/src/experimental/SerialLogger/read.m Normal file
View File

@ -0,0 +1,93 @@
function read(device)
downsample = 12; % relevant for knowing block size
Fs = 512; % need to verify, close
s = serial(device,'Baud',115200);
set(s,'InputBufferSize',10000);
fopen(s);
dat = [];
for i = 1:20
i
if(i > 5) % must flush buffer
dat = [dat; uint8(fread(s))];
end
end
raw_framing = 0:15;
starts = strfind(char(dat'),char(raw_framing));
if(starts) % found raw data, process
% warning - could occasionally crash if at very last 3 or less bytes
counts = typecast(reshape(dat(bsxfun(@plus,starts,(16:19)')),1,[]),'int32');
gaps = find(diff(counts) > 1) % exclude any discontiuous sections
if(gaps)
starts(1:gaps(end)) = [];
end
blocks = typecast(reshape(dat(bsxfun(@plus,starts(1:end-1),(20:20+downsample*8*2-1)')),1,[]),'int16');
blocks = double(reshape(blocks,8,[]));
accel_y = 0.012*(blocks(1,:)-2048);
accel_x = 0.012*(blocks(3,:)-2048);
accel_z = 0.012*(blocks(5,:)-2048);
gyro_x = 0.007*(blocks(2,:)-1675);
gyro_y = 0.007*(blocks(4,:)-1675);
gyro_z = 0.007*(blocks(6,:)-1675);
time = (1:length(accel_x))/512;
% display accels
figure(1)
subplot(321);
plot(time,accel_x);
xlim([0 1])
ylim([mean(accel_x)-2*std(accel_x) mean(accel_x)+4*std(accel_x)]);
title('Accel X');
subplot(322);
pwelch(accel_x-mean(accel_x),[],[],[],Fs);
subplot(323);
plot(time,accel_y);
xlim([0 1])
ylim([mean(accel_y)-2*std(accel_y) mean(accel_y)+4*std(accel_y)]);
title('Accel Y');
subplot(324);
pwelch(accel_y-mean(accel_y),[],[],[],Fs);
subplot(325);
plot(time,accel_z);
xlim([0 1])
ylim([mean(accel_z)-2*std(accel_z) mean(accel_z)+4*std(accel_z)]);
title('Accel Z');
subplot(326);
pwelch(accel_z-mean(accel_z),[],[],[],Fs);
% display gyros
figure(2)
subplot(321);
plot(time,gyro_x);
xlim([0 1])
ylim([mean(gyro_x)-2*std(gyro_x) mean(gyro_x)+4*std(gyro_x)]);
title('Gyro X');
subplot(322);
pwelch(gyro_x-mean(gyro_x),[],[],[],Fs);
subplot(323);
plot(time,gyro_y);
xlim([0 1])
ylim([mean(gyro_y)-2*std(gyro_y) mean(gyro_y)+4*std(gyro_y)]);
title('Gyro Y');
subplot(324);
pwelch(gyro_y-mean(gyro_y),[],[],[],Fs);
subplot(325);
plot(time,gyro_z);
xlim([0 1])
ylim([mean(gyro_z)-2*std(gyro_z) mean(gyro_z)+4*std(gyro_z)]);
title('Gyro Z');
subplot(326);
pwelch(gyro_z-mean(gyro_z),[],[],[],Fs);
end