-
Notifications
You must be signed in to change notification settings - Fork 13.2k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Move GPS aggregation and blending to sensors #14447
Merged
Merged
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,19 +1,36 @@ | ||
# EKF blended position in WGS84 coordinates. | ||
# GPS position in WGS84 coordinates. | ||
# the field 'timestamp' is for the position & velocity (microseconds) | ||
uint64 timestamp # time since system start (microseconds) | ||
|
||
int32 lat # Latitude in 1E-7 degrees | ||
int32 lon # Longitude in 1E-7 degrees | ||
int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) | ||
int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) | ||
|
||
float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) | ||
float32 c_variance_rad # GPS course accuracy estimate, (radians) | ||
uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. | ||
|
||
float32 eph # GPS horizontal position accuracy (metres) | ||
float32 epv # GPS vertical position accuracy (metres) | ||
|
||
float32 hdop # Horizontal dilution of precision | ||
float32 vdop # Vertical dilution of precision | ||
|
||
int32 noise_per_ms # GPS noise per millisecond | ||
int32 jamming_indicator # indicates jamming is occurring | ||
|
||
float32 vel_m_s # GPS ground speed, (metres/sec) | ||
float32 vel_n_m_s # GPS North velocity, (metres/sec) | ||
float32 vel_e_m_s # GPS East velocity, (metres/sec) | ||
float32 vel_d_m_s # GPS Down velocity, (metres/sec) | ||
float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) | ||
bool vel_ned_valid # True if NED velocity is valid | ||
|
||
int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) | ||
uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 | ||
|
||
uint8 satellites_used # Number of satellites used | ||
|
||
float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) | ||
float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) | ||
uint8 selected # GPS selection: 0: GPS1, 1: GPS2. 2: GPS1+GPS2 blend |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Submodule devices
updated
from bcd57a to 4d51e5
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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)), | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. In the future we should consider removing the use of CDev as a base class for UavcanSensorBridge; I don't think the CDev interfaces are used anymore. If they are, then ideally those sensor types would have a generic interface class like PX4Barometer / PX4Magnetometer. |
||
_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); | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
as @mhkabir said, it's the right time to fix #6275
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The discussion doesn't seem finished, are you suggesting the simple naming change?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'd be happy to implement additional fixes, however I don't fully understand what changes you guys are requesting as they aren't part of the direct subject matter of this PR. I suggested @mhkabir to push a commit up to fix them as he clearly knows exactly what he'd like done, as I see he has a PR (which is quite large) that is referenced in the issue #6275
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
s_accuracy_m_s
andc_accuracy_rad
are good names IMOThere was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This has to be changed in coordination with the GpsDrivers submodule. https://github.com/PX4/GpsDrivers
Let's do it separately.