Пример #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 angle and magnitude of a joystick.
 *
 *	@param js	[out] Pointer to a joystick_t structure.
 *	@param x	The raw x-axis value.
 *	@param y	The raw y-axis value.
 */
void calc_joystick_state(struct joystick_t* js, float x, float y) {
	float rx, ry, ang;

	/*
	 *	Since the joystick center may not be exactly:
	 *		(min + max) / 2
	 *	Then the range from the min to the center and the center to the max
	 *	may be different.
	 *	Because of this, depending on if the current x or y value is greater
	 *	or less than the assoicated axis center value, it needs to be interpolated
	 *	between the center and the minimum or maxmimum rather than between
	 *	the minimum and maximum.
	 *
	 *	So we have something like this:
	 *		(x min) [-1] ---------*------ [0] (x center) [0] -------- [1] (x max)
	 *	Where the * is the current x value.
	 *	The range is therefore -1 to 1, 0 being the exact center rather than
	 *	the middle of min and max.
	 */
   rx = applyCalibration(x, js->min.x, js->max.x, js->center.x);
   ry = applyCalibration(y, js->min.y, js->max.y, js->center.y);

	/* calculate the joystick angle and magnitude */
	ang = RAD_TO_DEGREE(atan2f(ry, rx));
	js->mag = sqrtf((rx * rx) + (ry * ry));
	js->ang = ang + 180.0f;
}
Пример #3
0
/**
 *	@brief Calculate yaw given the IR data.
 *
 *	@param ir	IR data structure.
 */
float calc_yaw(struct ir_t* ir) {
	float x;

	x = ir->ax - 512;
	x = x * (ir->z / 1024.0f);

	return RAD_TO_DEGREE( atanf(x / ir->z) );
}
Пример #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 vec3f_t* in, struct ang3f_t* out) {
	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 */
	out->yaw = 0.0f;

	/* find out how much it actually moved and normalize to +/- 1g */
	x = in->x;
	y = in->y;
	z = in->z;

	/* 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 the gravity vector cannot be identified */
	if (abs(in->x) <= 1.0) {
		/* roll */
		x = -RAD_TO_DEGREE(atan2f(x, z));

		out->roll = x;
	}

	if (abs(in->y) <= 1.0) {
		/* pitch */
		y = RAD_TO_DEGREE(atan2f(y, z));

		out->pitch = y;
	}
}
Пример #5
0
/**
 *	@brief Calculate the angle and magnitude of a joystick.
 *
 *	@param js	[out] Pointer to a joystick_t structure.
 *	@param x	The raw x-axis value.
 *	@param y	The raw y-axis value.
 */
void calc_joystick_state(struct joystick_t* js, float x, float y) {
	float rx, ry, ang;

	/*
	 *	Since the joystick center may not be exactly:
	 *		(min + max) / 2
	 *	Then the range from the min to the center and the center to the max
	 *	may be different.
	 *	Because of this, depending on if the current x or y value is greater
	 *	or less than the assoicated axis center value, it needs to be interpolated
	 *	between the center and the minimum or maxmimum rather than between
	 *	the minimum and maximum.
	 *
	 *	So we have something like this:
	 *		(x min) [-1] ---------*------ [0] (x center) [0] -------- [1] (x max)
	 *	Where the * is the current x value.
	 *	The range is therefore -1 to 1, 0 being the exact center rather than
	 *	the middle of min and max.
	 */
	if (x == js->center.x)
		rx = 0;
	else if (x >= js->center.x)
		rx = ((float)(x - js->center.x) / (float)(js->max.x - js->center.x));
	else
		rx = ((float)(x - js->min.x) / (float)(js->center.x - js->min.x)) - 1.0f;

	if (y == js->center.y)
		ry = 0;
	else if (y >= js->center.y)
		ry = ((float)(y - js->center.y) / (float)(js->max.y - js->center.y));
	else
		ry = ((float)(y - js->min.y) / (float)(js->center.y - js->min.y)) - 1.0f;

	/* calculate the joystick angle and magnitude */
	ang = RAD_TO_DEGREE(atanf(ry / rx));
	ang -= 90.0f;
	if (rx < 0.0f)
		ang -= 180.0f;
	js->ang = absf(ang);
	js->mag = (float) sqrt((rx * rx) + (ry * ry));
}
Пример #6
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);
    }
}