void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu),
sizeof(struct FloatEulers));
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu));
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
void ahrs_dcm_init(void)
{
ahrs_dcm.status = AHRS_DCM_UNINIT;
ahrs_dcm.is_aligned = FALSE;
/* init ltp_to_imu euler with zero */
FLOAT_EULERS_ZERO(ahrs_dcm.ltp_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_dcm.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
ahrs_dcm.gps_speed = 0;
ahrs_dcm.gps_acceleration = 0;
ahrs_dcm.gps_course = 0;
ahrs_dcm.gps_course_valid = FALSE;
ahrs_dcm.gps_age = 100;
}
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag)
{
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
struct FloatRMat ltp_to_imu_rmat;
float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
/* set filter dcm */
set_dcm_matrix_from_rmat(<p_to_imu_rmat);
/* use averaged gyro as initial value for bias */
ahrs_dcm.gyro_bias = *lp_gyro;
ahrs_dcm.status = AHRS_DCM_RUNNING;
ahrs_dcm.is_aligned = true;
return true;
}
void ahrs_align(void)
{
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_impl.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
struct FloatRMat ltp_to_imu_rmat;
FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
/* set filter dcm */
set_dcm_matrix_from_rmat(<p_to_imu_rmat);
/* Set initial body orientation */
set_body_orientation_and_rates();
/* use averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
#if USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
struct FloatRMat ltp_to_imu_rmat;
float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
/* set filter dcm */
set_dcm_matrix_from_rmat(<p_to_imu_rmat);
/* use averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0);
ahrs_dcm.status = AHRS_DCM_RUNNING;
ahrs_dcm.is_aligned = TRUE;
return TRUE;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat);
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
void Normalize(void)
{
float error = 0;
float temporary[3][3];
float renorm = 0;
uint8_t problem = FALSE;
// Find the non-orthogonality of X wrt Y
error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
// Add half the XY error to X, and half to Y
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
// The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
// Normalize lenght of X
renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
// a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
// b) if the norm is further from 1, use a real sqrt
// c) norm is huge: disaster! reset! mayday!
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
// Normalize lenght of Y
renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
// Normalize lenght of Z
renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
// Reset on trouble
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
problem = FALSE;
}
}
