yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Move GPS blending from ekf2 to sensors module

Browse Source 
- new sensors work item that subscribes to N x sensor_gps and publishes vehicle_gps_position
 - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

Co-authored-by: Jacob Crabill <jacob@volans-i.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Jacob Dahl
2020-09-25 19:28:31 -08:00
committed by GitHub
parent e792c46f20
commit a24488328f
27 changed files with 1147 additions and 861 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -336,7 +336,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
mavlink_msg_hil_gps_decode(msg, &hil_gps);
if (!_gps_blocked) {
vehicle_gps_position_s gps{};
sensor_gps_s gps{};
gps.timestamp = hrt_absolute_time();
gps.time_utc_usec = hil_gps.time_usec;
@@ -356,16 +356,16 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
// New publishers will be created based on the HIL_GPS ID's being different or not
for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
if (_vehicle_gps_position_pubs[i] && _gps_ids[i] == hil_gps.id) {
_vehicle_gps_position_pubs[i]->publish(gps);
if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) {
_sensor_gps_pubs[i]->publish(gps);
break;
}
if (_vehicle_gps_position_pubs[i] == nullptr) {
_vehicle_gps_position_pubs[i] = new uORB::PublicationMulti<vehicle_gps_position_s> {ORB_ID(vehicle_gps_position)};
if (_sensor_gps_pubs[i] == nullptr) {
_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
_gps_ids[i] = hil_gps.id;
_vehicle_gps_position_pubs[i]->publish(gps);
_sensor_gps_pubs[i]->publish(gps);
break;
}
}