bool MPU9250_DMP::read(float* acc, float* gyr, float* mag, int16_t* temp, ORIENTATION* ori)
{
if ( fifoAvailable() && dmpUpdateFifo() == INV_SUCCESS )
{
if (acc) {
acc[0] = calcAccel(ax);
acc[1] = calcAccel(ay);
acc[2] = calcAccel(az);
}
if (gyr) {
gyr[0] = calcGyro(gx);
gyr[1] = calcGyro(gy);
gyr[2] = calcGyro(gz);
}
if (mag) {
mag[0] = calcMag(mx);
mag[1] = calcMag(my);
mag[2] = calcMag(mz);
}
if ((_features & DMP_FEATURE_6X_LP_QUAT) && ori) {
computeEulerAngles();
ori->pitch = pitch;
ori->yaw = yaw;
ori->roll = roll;
}
if (temp) {
updateTemperature();
*temp = temperature;
}
return true;
}
return false;
}
int main(int argc, char **argv) {
int16 hexX, hexY, hexZ;
int hex1, hex2, hex3;
int8 pData[6];
int inp, i;
FILE *fp;
char filename[200];
filename[0] = 0;
strcat(filename, "/home/optimus-prime/DR-SensorTag-v2/gyro_");
strcat(filename, argv[argc-1]);
fp = fopen(filename, "a+");
int j=0;
for(i=2;i<8;i++,j++) {
sscanf(argv[i], "%x", &inp);
pData[j] = (int8)inp;
}
hexX = BUILD_INT16(pData[0], pData[1]);
hexY = BUILD_INT16(pData[2], pData[3]);
hexZ = BUILD_INT16(pData[4], pData[5]);
fprintf(fp, "%s , ", argv[1]);
fprintf(fp, "%5.3f , ", calcGyro(hexX));
fprintf(fp, "%5.3f , ", calcGyro(hexY));
fprintf(fp, "%5.3f \n", calcGyro(hexZ));
fclose(fp);
}
void readAndSendMeasurements(void (*sendFunction)(char *str))
{
if(!readingAllowed)
{
int16_t accelX = (adrAndData.dane[1] << 8) | adrAndData.dane[0]; // Store x-axis values into gx
int16_t accelY = (adrAndData.dane[3] << 8) | adrAndData.dane[2]; // Store y-axis values into gy
int16_t accelZ = (adrAndData.dane[5] << 8) | adrAndData.dane[4]; // Store z-axis values into gz
if (_autoCalc) //kalibracja
{
accelX -= aBiasRaw[X_AXIS];
accelX -= aBiasRaw[Y_AXIS];
accelX -= aBiasRaw[Z_AXIS];
}
accelX = calcAccel(accelX);
accelY = calcAccel(accelY);
accelZ = calcAccel(accelZ);
pomiaryAccel[counter].ax = accelX;
pomiaryAccel[counter].ay = accelY;
pomiaryAccel[counter].az = accelZ;
int16_t gyroX = (adrAndData.dane[7] << 1) | adrAndData.dane[6];
int16_t gyroY = (adrAndData.dane[9] << 1) | adrAndData.dane[8];
int16_t gyroZ = (adrAndData.dane[11] << 1) | adrAndData.dane[10];
if (_autoCalc) //kalibracja
{
gyroX -= gBiasRaw[X_AXIS];
gyroY -= gBiasRaw[Y_AXIS];
gyroZ -= gBiasRaw[Z_AXIS];
}
gyroX = calcGyro(gyroX);
gyroY = calcGyro(gyroY);
gyroZ = calcGyro(gyroZ);
pomiaryAccel[counter].ax = gyroX;
pomiaryAccel[counter].ay = gyroY;
pomiaryAccel[counter].az = gyroZ;
counter++;
readingAllowed = TRUE;
}
if(counter >= 100)
{
counter = 0;
}
}
void readGyro1(void)
{
/*for(int g = 0; g < 3; g++){*/
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
gx = ((int8_t)temp[1] << 8) | (int8_t)temp[0]; // Store x-axis values into gx
//gx = ((int8_t)temp[0] << 8) | (int8_t)temp[1]; // Store x-axis values into gx
gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
//gy = (temp[2] << 8) | temp[3];
gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
//gz = (temp[4] << 8) | temp[5];
/*if(g == 2)
{
int flu = 0;
}*/
if (_autoCalc)
{
gx -= gBiasRaw[X_AXIS];
gy -= gBiasRaw[Y_AXIS];
gz -= gBiasRaw[Z_AXIS];
}
gx = calcGyro(gx);
gy = calcGyro(gy);
gz = calcGyro(gz);
/*poms.gxs[g] = gx;
poms.gys[g] = gy;
poms.gzs[g] = gz;*/
for(int k = 0; k < 6; k++)
{
accelerationXYZ[k] = 0;
}
//}
}
void SensorInitGYRO()
{
float Cal[GYRO_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.GYRO_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.GYRO_Done = MPU6050_initialize();
SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#else
#endif
}
if(SensorInitState.GYRO_Done) {
printf("GYRO connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_GYRO, Cal);
if(FlashValid) {
CalFlashState.GYRO_FLASH = true;
GyroOffset[0] = Cal[0];
GyroOffset[1] = Cal[1];
GyroOffset[2] = Cal[2];
GyroScale[0] = Cal[3];
GyroScale[1] = Cal[4];
GyroScale[2] = Cal[5];
printf("GYRO calibration from [FLASH]\n");
}
else {
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
GyroScale[0] = 1.0;
GyroScale[1] = 1.0;
GyroScale[2] = 1.0;
printf("GYRO calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
printf("Scale: %f %f %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
nvtSetGyroScale(GyroScale);
nvtSetGyroOffset(GyroOffset);
#if defined(MPU6050) || defined(MPU6500)
nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
#else
nvtSetGYRODegPLSB(calcGyro(1));
#endif
}
else
printf("GYRO connect - [FAIL]\n");
}
void calibrate(bool autoCalc)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
uint8_t samples = 0;
int ii;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
// Turn on FIFO and set threshold to 32 samples
enableFIFO(TRUE);
setFIFO(FIFO_THS, 0x1F);
/*while (samples < 29)
{*/
samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
//samples = 10;
//}
for(ii = 0; ii < samples ; ii++)
{ // Read the gyro data stored in the FIFO
readGyro1();
gBiasRawTemp[0] += gx;
gBiasRawTemp[1] += gy;
gBiasRawTemp[2] += gz;
readAccel1();
aBiasRawTemp[0] += ax;
aBiasRawTemp[1] += ay;
aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
}
for (ii = 0; ii < samples/*3*/; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
gBias[ii] = calcGyro(gBiasRaw[ii]);
aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
aBias[ii] = calcAccel(aBiasRaw[ii]);
}
enableFIFO(FALSE);
setFIFO(FIFO_OFF, 0x00);
if (autoCalc) _autoCalc = TRUE;
}
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
// them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch
// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
// subtract the biases ourselves. This results in a more accurate measurement in general and can
// remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
// is good practice.
void LSM9DS1::calibrate(bool autoCalc)
{
uint8_t samples = 0;
int ii;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
// Turn on FIFO and set threshold to 32 samples
enableFIFO(true);
setFIFO(FIFO_THS, 0x1F);
while (samples < 0x1F)
{
samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
}
for(ii = 0; ii < samples ; ii++)
{ // Read the gyro data stored in the FIFO
readGyro();
gBiasRawTemp[0] += gx;
gBiasRawTemp[1] += gy;
gBiasRawTemp[2] += gz;
readAccel();
aBiasRawTemp[0] += ax;
aBiasRawTemp[1] += ay;
aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
}
for (ii = 0; ii < 3; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
gBias[ii] = calcGyro(gBiasRaw[ii]);
aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
aBias[ii] = calcAccel(aBiasRaw[ii]);
}
enableFIFO(false);
setFIFO(FIFO_OFF, 0x00);
if (autoCalc) _autoCalc = true;
}
void readAndSendMeasurements(void (*sendFunction)(char *str, int len))
{
if(!readingAllowed && (counter < 1))
{
accelX = (adrAndData.dane[1] << 8) | adrAndData.dane[0]; // Store x-axis values into gx
accelY = (adrAndData.dane[3] << 8) | adrAndData.dane[2]; // Store y-axis values into gy
accelZ = (adrAndData.dane[5] << 8) | adrAndData.dane[4]; // Store z-axis values into gz
if (_autoCalc) //kalibracja
{
accelX -= aBiasRaw[X_AXIS];
accelY -= aBiasRaw[Y_AXIS];
accelZ -= aBiasRaw[Z_AXIS];
}
accelXf = calcAccel(accelX);
accelYf = calcAccel(accelY);
accelZf = calcAccel(accelZ);
accelX = calcAccel(accelX);
accelY = calcAccel(accelY);
accelZ = calcAccel(accelZ);
pomiaryAccel[counter].ax = accelX;
pomiaryAccel[counter].ay = accelY;
pomiaryAccel[counter].az = accelZ;
gyroX = (adrAndData.dane[7] << 8) | adrAndData.dane[6];
gyroY = (adrAndData.dane[9] << 8) | adrAndData.dane[8];
gyroZ = (adrAndData.dane[11] << 8) | adrAndData.dane[10];
if (_autoCalc) //kalibracja
{
gyroX -= gBiasRaw[X_AXIS];
gyroY -= gBiasRaw[Y_AXIS];
gyroZ -= gBiasRaw[Z_AXIS];
}
gyroXf = calcGyro(gyroX);
gyroYf = calcGyro(gyroY);
gyroZf = calcGyro(gyroZ);
gyroX = calcGyro(gyroX);
gyroY = calcGyro(gyroY);
gyroZ = calcGyro(gyroZ);
pomiaryAccel[counter].gx = gyroX;
pomiaryAccel[counter].gy = gyroY;
pomiaryAccel[counter].gz = gyroZ;
magnetX = (adrAndData.dane[13] << 8) | adrAndData.dane[12];
magnetY = (adrAndData.dane[15] << 8) | adrAndData.dane[14];
magnetZ = (adrAndData.dane[17] << 8) | adrAndData.dane[16];
magnetXf = calcMag(magnetX);
magnetYf = calcMag(magnetY);
magnetZf = calcMag(magnetZ);
magnetX = calcMag(magnetX);
magnetY = calcMag(magnetY);
magnetZ = calcMag(magnetZ);
counter++;
/*if(counter < 50)
{
readingAllowed = TRUE;
}*/
}
if(counter >= 1/* && copied == 0*/)
{
int i = 0;
/*for(i = 0; i < 50; i++)
{
copiedData[i].ax = pomiaryAccel[i].ax;
copiedData[i].ay = pomiaryAccel[i].ay;
copiedData[i].az = pomiaryAccel[i].az;
copiedData[i].gx = pomiaryAccel[i].gx;
copiedData[i].gy = pomiaryAccel[i].gy;
copiedData[i].gz = pomiaryAccel[i].gz;
}*/
counter = 0;
copied = 1;
readingAllowed = TRUE;
}
}
int main(void)
{
// status_t status; // Declaration of return variable for DAVE3 APIs (toggle comment if required)
DAVE_Init(); // Initialization of DAVE Apps
int counter = 0;
handle_t TimerId;
uint32_t Status = SYSTM001_ERROR;
addressAndData adrAndData;
adrAndData.adr.addressDevice[0] = 0x6B;
adrAndData.adr.addressDevice[1] = 0x1E;
adrAndData.adr.subAddress[0] = OUT_X_L_XL; //subaddres for accel
adrAndData.adr.subAddress[1] = OUT_X_L_G; //sub address for gyroscope
adrAndData.adr.subAddress[2] = OUT_X_L_M;
initLSM9DS1();
calibrate(TRUE);
//readAccel1();
//makeTimer(100, SYSTM001_PERIODIC, timerHandlerReadByte1, &a, &Status, &TimerId);
TimerId=SYSTM001_CreateTimer(2,SYSTM001_PERIODIC,timerHandlerReadByte1,&adrAndData);
SYSTM001_StartTimer(TimerId);
while(1)
{
if(!readingAllowed)
{
int16_t accelX = (adrAndData.dane[1] << 8) | adrAndData.dane[0]; // Store x-axis values into gx
int16_t accelY = (adrAndData.dane[3] << 8) | adrAndData.dane[2]; // Store y-axis values into gy
int16_t accelZ = (adrAndData.dane[5] << 8) | adrAndData.dane[4]; // Store z-axis values into gz
if (_autoCalc) //kalibracja
{
accelX -= aBiasRaw[X_AXIS];
accelX -= aBiasRaw[Y_AXIS];
accelX -= aBiasRaw[Z_AXIS];
}
accelX = calcAccel(accelX);
accelY = calcAccel(accelY);
accelZ = calcAccel(accelZ);
pomiary[counter].ax = accelX;
pomiary[counter].ay = accelY;
pomiary[counter].az = accelZ;
int16_t gyroX = (adrAndData.dane[7] << 1) | adrAndData.dane[6];
int16_t gyroY = (adrAndData.dane[9] << 1) | adrAndData.dane[8];
int16_t gyroZ = (adrAndData.dane[11] << 1) | adrAndData.dane[10];
if (_autoCalc) //kalibracja
{
gyroX -= gBiasRaw[X_AXIS];
gyroY -= gBiasRaw[Y_AXIS];
gyroZ -= gBiasRaw[Z_AXIS];
}
gyroX = calcGyro(gyroX);
gyroY = calcGyro(gyroY);
gyroZ = calcGyro(gyroZ);
pomiary1[counter].ax = gyroX;
pomiary1[counter].ay = gyroY;
pomiary1[counter].az = gyroZ;
counter++;
readingAllowed = TRUE;
}
if(counter >= 100)
{
counter = 0;
}
}
return 0;
}
float readFloatGyroZ( void )
{
float output = calcGyro(readRawGyroZ());
return output;
}
float LSM6DS3::readFloatGyroY( void )
{
float output = calcGyro(readRawGyroY());
return output;
}
