fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 17:33:33 +08: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 23:28:31 -04:00
committed by GitHub
co-authored by Jacob Crabill Jacob Dahl
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
src/modules/simulator/simulator.h
+2 -2
View File
@@ -65,10 +65,10 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
@@ -231,7 +231,7 @@ private:
// HIL GPS
static constexpr int MAX_GPS = 3;
uORB::PublicationMulti<vehicle_gps_position_s> *_vehicle_gps_position_pubs[MAX_GPS] {};
uORB::PublicationMulti<sensor_gps_s> *_sensor_gps_pubs[MAX_GPS] {};
uint8_t _gps_ids[MAX_GPS] {};
std::default_random_engine _gen{};
src/modules/simulator/simulator_mavlink.cpp
+6 -6
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;
}
}