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); }
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; }
// Returns the angular difference in the horizontal plane between // a default vector an north in degrees. float getHeading() { // Default vector of x axis float from[3] = { 1, 0, 0 }; // Get latest compass readings for calculations accRead(&acc); magRead(&mag); return calcHeading(from, &acc, &mag); }
void getMag(int16_t *data) { int ii; int16_t buf[4]; uint8_t *tt=(uint8_t *) &buf[0]; magRead(tt); for(ii=0;ii<3;ii++) data[6+ii]=ByteSwap(buf[ii]);//(int16_t)(tt[jj++]<<8 | tt[jj++]); }
void getMag(uint16_t *data){ magRead((uint8_t *)data);}