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