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); }