/**
* @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);
}
}
/**
* @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;
}
/**
* @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) );
}
/**
* @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;
}
}
/**
* @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));
}
/**
* @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);
}
}
