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