void state_init(void)
{
accel_g=sqrt(ahrs_accel[0]*ahrs_accel[0] + ahrs_accel[1]*ahrs_accel[1] + ahrs_accel[2]*ahrs_accel[2]);
accel2euler( raw_ahrs_theta, ahrs_accel, ahrs_compass );
euler2quat( raw_K_Quat, raw_ahrs_theta );
norm( raw_K_Quat );
}
/**
* ahrs_init() takes the initial values from the IMU and converts
* them into the quaterion state vector.
*/
void
ahrs_init(
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
v_t euler_angles;
accel2euler(
euler_angles,
ax,
ay,
az,
heading
);
euler2quat(
X,
euler_angles[0],
euler_angles[1],
euler_angles[2]
);
}
/*--------------------------------------------------------------------------------------*/
void ahrs_init(void)
{
accel2euler( raw_ahrs_theta, imu_accel, ahrs_compass );
euler2quat( raw_K_Quat, raw_ahrs_theta );
norm( raw_K_Quat );
//printf("INIT_q0=%f\tq1=%f\tq2=%f\tq3=%f\n",quat[0],quat[1],quat[2],quat[3] );
//bias[0] = ahrs_pqr[0];
//bias[1] = ahrs_pqr[1];
//bias[2] = ahrs_pqr[2];
}
