fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 23:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: subscribe only to 2 gps topic instances

Browse Source
This commit is contained in:
Beat Küng
2018-10-02 14:14:10 +02:00
committed by Lorenz Meier
parent e167e6bec4
commit 23094b9ac9
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+9 -5
View File
@@ -260,7 +260,7 @@ private:
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
// because we can have multiple GPS instances
int _gps_subs[ORB_MULTI_MAX_INSTANCES] {};
int _gps_subs[GPS_MAX_RECEIVERS] {};
int _gps_orb_instance{-1};
orb_advert_t _att_pub{nullptr};
@@ -281,7 +281,7 @@ private:
DEFINE_PARAMETERS(
(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
_obs_dt_min_ms, ///< Maximmum time delay of any sensor used to increse buffer length to handle large timing jitter (mSec)
_obs_dt_min_ms, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
_mag_delay_ms, ///< magnetometer measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
@@ -605,8 +605,11 @@ Ekf2::Ekf2():
_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
for (unsigned i = 0; i < GPS_MAX_RECEIVERS; i++) {
_gps_subs[i] = orb_subscribe_multi(ORB_ID(vehicle_gps_position), i);
}
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
_range_finder_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i);
}
@@ -633,9 +636,10 @@ Ekf2::~Ekf2()
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
orb_unsubscribe(_range_finder_subs[i]);
_range_finder_subs[i] = -1;
}
for (unsigned i = 0; i < GPS_MAX_RECEIVERS; i++) {
orb_unsubscribe(_gps_subs[i]);
_gps_subs[i] = -1;
}
}