/** * Initialize Vectornav struct */ void ins_vectornav_init(void) { // Initialize variables ins_vn.vn_status = VNNotTracking; ins_vn.vn_time = get_sys_time_float(); // Initialize packet ins_vn.vn_packet.status = VNMsgSync; ins_vn.vn_packet.msg_idx = 0; ins_vn.vn_packet.msg_available = FALSE; ins_vn.vn_packet.chksm_error = 0; ins_vn.vn_packet.hdr_error = 0; ins_vn.vn_packet.overrun_error = 0; ins_vn.vn_packet.noise_error = 0; ins_vn.vn_packet.framing_error = 0; INT32_VECT3_ZERO(ins_vn.ltp_pos_i); INT32_VECT3_ZERO(ins_vn.ltp_speed_i); INT32_VECT3_ZERO(ins_vn.ltp_accel_i); FLOAT_VECT3_ZERO(ins_vn.vel_ned); FLOAT_VECT3_ZERO(ins_vn.lin_accel); FLOAT_VECT3_ZERO(ins_vn.vel_body); #if USE_INS_NAV_INIT ins_init_origin_from_flightplan(); ins_vn.ltp_initialized = TRUE; #else ins_vn.ltp_initialized = FALSE; #endif struct FloatEulers body_to_imu_eulers = {INS_VN_BODY_TO_IMU_PHI, INS_VN_BODY_TO_IMU_THETA, INS_VN_BODY_TO_IMU_PSI}; orientationSetEulers_f(&ins_vn.body_to_imu, &body_to_imu_eulers); #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); register_periodic_telemetry(DefaultPeriodic, "VECTORNAV_INFO", send_vn_info); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); #endif }
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 }