static msg_t sensorsRead (void*arg) { float acc_x, acc_y, acc_z; float gyro_x, gyro_y, gyro_z; uint8_t temp; /*sensors initializing*/ gyroInit (&I2CD2, IMU01A_GYRO); accInit (&I2CD2, IMU01A_ACC); chprintf ((BaseSequentialStream *)&SD2, "Configuration done\n\r"); while (true) { /*gyroscope and temperature reading*/ gyroRead (&I2CD2, IMU01A_GYRO, &gyro_x, &gyro_y, &gyro_z); tempRead (&I2CD2, IMU01A_GYRO, &temp); /*accelerometer reading*/ accRead (&I2CD2, IMU01A_ACC, &acc_x, &acc_y, &acc_z); /*printing out magnetometer values*/ chprintf ((BaseSequentialStream *)&SD2, "%f gX %f gY %f gZ ", gyro_x, gyro_y, gyro_z); /*printing out accelerometer and temperature values*/ chprintf ((BaseSequentialStream *)&SD2, "%f aX %f aY %f aZ ", acc_x, acc_y, acc_z); chprintf ((BaseSequentialStream *)&SD2, "%d T\n\r", temp); } }
void printRaw(void) { Vector3f v; accRead(&v); printf("Ax: %f Ay: %f Az: %f\n", v.x, v.y, v.z); gyroRead(&v); printf("Gx: %f Gy: %f Gz: %f\n", v.x, v.y, v.z); magRead(&v); printf("Mx: %f My: %f Mz: %f\n", v.x, v.y, v.z); }
void _ISR _NOPSV _T1Interrupt(void) //Running at 10Hz { OUTER_LED = ON; tiltX = (INT32)(accelRead(XINCL)*TILT_MULT); gRead = (INT32)(gyroRead(GRATE)*GYRO_MULT); //convert from quadrants to pitch angle tilt = tiltX; //update filter do_kalman(gRead, tilt); newData = TRUE; _T1IF = 0; }
uint8_t compassRead(int16_t *data) { int ii; int16_t buf[4]; uint8_t *tt=(uint8_t *) &buf[0]; // data are LSB first no need for byte swap gyroRead(tt); for(ii=0;ii<3;ii++) data[ii]=buf[ii]; #ifdef LSM303D accRead(tt); for(ii=0;ii<3;ii++) data[3+ii]=buf[ii]; magRead(tt); for(ii=0;ii<3;ii++) data[6+ii]=buf[ii]; tempRead(tt); data[9]=buf[0]; #else accRead(tt); for(ii=0;ii<3;ii++) data[3+ii]=buf[ii]>>4; magRead(tt); for(ii=0;ii<3;ii++) data[6+ii]=ByteSwap(buf[ii]);//(int16_t)(tt[jj++]<<8 | tt[jj++]); tempRead(tt); data[9]=ByteSwap(buf[0])>>4;// ((int16_t)(tt[0]<<8 | tt[1]))>>4; #endif // return 1; }
int main() { TRISA = 0; TRISB = BIT(0)|BIT(1)|BIT(3)|BIT(7)|BIT(12); TRISC = 0; OUTER_LED = OFF; INNER_LED = OFF; init_pll(); peripheral_pin_config(); init_uart1(); init_spi(); init_adc(); init_timer1(); TX_string("Wait\r\n"); delay(1000); TX_string("Start\r\n"); T1CONbits.TON = 1; //remove first trash reading tiltX = accelRead(XINCL); gRead = gyroRead(GRATE); while(1) { if( newData == TRUE) { TX_snum5(gRead); TX('\t'); TX_snum5(tilt); TX('\t'); TX_snum5(x_00); TX_string("\r\n"); OUTER_LED = OFF; newData = FALSE; } } return 0; };
void getGytro(int16_t *data) { int ii; int16_t buf[4]; uint8_t *tt=(uint8_t *) &buf[0]; gyroRead(tt); for(ii=0;ii<3;ii++) data[ii]=buf[ii]; }
void getGyro(uint16_t *data){ gyroRead((uint8_t *)data);}