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); }