diff options
| author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-08-23 23:14:59 +0400 |
|---|---|---|
| committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-08-23 23:14:59 +0400 |
| commit | 4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49 (patch) | |
| tree | aaa6c0ead7eb140d0943e3a5737ec24ee939c9f9 /src/modules/uavcan/sensors/gnss.cpp | |
| parent | 6a8971e28f492073a951d96065df30034853bea7 (diff) | |
| download | px4-firmware-4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49.tar.gz px4-firmware-4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49.tar.bz2 px4-firmware-4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49.zip | |
UAVCAN: redundant sensors support
Diffstat (limited to 'src/modules/uavcan/sensors/gnss.cpp')
| -rw-r--r-- | src/modules/uavcan/sensors/gnss.cpp | 15 |
1 files changed, 15 insertions, 0 deletions
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 6b69d163f..f2bb28087 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -73,8 +73,23 @@ int UavcanGnssBridge::init() return res; } +unsigned UavcanGnssBridge::get_num_redundant_channels() const +{ + return (_receiver_node_id < 0) ? 0 : 1; +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg) { + // This bridge does not support redundant GNSS receivers yet. + if (_receiver_node_id < 0) { + _receiver_node_id = msg.getSrcNodeID().get(); + warnx("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. + } + } + _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; _report.lon = msg.lon_1e7; |