/**
* \brief main function of the data acquisition system
* \author Jürgen Funck
* \date 2010-03-24
*/
int main(void) {
cli(); // disable interrupts globally
// set clock prescaler
CLKPR = (1<<CLKPCE);
CLKPR = 0;
_delay_ms(2);
// initialisation
DDRC |= (1<<PC2);
progState_t state = IDLE;
adcInit(); // adc
filterInit(); // filter
serialInit(BAUD_115200); // serial port
state = IDLE; // state
serialSendString("Hello World!\n");
// set sleep-mode
set_sleep_mode(SLEEP_MODE_IDLE);
sei(); // enable interrupts globally
// main-loop
int16_t val = 0;
while(1) {
// ACQUIRE
switch(state) {
case ANA_MEAS:
if(adcIsRunning() > 0) {
state = ANA_MEAS;
}
else {
state = FILT;
}
break;
// FILTER
case FILT:
if(filterWaitingVals() > FILTER_ORD) {
if(decimation==OFF) {
val = filterFIR();//Identity();
}
else {
val = filterFIRDecim();
}
while(serialSendWord(val)){}
}
else {
filterReset();
state = IDLE;
}
break;
case IDLE:
default:
// WAIT FOR COMMANDS
// check incomming messages
state = checkMessages();
if(state == IDLE) {
// sleep until the next interrupt
sleep_enable();
sleep_cpu();
sleep_disable();
}
}
}
return 0;
}
int iglasses_read_headset( fix *yaw, fix *pitch, fix *roll )
{
static unsigned char buff[16];
float radPitch,radRoll,sinPitch,sinRoll,cosPitch,cosRoll;
float rotx,roty,rotz;
float fx,fy,fz;
long x,y,z,p,r;
unsigned char checksum;
long i,count;
ReadBufferTimed(Iport, buff, 12, 10 ); // Wait 1/100 second for timeout.
checksum = 0;
count = Iport->count;
ClearRXBuffer(Iport);
WriteChar( Iport, 'S' );
for (i=0; i < 11; i++) checksum += buff[i];
if ( (count < 12) || (checksum != buff[11]) ) {
*yaw = i_yaw;
*pitch = i_pitch;
*roll = i_roll;
return 0;
}
x = (short)(buff[1] << 8) | buff[2];
y = (short)(buff[3] << 8) | buff[4];
z = (short)(buff[5] << 8) | buff[6];
p = (short)(buff[7] << 8) | buff[8];
r = (short)(buff[9] << 8) | buff[10];
fx = (float)x/FBITS;
fy = (float)y/FBITS;
fz = (float)z/FBITS;
radPitch = (float)p*TO_RADIANS;
radRoll = (float)r*TO_RADIANS;
sinPitch = sin(radPitch);
cosPitch = cos(radPitch);
sinRoll = sin(radRoll);
cosRoll = cos(radRoll);
roty = cosPitch*fy - sinPitch*fz;
rotz = sinPitch*fy + cosPitch*fz;
rotx = cosRoll*fx - sinRoll*roty;
#ifdef USE_FILTERS
*yaw = filterFIR( &X_filter,fl2f(-atan2(rotz,rotx)*M_PI/2.0));
*pitch = filterFIR( &Y_filter,fl2f(-radPitch*M_PI/2.0));
*roll = filterFIR( &Z_filter,fl2f(radRoll*M_PI/2.0));
if ( Num_readings < 30 ) {
Num_readings++;
BasisYaw = *yaw;
BasisPitch = *pitch;
BasisBank = *roll;
}
*yaw -= BasisYaw;
*pitch -= BasisPitch;
*roll -= BasisBank;
#else
*yaw = fl2f(-atan2(rotz,rotx)*M_PI/2.0);
*pitch = fl2f(-radPitch*M_PI/2.0);
*roll = fl2f(radRoll*M_PI/2.0);
#endif
i_yaw = *yaw;
i_pitch = *pitch;
i_roll = *roll;
return 1;
}
