void ins_init() { #if USE_INS_NAV_INIT ins_ltp_initialised = TRUE; /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); llh_nav0.lon = INT32_RAD_OF_DEG(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); ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); ins_ltp_def.hmsl = NAV_ALT0; #else ins_ltp_initialised = FALSE; #endif ins_baro_initialised = FALSE; init_median_filter(&baro_median); ins_update_on_agl = FALSE; init_median_filter(&sonar_median); ins_sonar_offset = INS_SONAR_OFFSET; vff_init(0., 0., 0., 0.); ins.vf_realign = FALSE; ins.hf_realign = FALSE; #if USE_HFF b2_hff_init(0., 0., 0., 0.); #endif INT32_VECT3_ZERO(ins_ltp_pos); INT32_VECT3_ZERO(ins_ltp_speed); INT32_VECT3_ZERO(ins_ltp_accel); }
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_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() { #ifdef USE_INS_NAV_INIT ins_ltp_initialised = TRUE; /** FIXME: should use the same code than MOVE_WP in booz2_datalink.c */ struct LlaCoor_i llh; /* Height above the ellipsoid */ llh.lat = INT32_RAD_OF_DEG(NAV_LAT0); llh.lon = INT32_RAD_OF_DEG(NAV_LON0); //llh.alt = NAV_ALT0 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; llh.alt = NAV_ALT0 + NAV_HMSL0; struct EcefCoor_i nav_init; ecef_of_lla_i(&nav_init, &llh); ltp_def_from_ecef_i(&ins_ltp_def, &nav_init); ins_ltp_def.hmsl = NAV_ALT0; #else ins_ltp_initialised = FALSE; #endif #ifdef USE_VFF ins_baro_initialised = FALSE; #ifdef BOOZ2_SONAR ins_update_on_agl = FALSE; #endif vff_init(0., 0., 0.); #endif ins_vf_realign = FALSE; ins_hf_realign = FALSE; #ifdef USE_HFF b2_hff_init(0., 0., 0., 0.); #endif INT32_VECT3_ZERO(ins_ltp_pos); INT32_VECT3_ZERO(ins_ltp_speed); INT32_VECT3_ZERO(ins_ltp_accel); INT32_VECT3_ZERO(ins_enu_pos); INT32_VECT3_ZERO(ins_enu_speed); INT32_VECT3_ZERO(ins_enu_accel); }
void ins_init() { #if USE_INS_NAV_INIT ins_ltp_initialised = TRUE; /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); llh_nav0.lon = INT32_RAD_OF_DEG(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); ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); ins_ltp_def.hmsl = NAV_ALT0; stateSetLocalOrigin_i(&ins_ltp_def); #else ins_ltp_initialised = FALSE; #endif #if USE_VFF ins_baro_initialised = FALSE; vff_init(0., 0., 0.); #endif ins.vf_realign = FALSE; ins.hf_realign = FALSE; #if USE_HFF b2_hff_init(0., 0., 0., 0.); #endif INT32_VECT3_ZERO(ins_ltp_pos); INT32_VECT3_ZERO(ins_ltp_speed); INT32_VECT3_ZERO(ins_ltp_accel); // TODO correct init ins.status = INS_RUNNING; }