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); }
/** * Initialization of horizontal guidance module. */ void guidance_h_module_init(void) { // Subscribe to the VELOCITY_ESTIMATE ABI message AbiBindMsgVELOCITY_ESTIMATE(VISION_VELOCITY_ESTIMATE_ID, &velocity_est_ev, stabilization_opticflow_vel_cb); }