fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 06:13:35 +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/drivers/gps/definitions.h
+1 -1
View File
@@ -41,7 +41,7 @@
#include <drivers/drv_hrt.h>
#include <px4_platform_common/defines.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/satellite_info.h>
#define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
src/drivers/gps/devices
+1 -1
Submodule src/drivers/gps/devices updated: bcd57ae239...4d51e5378e
src/drivers/gps/gps.cpp
+3 -2
View File
@@ -59,6 +59,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/gps_dump.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/sensor_gps.h>
#include "devices/src/ashtech.h"
#include "devices/src/emlid_reach.h"
@@ -160,10 +161,10 @@ private:
GPS_Sat_Info *_sat_info{nullptr}; ///< instance of GPS sat info data object
vehicle_gps_position_s _report_gps_pos{}; ///< uORB topic for gps position
sensor_gps_s _report_gps_pos{}; ///< uORB topic for gps position
satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info
uORB::PublicationMulti<vehicle_gps_position_s> _report_gps_pos_pub{ORB_ID(vehicle_gps_position)}; ///< uORB pub for gps position
uORB::PublicationMulti<sensor_gps_s> _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position
uORB::PublicationMulti<satellite_info_s> _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info
float _rate{0.0f}; ///< position update rate
src/drivers/uavcan/sensors/gnss.cpp
+13 -54
View File
@@ -52,13 +52,11 @@ using namespace time_literals;
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(vehicle_gps_position)),
UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(sensor_gps)),
_node(node),
_sub_auxiliary(node),
_sub_fix(node),
_sub_fix2(node),
_pub_fix2(node),
_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)),
_report_pub(nullptr),
_channel_using_fix2(new bool[_max_channels])
{
@@ -81,13 +79,6 @@ UavcanGnssBridge::init()
return res;
}
res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
if (res < 0) {
PX4_WARN("GNSS fix2 pub failed %i", res);
return res;
}
res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb));
if (res < 0) {
@@ -109,8 +100,6 @@ UavcanGnssBridge::init()
return res;
}
_orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
return res;
}
@@ -268,7 +257,18 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
const float (&pos_cov)[9], const float (&vel_cov)[9],
const bool valid_pos_cov, const bool valid_vel_cov)
{
auto report = ::vehicle_gps_position_s();
// This bridge does not support redundant GNSS receivers yet.
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
}
}
sensor_gps_s report{};
/*
* FIXME HACK
@@ -396,44 +396,3 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
publish(msg.getSrcNodeID().get(), &report);
}
void
UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
{
vehicle_gps_position_s orb_msg{};
if (!_orb_sub_gnss.update(&orb_msg)) {
return;
}
// Convert to UAVCAN
using uavcan::equipment::gnss::Fix2;
Fix2 msg;
msg.gnss_timestamp = uavcan::UtcTime::fromUSec(orb_msg.time_utc_usec);
msg.gnss_time_standard = Fix2::GNSS_TIME_STANDARD_UTC;
msg.longitude_deg_1e8 = std::int64_t(orb_msg.lon) * 10LL;
msg.latitude_deg_1e8 = std::int64_t(orb_msg.lat) * 10LL;
msg.height_ellipsoid_mm = orb_msg.alt_ellipsoid;
msg.height_msl_mm = orb_msg.alt;
msg.ned_velocity[0] = orb_msg.vel_n_m_s;
msg.ned_velocity[1] = orb_msg.vel_e_m_s;
msg.ned_velocity[2] = orb_msg.vel_d_m_s;
msg.sats_used = orb_msg.satellites_used;
msg.status = orb_msg.fix_type;
// mode skipped
// sub mode skipped
// diagonal covariance matrix
msg.covariance.resize(2, orb_msg.eph * orb_msg.eph);
msg.covariance.resize(3, orb_msg.epv * orb_msg.epv);
msg.covariance.resize(6, orb_msg.s_variance_m_s * orb_msg.s_variance_m_s);
msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop; // this is a hack :(
// Publishing now
_pub_fix2.broadcast(msg);
}
src/drivers/uavcan/sensors/gnss.hpp
+2 -9
View File
@@ -46,7 +46,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
@@ -82,8 +82,6 @@ private:
const float (&pos_cov)[9], const float (&vel_cov)[9],
const bool valid_pos_cov, const bool valid_vel_cov);
void broadcast_from_orb(const uavcan::TimerEvent &);
typedef uavcan::MethodBinder < UavcanGnssBridge *,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
AuxiliaryCbBinder;
@@ -106,16 +104,11 @@ private:
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
uavcan::Publisher<uavcan::equipment::gnss::Fix2> _pub_fix2;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_to_uavcan_pub_timer;
uint64_t _last_gnss_auxiliary_timestamp{0};
float _last_gnss_auxiliary_hdop{0.0f};
float _last_gnss_auxiliary_vdop{0.0f};
uORB::PublicationMulti<vehicle_gps_position_s> _gps_pub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _orb_sub_gnss{ORB_ID(vehicle_gps_position)};
uORB::PublicationMulti<sensor_gps_s> _gps_pub{ORB_ID(sensor_gps)};
int _receiver_node_id{-1};
bool _old_fix_subscriber_active{true};