void ins_init() { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); ins.status = INS_RUNNING; }
void ins_update_gps(void) { #if USE_GPS struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.; utm.north = gps.utm_pos.north / 100.; utm.zone = nav_utm_zone0; #if !USE_BAROMETER float falt = gps.hmsl / 1000.; EstimatorSetAlt(falt); #endif utm.alt = ins_alt; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { gps.ned_vel.x / 100., gps.ned_vel.y / 100., gps.ned_vel.z / 100. }; // set velocity stateSetSpeedNed_f(&ned_vel); #endif }
void ins_update_baro() { #if USE_BAROMETER // TODO update kalman filter with baro struct if (baro.status == BS_RUNNING) { if (!ins_baro_initialised) { ins_qfe = baro.absolute; ins_baro_initialised = TRUE; } if (ins.vf_realign) { ins.vf_realign = FALSE; ins_qfe = baro.absolute; } else { /* not realigning, so normal update with baro measurement */ /* altitude decreases with increasing baro.absolute pressure */ ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS; /* run the filter */ EstimatorSetAlt(ins_baro_alt); /* set new altitude, just copy old horizontal position */ struct UtmCoor_f utm; UTM_COPY(utm, *stateGetPositionUtm_f()); utm.alt = ins_alt; stateSetPositionUtm_f(&utm); } } #endif }
void ins_init(void) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); xsens_init(); }
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_init(void) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; xsens_init(); }
void ins_xsens700_init(void) { xsens700_init(); ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); }
void ins_xsens_update_gps(struct GpsState *gps_s) { struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { gps_s->ned_vel.x / 100., gps_s->ned_vel.y / 100., gps_s->ned_vel.z / 100. }; // set velocity stateSetSpeedNed_f(&ned_vel); }
static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s) { struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { gps_s->ned_vel.x / 100., gps_s->ned_vel.y / 100., gps_s->ned_vel.z / 100. }; // set velocity stateSetSpeedNed_f(&ned_vel); }
void ins_update_gps(void) { struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.; utm.north = gps.utm_pos.north / 100.; utm.zone = nav_utm_zone0; utm.alt = gps.hmsl / 1000.; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { gps.ned_vel.x / 100., gps.ned_vel.y / 100., gps.ned_vel.z / 100. }; // set velocity stateSetSpeedNed_f(&ned_vel); }
void ins_init() { 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_qfe = 0;; ins_baro_initialised = FALSE; ins_baro_alt = 0.; #endif ins.vf_realign = FALSE; EstimatorSetAlt(0.); }
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; }
void ins_init() { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0);//通过UTM坐标设置本地坐标 stateSetPositionUtm_f(&utm0); }