fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 10:53: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/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);
}