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 calibrateMag(bool loadIn)
{
int i, j;
int16_t magMin[3] = {0, 0, 0};
int16_t magMax[3] = {0, 0, 0}; // The road warrior
for (i=0; i<128; i++)
{
//tu nie wiem
while (!magAvailable(i))
;
readMag1();
int16_t magTemp[3] = {0, 0, 0};
magTemp[0] = mx;
magTemp[1] = my;
magTemp[2] = mz;
for (j = 0; j < 3; j++)
{
if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j];
if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j];
}
}
for (j = 0; j < 3; j++)
{
mBiasRaw[j] = (magMax[j] + magMin[j]) / 2;
mBias[j] = calcMag(mBiasRaw[j]);
if (loadIn)
magOffset(j, mBiasRaw[j]);
}
}
void readMag1(void)
{
//for(int kl = 0; kl < 10; kl++){
uint8_t temp[6]; // We'll read six bytes from the mag into temp
mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
mx = calcMag(mx);
my = calcMag(my);
mz = calcMag(mz);
/*}*/
}
Point &Point::operator*=(float rhs) {
x *= rhs;
y *= rhs;
calcMag();
return *this;
}
Point &Point::operator-=(const Point &rhs) {
x -= rhs.x;
y -= rhs.y;
calcMag();
return *this;
}
Point &Point::operator/=(float rhs) {
x /= rhs;
y /= rhs;
calcMag();
return *this;
}
Point &Point::operator+=(const Point &rhs) {
x += rhs.x;
y += rhs.y;
calcMag();
return *this;
}
/*
* This is just an entry point. All this routine does is shuffle the force
* fields, so that we can save the last two forcing evaluations, and call
* the routines in charge of each individual equation.
*/
void calcForces()
{
debug("Calculating forces\n");
//cycle the force pointers for any active equation.
complex PRECISION * temp;
if(momEquation)
{
temp = u->sol->poloidal->force3;
u->sol->poloidal->force3 = u->sol->poloidal->force2;
u->sol->poloidal->force2 = u->sol->poloidal->force1;
u->sol->poloidal->force1 = temp;
temp = u->sol->toroidal->force3;
u->sol->toroidal->force3 = u->sol->toroidal->force2;
u->sol->toroidal->force2 = u->sol->toroidal->force1;
u->sol->toroidal->force1 = temp;
temp = u->sol->mean_xf3;
u->sol->mean_xf3 = u->sol->mean_xf2;
u->sol->mean_xf2 = u->sol->mean_xf1;
u->sol->mean_xf1 = temp;
temp = u->sol->mean_yf3;
u->sol->mean_yf3 = u->sol->mean_yf2;
u->sol->mean_yf2 = u->sol->mean_yf1;
u->sol->mean_yf1 = temp;
}
if(magEquation)
{
temp = B->sol->poloidal->force3;
B->sol->poloidal->force3 = B->sol->poloidal->force2;
B->sol->poloidal->force2 = B->sol->poloidal->force1;
B->sol->poloidal->force1 = temp;
temp = B->sol->toroidal->force3;
B->sol->toroidal->force3 = B->sol->toroidal->force2;
B->sol->toroidal->force2 = B->sol->toroidal->force1;
B->sol->toroidal->force1 = temp;
temp = B->sol->mean_xf3;
B->sol->mean_xf3 = B->sol->mean_xf2;
B->sol->mean_xf2 = B->sol->mean_xf1;
B->sol->mean_xf1 = temp;
temp = B->sol->mean_yf3;
B->sol->mean_yf3 = B->sol->mean_yf2;
B->sol->mean_yf2 = B->sol->mean_yf1;
B->sol->mean_yf1 = temp;
}
if(tEquation)
{
temp = T->force3;
T->force3 = T->force2;
T->force2 = T->force1;
T->force1 = temp;
}
//real force calculations are in these methods.
if(momEquation)
calcMomentum();
if(tEquation)
calcTemp();
if(magEquation)
calcMag();
debug("Forces done\n");
}
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;
}
}
void Point::setXY(float _x, float _y) {
x = _x;
y = _y;
calcMag();
}
void Point::setY(float _y) {
y = _y;
calcMag();
}
void Point::setX(float _x) {
x = _x;
calcMag();
}
Point::Point(float _x, float _y) {
x = _x;
y = _y;
calcMag();
}
