void Euler_angles(void)
{
// printf("%f,%f,%f\n",fix16_to_dbl(-DCM_Matrix[2][0]),fix16_to_dbl(DCM_Matrix[2][1]),fix16_to_dbl(DCM_Matrix[2][2]));
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
roll = fix16_atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
pitch = fix16_atan2((Accel_Vector[0]),Accel_Vector[2]); // asin(acc_x)
yaw = 0;
#else // Euler angles from DCM matrix
pitch = fix16_asin(-DCM_Matrix[2][0]);
roll = fix16_atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = fix16_atan2(-DCM_Matrix[1][0],-DCM_Matrix[0][0]);
#endif
}
void imu_get_angles(fix16_t angles[]){
magnetometer_get_data();
accelerometer_get_data();
angles[0] = fix16_atan2((fix16_t)imu.accADC[ROLL] ,(fix16_t)imu.accADC[YAW]);
angles[1] = fix16_atan2((fix16_t)imu.accADC[PITCH],(fix16_t)imu.accADC[YAW]);
angles[2] = fix16_atan2((fix16_t)imu.magADC[PITCH] ,(fix16_t)imu.magADC[ROLL]);
angles[0] = fix16_rad_to_deg(angles[0]);
angles[1] = fix16_rad_to_deg(angles[1]);
angles[2] = fix16_rad_to_deg(angles[2]);
}
void calculate_heading_hmc5843( fix16_t roll, fix16_t pitch)
{
// float Head_X;
// float Head_Y;
// float cos_roll;
// float sin_roll;
// float cos_pitch;
// float sin_pitch;
//
//
// cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM?
// sin_roll = sin(roll);
// cos_pitch = cos(pitch);
// sin_pitch = sin(pitch);
// // Tilt compensated Magnetic field X component:
// Head_X = hmc5843_mag_x*cos_pitch+
// hmc5843_mag_y*sin_roll*sin_pitch+
// hmc5843_mag_z*cos_roll*sin_pitch;
// // Tilt compensated Magnetic field Y component:
// Head_Y = hmc5843_mag_y*cos_roll-
// hmc5843_mag_z*sin_roll;
// // Magnetic Heading
// //hmc_5843_heading = atan2(-Head_Y,Head_X);
fix16_t cos_roll = fix16_cos(roll);
fix16_t sin_roll = fix16_sin(roll);
fix16_t cos_pitch = fix16_cos(pitch);
fix16_t sin_pitch = fix16_sin(pitch);
hmc5843_head_x = fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_x),cos_pitch),
fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_y),fix16_mul(sin_roll,sin_pitch)),
fix16_mul(fix16_from_int(hmc5843_mag_z),fix16_mul(cos_roll,sin_pitch))));
hmc5843_head_y = fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_y),cos_roll),
-fix16_mul(fix16_from_int(hmc5843_mag_z),sin_roll));
hmc5843_heading = fix16_atan2(-hmc5843_head_y,hmc5843_head_x);
}
Fix16 atan2(const Fix16 &inY) { return Fix16(fix16_atan2(value, inY.value)); }
