void gps_init(void) { multi_gps_mode = MULTI_GPS_MODE; gps.valid_fields = 0; gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; gps.cacc = 0; gps.last_3dfix_ticks = 0; gps.last_3dfix_time = 0; gps.last_msg_ticks = 0; gps.last_msg_time = 0; #ifdef GPS_POWER_GPIO gpio_setup_output(GPS_POWER_GPIO); GPS_POWER_GPIO_ON(GPS_POWER_GPIO); #endif #ifdef GPS_LED LED_OFF(GPS_LED); #endif AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo); #endif }
void ahrs_infrared_init(void) { heading = 0.; AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb); AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared); register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status); #endif }
void intermcu_init(void) { pprz_transport_init(&intermcu.transport); #if USE_GPS AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb); #endif #ifdef BOARD_PX4IO px4bl_tid = sys_time_register_timer(10.0, NULL); #endif }
void ins_xsens700_init(void) { xsens700_init(); ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); }
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); }
void gps_init(void) { multi_gps_mode = MULTI_GPS_MODE; gps.valid_fields = 0; gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; gps.cacc = 0; gps.last_3dfix_ticks = 0; gps.last_3dfix_time = 0; gps.last_msg_ticks = 0; gps.last_msg_time = 0; #ifdef GPS_POWER_GPIO gpio_setup_output(GPS_POWER_GPIO); GPS_POWER_GPIO_ON(GPS_POWER_GPIO); #endif #ifdef GPS_LED LED_OFF(GPS_LED); #endif RegisterGps(PRIMARY_GPS); #ifdef SECONDARY_GPS RegisterGps(SECONDARY_GPS); #endif for (int i=0; i < GPS_NB_IMPL; i++) { if (GpsInstances[i].init != NULL) { GpsInstances[i].init(); } } AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo); #endif }
void ins_reset_altitude_ref(void) { struct LlaCoor_i lla = { .lat = state.ned_origin_i.lla.lat, .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; ltp_def_from_lla_i(&ins_gp.ltp_def, &lla); ins_gp.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_gp.ltp_def); } #include "subsystems/abi.h" static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s) { if (gps_s->fix == GPS_FIX_3D) { if (!ins_gp.ltp_initialized) { ins_reset_local_origin(); } /* simply scale and copy pos/speed from gps */ struct NedCoor_i gps_pos_cm_ned; ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos); INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); stateSetPositionNed_i(&ins_gp.ltp_pos); struct NedCoor_i gps_speed_cm_s_ned; ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel); INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); stateSetSpeedNed_i(&ins_gp.ltp_speed); } } void ins_gps_passthrough_register(void); { ins_register_impl(ins_gps_passthrough_init); AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); }
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_xsens_register(void) { ins_register_impl(ins_xsens_init); AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); }
void ahrs_chimu_register(void) { ahrs_chimu_init(); ahrs_register_impl(ahrs_chimu_enable_output); AbiBindMsgGPS(AHRS_CHIMU_GPS_ID, &gps_ev, gps_cb); }
void ins_xsens_register(void) { ins_register_impl(ins_xsens_init); AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); }