void ahrs_init(void) {
QUAT_ASSIGN(ned_to_body_orientation_quat_i, 1, 0, 0, 0);
INT32_QUAT_NORMALIZE(ned_to_body_orientation_quat_i);
RATES_ASSIGN(body_rates_i, 0, 0, 0);
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
INT_RATES_ZERO(ahrs_impl.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;//AHRS状态未初始化
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;//未航姿校准
/* set ltp_to_imu so that body is zero */
//设置ltp_to_imu ，imu速度，陀螺仪的偏差，速度矫正，
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
INT_RATES_ZERO(ahrs_impl.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
INT_RATES_ZERO(ahrs_impl.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
//INT32_VECT3_ZERO(imu_accel_local); //This is part of the grav correction hack
}
void handle_ins_msg(void) {
#if USE_INS
update_fw_estimator();
#endif
#if USE_IMU
#ifdef XSENS_BACKWARDS
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
}
#else
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
}
#endif /* XSENS_BACKWARDS */
#endif /* USE_IMU */
#if USE_GPS_XSENS
#ifndef ALT_KALMAN
#warning NO_VZ
#endif
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D) {
fspeed = 0;
}
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
ahrs_impl.ltp_vel_norm_valid = FALSE;
ahrs_impl.heading_aligned = FALSE;
/* set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_i(&imu.body_to_imu),
sizeof(struct Int32Quat));
INT_RATES_ZERO(ahrs_impl.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_set_accel_gains();
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
ahrs_set_mag_gains();
/* set default gravity heuristic */
ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE;
#else
ahrs_impl.correct_gravity = FALSE;
#endif
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
ahrs_impl.accel_cnt = 0;
ahrs_impl.mag_cnt = 0;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
static inline void ahrs_update_mag_2d(void) {
const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y)};
struct Int32Vect3 measured_ltp;
INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
struct Int32Vect3 residual_ltp =
{ 0,
0,
(measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
struct Int32Vect3 residual_imu;
INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^22 * 2.5 = 1/410
// ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
// ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
// ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
ahrs_impl.rate_correction.p += residual_imu.x/16;
ahrs_impl.rate_correction.q += residual_imu.y/16;
ahrs_impl.rate_correction.r += residual_imu.z/16;
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^22 * 2.5e-3 = 655
// ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
// ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
// ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
void ahrs_icq_init(void)
{
ahrs_icq.status = AHRS_ICQ_UNINIT;
ahrs_icq.is_aligned = FALSE;
ahrs_icq.ltp_vel_norm_valid = FALSE;
ahrs_icq.heading_aligned = FALSE;
/* init ltp_to_imu quaternion as zero/identity rotation */
int32_quat_identity(&ahrs_icq.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs_icq.imu_rate);
INT_RATES_ZERO(ahrs_icq.gyro_bias);
INT_RATES_ZERO(ahrs_icq.rate_correction);
INT_RATES_ZERO(ahrs_icq.high_rez_bias);
/* set default filter cut-off frequency and damping */
ahrs_icq.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_icq.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_icq_set_accel_gains();
ahrs_icq.mag_omega = AHRS_MAG_OMEGA;
ahrs_icq.mag_zeta = AHRS_MAG_ZETA;
ahrs_icq_set_mag_gains();
/* set default gravity heuristic */
ahrs_icq.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_icq.correct_gravity = TRUE;
#else
ahrs_icq.correct_gravity = FALSE;
#endif
VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
ahrs_icq.accel_cnt = 0;
ahrs_icq.mag_cnt = 0;
}
void geo_mag_event(void)
{
if (geo_mag_calc_flag) {
double gha[MAXCOEFF]; // Geomag global variables
int32_t nmax;
/* Current date in decimal year, for example 2012.68 */
double sdate = GPS_EPOCH_BEGIN +
(double)gps.week / WEEKS_IN_YEAR +
(double)gps.tow / 1000 / SECS_IN_YEAR;
/* LLA Position in decimal degrees and altitude in km */
double latitude = (double)gps.lla_pos.lat / 1e7;
double longitude = (double)gps.lla_pos.lon / 1e7;
double alt = (double)gps.lla_pos.alt / 1e6;
// Calculates additional coeffs
nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
// Calculates absolute magnet fields
mag_calc(1, latitude, longitude, alt, nmax, gha,
&geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
double_vect3_normalize(&geo_mag.vect);
// copy to ahrs
#ifdef AHRS_FLOAT
VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect);
#else
// convert to MAG_BFP and copy to ahrs
VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x),
MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif
geo_mag.ready = TRUE;
}
geo_mag_calc_flag = FALSE;
}
static inline void ahrs_update_mag_full(void) {
const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y),
MAG_BFP_OF_REAL(AHRS_H_Z)};
struct Int32Vect3 expected_imu;
INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
ahrs_impl.rate_correction.p += residual.x/32/16;
ahrs_impl.rate_correction.q += residual.y/32/16;
ahrs_impl.rate_correction.r += residual.z/32/16;
ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
