示例#1
0
/**
 *	@brief Calculate the roll, pitch, yaw.
 *
 *	@param ac			An accelerometer (accel_t) structure.
 *	@param accel		[in] Pointer to a vec3w_t structure that holds the raw acceleration data.
 *	@param orient		[out] Pointer to a orient_t structure that will hold the orientation data.
 *	@param rorient		[out] Pointer to a orient_t structure that will hold the non-smoothed orientation data.
 *	@param smooth		If smoothing should be performed on the angles calculated. 1 to enable, 0 to disable.
 *
 *	Given the raw acceleration data from the accelerometer struct, calculate
 *	the orientation of the device and set it in the \a orient parameter.
 */
void calculate_orientation(struct accel_t* ac, struct vec3w_t* accel, struct orient_t* orient, int smooth) {
	float xg, yg, zg;
	float x, y, z;

	/*
	 *	roll	- use atan(z / x)		[ ranges from -180 to 180 ]
	 *	pitch	- use atan(z / y)		[ ranges from -180 to 180 ]
	 *	yaw		- impossible to tell without IR
	 */

	/* yaw - set to 0, IR will take care of it if it's enabled */
	orient->yaw = 0.0f;

	/* find out how much it has to move to be 1g */
	xg = (float)ac->cal_g.x;
	yg = (float)ac->cal_g.y;
	zg = (float)ac->cal_g.z;

	/* find out how much it actually moved and normalize to +/- 1g */
	x = ((float)accel->x - (float)ac->cal_zero.x) / xg;
	y = ((float)accel->y - (float)ac->cal_zero.y) / yg;
	z = ((float)accel->z - (float)ac->cal_zero.z) / zg;

	/* make sure x,y,z are between -1 and 1 for the tan functions */
	if (x < -1.0f)			x = -1.0f;
	else if (x > 1.0f)		x = 1.0f;
	if (y < -1.0f)			y = -1.0f;
	else if (y > 1.0f)		y = 1.0f;
	if (z < -1.0f)			z = -1.0f;
	else if (z > 1.0f)		z = 1.0f;

	/* if it is over 1g then it is probably accelerating and not reliable */
	if (abs(accel->x - ac->cal_zero.x) <= (ac->cal_g.x+10)) {
		/* roll */
		x = RAD_TO_DEGREE(atan2f(x, z));
		if(isfinite(x)) {
			orient->roll = x;
			orient->a_roll = x;
		}
	}

	if (abs(accel->y - ac->cal_zero.y) <= (ac->cal_g.y+10)) {
		/* pitch */
		y = RAD_TO_DEGREE(atan2f(y, z));
		if(isfinite(y)) {
			orient->pitch = y;
			orient->a_pitch = y;
		}
	}

	/* smooth the angles if enabled */
	if (smooth) {
		apply_smoothing(ac, orient, SMOOTH_ROLL);
		apply_smoothing(ac, orient, SMOOTH_PITCH);
	}
}
示例#2
0
/**
 *	@brief Calculate the gravity forces on each axis.
 *
 *	@param ac			An accelerometer (accel_t) structure.
 *	@param accel		[in] Pointer to a vec3b_t structure that holds the raw acceleration data.
 *	@param gforce		[out] Pointer to a gforce_t structure that will hold the gravity force data.
 */
void calculate_gforce(struct accel_t* ac, struct vec3b_t* accel, struct gforce_t* gforce, int smooth) {
	float xg, yg, zg;

	/* find out how much it has to move to be 1g */
	xg = (int)ac->cal_g.x;
	yg = (int)ac->cal_g.y;
	zg = (int)ac->cal_g.z;

	/* find out how much it actually moved and normalize to +/- 1g */
	gforce->a_vec.x = ((int)accel->x - (int)ac->cal_zero.x) / xg;
	gforce->a_vec.y = ((int)accel->y - (int)ac->cal_zero.y) / yg;
	gforce->a_vec.z = ((int)accel->z - (int)ac->cal_zero.z) / zg;
	
	if(smooth) {
		apply_smoothing(gforce, ac->st_alpha);
	}
	else {
		gforce->vec.x = gforce->a_vec.x;
		gforce->vec.y = gforce->a_vec.y;
		gforce->vec.z = gforce->a_vec.z;		
	}
}
static uchar mouse_axes_linear_equation_system() {  // {{{
#define W 4
#define H 3
	// Matrix of 3 lines and 4 columns
	float m[H][W];
	// The solution of this system
	float sol[H];

	uchar y;

	fill_matrix_from_sensor(m);

	// Gauss-Jordan elimination, based on:
	// http://elonen.iki.fi/code/misc-notes/python-gaussj/index.html

	for(y = 0; y < H; y++) {
		uchar maxrow;
		uchar y2;
		float pivot;

		// Finding the row with the maximum value at this column
		maxrow = y;
		pivot = fabs(m[maxrow][y]);
		for(y2 = y+1; y2 < H; y2++) {
			float newpivot;
			newpivot = fabs(m[y2][y]);
			if (newpivot > pivot) {
				pivot = newpivot;
				maxrow = y2;
			}
		}
		// If the maximum value in this column is zero
		if (pivot < 0.0009765625) {  // 2**-10 == 0.0009765625
			// Singular
			return 0;
		}
		if (maxrow != y) {
			swap_rows(m[y], m[maxrow]);
		}

		// Now we are ready to eliminate the column y, using m[y][y]
		for(y2 = y+1; y2 < H; y2++) {
			uchar x;
			float c;
			c = m[y2][y] / m[y][y];

			// Subtracting from every element in row y2
			for(x = y; x < W; x++) {
				m[y2][x] -= m[y][x] * c;
			}
		}
	}

	// The matrix is now in "row echelon form" (it is a triangular matrix).
	// Instead of using a loop for back-substituting all 3 elements from
	// sol[], I'm calculating only 2 of them, as only those are needed.

	sol[2] = m[2][3] / m[2][2];
	sol[1] = m[1][3] / m[1][1] - m[1][2] * sol[2] / m[1][1];
	// sol[0] is discarded

	if (   sol[1] < -0.25
		|| sol[1] >  1.25
		|| sol[2] < -0.25
		|| sol[2] >  1.25
	) {
		// Out-of-bounds
		return 0;
	}

	mouse_report.x = apply_smoothing(0, &sol[1]);
	mouse_report.y = apply_smoothing(1, &sol[2]);

	return 1;
#undef W
#undef H
}  // }}}
示例#4
0
/**
 *	@brief Calculate the roll, pitch, yaw.
 *
 *	@param ac			An accelerometer (accel_t) structure.
 *	@param accel		[in] Pointer to a vec3b_t structure that holds the raw acceleration data.
 *	@param orient		[out] Pointer to a orient_t structure that will hold the orientation data.
 *	@param rorient		[out] Pointer to a orient_t structure that will hold the non-smoothed
 *orientation data.
 *	@param smooth		If smoothing should be performed on the angles calculated. 1 to enable, 0 to
 *disable.
 *
 *	Given the raw acceleration data from the accelerometer struct, calculate
 *	the orientation of the device and set it in the \a orient parameter.
 */
void calculate_orientation(struct accel_t *ac, struct vec3b_t *accel, struct orient_t *orient, int smooth)
{
    float xg, yg, zg;
    float x, y, z;

    /*
     *	roll	- use atan(z / x)		[ ranges from -180 to 180 ]
     *	pitch	- use atan(z / y)		[ ranges from -180 to 180 ]
     *	yaw		- impossible to tell without IR
     */

    /* yaw - set to 0, IR will take care of it if it's enabled */
    orient->yaw = 0.0f;

    /* find out how much it has to move to be 1g */
    xg = (float)ac->cal_g.x;
    yg = (float)ac->cal_g.y;
    zg = (float)ac->cal_g.z;

    /* find out how much it actually moved and normalize to +/- 1g */
    x = ((float)accel->x - (float)ac->cal_zero.x) / xg;
    y = ((float)accel->y - (float)ac->cal_zero.y) / yg;
    z = ((float)accel->z - (float)ac->cal_zero.z) / zg;

    /* make sure x,y,z are between -1 and 1 for the tan functions */
    if (x < -1.0f)
    {
        x = -1.0f;
    } else if (x > 1.0f)
    {
        x = 1.0f;
    }
    if (y < -1.0f)
    {
        y = -1.0f;
    } else if (y > 1.0f)
    {
        y = 1.0f;
    }
    if (z < -1.0f)
    {
        z = -1.0f;
    } else if (z > 1.0f)
    {
        z = 1.0f;
    }

    /*
    If it is over 1g then it is probably accelerating and not reliable
    Formulas from: http://husstechlabs.com/projects/atb1/using-the-accelerometer/
*/

    if (abs(accel->x - ac->cal_zero.x) <= ac->cal_g.x)
    {
        /* roll */
        float roll = RAD_TO_DEGREE(atan2f(x, z));

        orient->roll   = roll;
        orient->a_roll = roll;
    }

    if (abs(accel->y - ac->cal_zero.y) <= ac->cal_g.y)
    {
        /* pitch */
        float pitch = RAD_TO_DEGREE(atan2f(y, sqrtf(x * x + z * z)));

        orient->pitch   = pitch;
        orient->a_pitch = pitch;
    }

    /* smooth the angles if enabled */
    if (smooth)
    {
        apply_smoothing(ac, orient, SMOOTH_ROLL);
        apply_smoothing(ac, orient, SMOOTH_PITCH);
    }
}