void ins_init()
{
// init position
#if INS_UPDATE_FW_ESTIMATOR
struct UtmCoor_f utm0;
utm0.north = (float)nav_utm_north0;
utm0.east = (float)nav_utm_east0;
utm0.alt = GROUND_ALT;
utm0.zone = nav_utm_zone0;
stateSetLocalUtmOrigin_f(&utm0);
stateSetPositionUtm_f(&utm0);
#else
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
struct LtpDef_i ltp_def;
ltp_def_from_ecef_i(<p_def, &ecef_nav0);
ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(<p_def);
#endif
B.x = INS_H_X;
B.y = INS_H_Y;
B.z = INS_H_Z;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
// init state and measurements
init_invariant_state();
// init gains
ins_impl.gains.lv = INS_INV_LV;
ins_impl.gains.lb = INS_INV_LB;
ins_impl.gains.mv = INS_INV_MV;
ins_impl.gains.mvz = INS_INV_MVZ;
ins_impl.gains.mh = INS_INV_MH;
ins_impl.gains.nx = INS_INV_NX;
ins_impl.gains.nxz = INS_INV_NXZ;
ins_impl.gains.nh = INS_INV_NH;
ins_impl.gains.ov = INS_INV_OV;
ins_impl.gains.ob = INS_INV_OB;
ins_impl.gains.rv = INS_INV_RV;
ins_impl.gains.rh = INS_INV_RH;
ins_impl.gains.sh = INS_INV_SH;
ins.status = INS_UNINIT;
ins_impl.reset = FALSE;
#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}
void ins_int_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_int.ltp_def);
ins_int.ltp_initialized = true;
#else
ins_int.ltp_initialized = false;
#endif
/* we haven't had any measurement updates yet, so set the counter to max */
ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb);
ins_int.baro_initialized = false;
#if USE_SONAR
ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_INT_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_int.vf_reset = false;
ins_int.hf_realign = false;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
INT32_VECT3_ZERO(ins_int.ltp_speed);
INT32_VECT3_ZERO(ins_int.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
#endif
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
}
void ins_int_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_from_flightplan();
ins_int.ltp_initialized = TRUE;
#else
ins_int.ltp_initialized = FALSE;
#endif
/* we haven't had any measurement updates yet, so set the counter to max */
ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
ins_int.baro_initialized = FALSE;
#if USE_SONAR
ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_int.vf_reset = FALSE;
ins_int.hf_realign = FALSE;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
INT32_VECT3_ZERO(ins_int.ltp_speed);
INT32_VECT3_ZERO(ins_int.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
#endif
}
void ins_init(void) {
#if USE_INS_NAV_INIT
ins_init_origin_from_flightplan();
ins_impl.ltp_initialized = TRUE;
#else
ins_impl.ltp_initialized = FALSE;
#endif
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
ins_impl.baro_initialized = FALSE;
#if USE_SONAR
ins_impl.update_on_agl = FALSE;
init_median_filter(&ins_impl.sonar_median);
ins_impl.sonar_offset = INS_SONAR_OFFSET;
#endif
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
/* init vertical and horizontal filters */
#if USE_VFF_EXTENDED
vff_init(0., 0., 0., 0.);
#else
vff_init(0., 0., 0.);
#endif
#if USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
INT32_VECT3_ZERO(ins_impl.ltp_accel);
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}
void ins_module_wrapper_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_module.ltp_def);
ins_module.ltp_initialized = true;
#else
ins_module.ltp_initialized = false;
#endif
INT32_VECT3_ZERO(ins_module.ltp_pos);
INT32_VECT3_ZERO(ins_module.ltp_speed);
INT32_VECT3_ZERO(ins_module.ltp_accel);
ins_module_init();
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
void ins_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_from_flightplan();
ins_impl.ltp_initialized = TRUE;
#else
ins_impl.ltp_initialized = FALSE;
#endif
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
ins_impl.baro_initialized = FALSE;
#if USE_SONAR
ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_impl.vf_reset = FALSE;
ins_impl.hf_realign = FALSE;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
INT32_VECT3_ZERO(ins_impl.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
#endif
}
void ins_init(void) {
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0);
stateSetPositionUtm_f(&utm0);
alt_kalman_init();
#if USE_BAROMETER
ins_impl.qfe = 0.0f;
ins_impl.baro_initialized = FALSE;
ins_impl.baro_alt = 0.0f;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
#endif
ins_impl.reset_alt_ref = FALSE;
alt_kalman(0.0f);
ins.status = INS_RUNNING;
}
