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;
}
void readAccelToSensor(accel *pomiar)
{
uint8_t i = 0; //licznik dla czytania
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
uint8_t subAddr = OUT_X_L_XL;
while(i < 6)
{
subAddr = OUT_X_L_XL;
subAddr = subAddr + i;
temp[i] = I2CreadBytes(_xgAddress, subAddr, NULL, 0);
i++;
}
ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
if (_autoCalc) //kalibracja
{
ax -= aBiasRaw[X_AXIS];
ay -= aBiasRaw[Y_AXIS];
az -= aBiasRaw[Z_AXIS];
}
ax = calcAccel(ax);
ay = calcAccel(ay);
az = calcAccel(az);
pomiar->ax = ax;
pomiar->ay = ay;
pomiar->az = az;
}
void readAccel1v1(accel *a)
{
uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
int index = 0;
xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL
ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
if (_autoCalc)
{
ax -= aBiasRaw[X_AXIS];
ay -= aBiasRaw[Y_AXIS];
az -= aBiasRaw[Z_AXIS];
}
ax = calcAccel(ax);
ay = calcAccel(ay);
az = calcAccel(az);
if((accelMeasurementsNum > 99) && (measurementsLSMRead == 0))
{
measurementsLSMRead = 1;
/*a[accelMeasurementsNum].ax = ax;
a[accelMeasurementsNum].ay = ay;
a[accelMeasurementsNum].az = az;*/
return;
}
a[accelMeasurementsNum].ax = ax;
a[accelMeasurementsNum].ay = ay;
a[accelMeasurementsNum].az = az;
accelMeasurementsNum++;
/*a[accelMeasurementsNum].ax = ax;
a[accelMeasurementsNum].ay = ay;
a[accelMeasurementsNum].az = az;
accelMeasurementsNum++;*/
toAscii(ax, &index);
toAscii(ay, &index);
toAscii(az, &index);
/*lk[0] = '0' + ax;
lk[1] = '0' + ay;
lk[2] = '0' + az;*/
for(int k = 0; k < 6; k++)
{
accelerationXYZ[k] = 0;
}
}
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 CCMoveAccelBy::startWithTarget(CCNode *pTarget)
{
CCActionInterval::startWithTarget(pTarget);
m_startPosition = pTarget->getPosition();
m_endPosition = ccpAdd(m_startPosition, m_delta);
calcAccel();
}
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 readFloatAccelZ( void )
{
float output = calcAccel(readRawAccelZ());
return output;
}
void
RungeKuttaSolver::step( double h )
{
int numParticles = mParticleSystem->getNumParticles();
double airResistance = mParticleSystem->getAirResistance();
double halfH = h / 2.0;
double h6 = h / 6.0;
//Allocate temp. storage on first call
if( mTmpPos == NULL )
{
mTmpPos = new Vector3d[numParticles];
mTmpV = new Vector3d[numParticles];
mK1 = new Vector3d[numParticles];
mK2 = new Vector3d[numParticles];
mK3 = new Vector3d[numParticles];
mK4 = new Vector3d[numParticles];
}
//Copy out original position and velocity
Vector3d *origPos = mParticleSystem->getParticlePositions();
Vector3d *origV = mParticleSystem->getParticleVelocities();
double *invMasses = mParticleSystem->getParticleInvMasses();
ParticleSystem::ParticleInfo *pInfo =
mParticleSystem->getParticleInfo();
//---------- 1. k1 = h*f(x,y) ----------
calcAccel( origPos, origV, invMasses, mK1 );
//---------- 2. k2 = h*f(x,y+k1/2) ----------
for( int i = 0; i < numParticles; i++ )
{
if( pInfo[i].pIsPinned )
{
mTmpV[i] = origV[i];
mTmpPos[i] = origPos[i];
continue;
}
mTmpV[i] = origV[i] + (mK1[i] * halfH);
mTmpPos[i] = origPos[i] + (mTmpV[i] * halfH);
}
calcAccel( mTmpPos, mTmpV, invMasses, mK2 );
//---------- 3. k3 = h*f(x,y+k2/2) ----------
for( int i = 0; i < numParticles; i++ )
{
if( pInfo[i].pIsPinned )
{
mTmpV[i] = origV[i];
mTmpPos[i] = origPos[i];
continue;
}
mTmpV[i] = origV[i] + (mK2[i] * halfH);
mTmpPos[i] = origPos[i] + (mTmpV[i] * halfH);
}
calcAccel( mTmpPos, mTmpV, invMasses, mK3 );
//---------- 3. k4 = h*f(x,y+k3) ----------
for( int i = 0; i < numParticles; i++ )
{
if( pInfo[i].pIsPinned )
{
mTmpV[i] = origV[i];
mTmpPos[i] = origPos[i];
continue;
}
mTmpV[i] = origV[i] + (mK3[i] * h);
mTmpPos[i] = origPos[i] + (mTmpV[i] * h);
}
calcAccel( mTmpPos, mTmpV, invMasses, mK4 );
//---------- 4. set x & v of particles ----------
//Set particle position
for( int i = 0; i < numParticles; i++ )
{
//don't move pinned particles
if( pInfo[i].pIsPinned )
continue;
Vector3d &tmpV = origV[i];
//origPos[i] += ( tmpV + (tmpV + mK1[i]*halfH)*2.0 + (tmpV+mK2[i]*halfH)*2.0 + (tmpV+mK3[i]*h) ) * h6;
origPos[i] += tmpV*h + ( mK1[i]*h + mK2[i]*h + mK3[i]*h ) * h6;
tmpV += (mK1[i] + mK2[i]*2.0 + mK3[i]*2.0 + mK4[i]) * h6;
origV[i] *= airResistance;
}
}
float LSM6DS3::readFloatAccelY( void )
{
float output = calcAccel(readRawAccelY());
return output;
}
/* Sensors Init */
void SensorInitACC()
{
float Cal[ACC_CAL_DATA_SIZE];
bool FlashValid;
#if defined(LSM6DS3)
status_t status;
#endif
if(!SensorInitState.ACC_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.ACC_Done = MPU6050_initialize();
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#else
LSM6DS3_init();
status = begin();
if(status==0)
SensorInitState.ACC_Done = true;
else
SensorInitState.ACC_Done = false;
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
}
if(SensorInitState.ACC_Done) {
printf("ACC connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_ACC, Cal);
if(FlashValid) {
CalFlashState.ACC_FLASH = true;
AccOffset[0] = Cal[0];
AccOffset[1] = Cal[1];
AccOffset[2] = Cal[2];
AccScale[0] = Cal[3];
AccScale[1] = Cal[4];
AccScale[2] = Cal[5];
AccRotate[0] = Cal[6];
AccRotate[1] = Cal[7];
AccRotate[2] = Cal[9];
AccRotate[3] = Cal[9];
AccRotate[4] = Cal[10];
AccRotate[5] = Cal[11];
AccRotate[6] = Cal[12];
AccRotate[7] = Cal[13];
AccRotate[8] = Cal[14];
printf("ACC calibration from - [FLASH]\n");
}
else {
AccOffset[0] = 0;
AccOffset[1] = 0;
AccOffset[2] = 0;
AccScale[0] = IMU_G_PER_LSB_CFG;
AccScale[1] = IMU_G_PER_LSB_CFG;
AccScale[2] = IMU_G_PER_LSB_CFG;
AccRotate[0] = 1;
AccRotate[1] = 0;
AccRotate[2] = 0;
AccRotate[3] = 0;
AccRotate[4] = 1;
AccRotate[5] = 0;
AccRotate[6] = 0;
AccRotate[7] = 0;
AccRotate[8] = 1;
printf("ACC calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
printf("Scale: %f %f %f\n", AccScale[0], AccScale[1], AccScale[2]);
printf("M[0][1][2]: %f %f %f\n", AccRotate[0], AccRotate[1], AccRotate[2]);
printf("M[3][4][5]: %f %f %f\n", AccRotate[3], AccRotate[4], AccRotate[5]);
printf("M[6][7][8]: %f %f %f\n", AccRotate[6], AccRotate[7], AccRotate[8]);
nvtSetAccScale(AccScale);
nvtSetAccOffset(AccOffset);
nvtSetAccRotate(AccRotate);
#if defined(MPU6050) || defined(MPU6500)
nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
#else
nvtSetAccG_PER_LSB(calcAccel(1)/*IMU_G_PER_LSB_CFG*/);
#endif
}
else {
__disable_irq();
SYS_UnlockReg();
SYS_ResetChip();
printf("ACC connect - [FAIL]\n");
}
}
