コード例 #1
0
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;
}
コード例 #2
0
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]);
	}
}
コード例 #3
0
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);
	/*}*/


}
コード例 #4
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
Point &Point::operator*=(float rhs) {
    x *= rhs;
    y *= rhs;
    
    calcMag();
    
    return *this;
}
コード例 #5
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
Point &Point::operator-=(const Point &rhs) {
    x -= rhs.x;
    y -= rhs.y;
    
    calcMag();
    
    return *this;
}
コード例 #6
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
Point &Point::operator/=(float rhs) {
    x /= rhs;
    y /= rhs;
    
    calcMag();
    
    return *this;
}
コード例 #7
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
Point &Point::operator+=(const Point &rhs) {
    x += rhs.x;
    y += rhs.y;
    
    calcMag();
    
    return *this;
}
コード例 #8
0
ファイル: Physics.c プロジェクト: BenByington/Proteus
/*
 * 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");
}
コード例 #9
0
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;
	}
}
コード例 #10
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
void Point::setXY(float _x, float _y) {
    x = _x;
    y = _y;
    
    calcMag();
}
コード例 #11
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
void Point::setY(float _y) {
    y = _y;
    calcMag();
}
コード例 #12
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
void Point::setX(float _x) {
    x = _x;
    calcMag();
}
コード例 #13
0
ファイル: Point.cpp プロジェクト: bradenwatling/Physics
Point::Point(float _x, float _y) {
    x = _x;
    y = _y;
    calcMag();
}