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_init(void) {
#if USE_INS_NAV_INIT
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_impl.ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
ins_impl.ltp_initialized = TRUE;
#else
ins_impl.ltp_initialized = FALSE;
#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() {
#if CONTROL_USE_VFF
ins.ltp_initialised = FALSE;
ins.baro_initialised = FALSE;
ins.vff_realign = FALSE;
b2_vff_init(0., 0., 0.);
#endif
b2ins_init();
INT32_VECT3_ZERO(ins.enu_pos);
INT32_VECT3_ZERO(ins.enu_speed);
INT32_VECT3_ZERO(ins.enu_accel);
}
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);
}
/**
* Initialize Vectornav struct
*/
void ahrs_vectornav_init(void)
{
// Initialize variables
ahrs_vn.vn_status = VNNotTracking;
ahrs_vn.vn_freq = 0;
// Initialize packet
ahrs_vn.vn_packet.status = VNMsgSync;
ahrs_vn.vn_packet.msg_idx = 0;
ahrs_vn.vn_packet.msg_available = false;
ahrs_vn.vn_packet.chksm_error = 0;
ahrs_vn.vn_packet.hdr_error = 0;
ahrs_vn.vn_packet.overrun_error = 0;
ahrs_vn.vn_packet.noise_error = 0;
ahrs_vn.vn_packet.framing_error = 0;
INT32_VECT3_ZERO(ahrs_vn.accel_i);
// initialize data struct
memset(&(ahrs_vn.vn_data), sizeof(struct VNData), 0);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
#endif
}
/**
* 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_module_wrapper_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_module.ltp_def);
ins_module.ltp_initialized = true;
#else
ins_module.ltp_initialized = false;
#endif
INT32_VECT3_ZERO(ins_module.ltp_pos);
INT32_VECT3_ZERO(ins_module.ltp_speed);
INT32_VECT3_ZERO(ins_module.ltp_accel);
ins_module_init();
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
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() {
#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;
}
void ins_init() {
#if USE_INS_NAV_INIT
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);
ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
ins_impl.ltp_initialized = TRUE;
#else
ins_impl.ltp_initialized = FALSE;
#endif
INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
INT32_VECT3_ZERO(ins_impl.ltp_accel);
}
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);
}
