zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ekf2 replay: handle multi multiple GPS instances (#9433)

sbg
Beat Küng 7 years ago committed by Paul Riseborough
parent
9a7195cb2d
commit
71b4ad62a1
1 changed files with 7 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      src/modules/replay/replay_main.cpp

10
src/modules/replay/replay_main.cpp
Unescape View File

@ -946,7 +946,8 @@ bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream
return true; return true;
} else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)) { } else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)
|| sub.orb_meta == ORB_ID(vehicle_gps_position)) {
return publishTopic(sub, data); return publishTopic(sub, data);
} // else: do not publish } // else: do not publish
@ -965,7 +966,9 @@ void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
_distance_sensor_msg_id = msg_id; _distance_sensor_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_gps_position)) { } else if (sub.orb_meta == ORB_ID(vehicle_gps_position)) {
_gps_msg_id = msg_id; if (sub.multi_id == 0) {
_gps_msg_id = msg_id;
}
} else if (sub.orb_meta == ORB_ID(optical_flow)) { } else if (sub.orb_meta == ORB_ID(optical_flow)) {
_optical_flow_msg_id = msg_id; _optical_flow_msg_id = msg_id;
@ -986,7 +989,8 @@ void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
// the main loop should only handle publication of the following topics, the sensor topics are // the main loop should only handle publication of the following topics, the sensor topics are
// handled separately in publishEkf2Topics() // handled separately in publishEkf2Topics()
sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status) sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status)
&& sub.orb_meta != ORB_ID(vehicle_land_detected); && sub.orb_meta != ORB_ID(vehicle_land_detected) &&
(sub.orb_meta != ORB_ID(vehicle_gps_position) || sub.multi_id == 0);
} }
bool ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file) bool ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)

Write Preview
Loading…
Cancel
Save