/**
* 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
}
