From bfd5a90a5d3f947cb4ae56d727cc799a995c7519 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Thu, 7 Oct 2021 22:23:27 -0600 Subject: [PATCH] UAVCAN Moving Baseline Working Set uavcan publisher priorities Switch to ardupilot rtcm message and add heading accuracy --- msg/gps_inject_data.msg | 9 +- msg/sensor_gps.msg | 1 + src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 86 ++++++++++++-- src/drivers/gps/params.c | 12 +- src/drivers/uavcan/beep.cpp | 1 + src/drivers/uavcan/safety_state.cpp | 1 + src/drivers/uavcan/sensors/gnss.cpp | 38 ++++-- src/drivers/uavcan/sensors/gnss.hpp | 8 +- .../uavcannode/Publishers/BatteryInfo.hpp | 4 +- .../uavcannode/Publishers/FlowMeasurement.hpp | 2 + .../uavcannode/Publishers/GnssFix2.hpp | 24 +++- .../Publishers/MagneticFieldStrength2.hpp | 4 +- .../Publishers/MovingBaselineData.hpp | 109 ++++++++++++++++++ .../Publishers/RangeSensorMeasurement.hpp | 4 +- .../uavcannode/Publishers/RawAirData.hpp | 4 +- .../uavcannode/Publishers/SafetyButton.hpp | 4 +- .../uavcannode/Publishers/StaticPressure.hpp | 4 +- .../Publishers/StaticTemperature.hpp | 4 +- ...{RTCMStream.hpp => MovingBaselineData.hpp} | 54 +++++---- src/drivers/uavcannode/UavcanNode.cpp | 6 +- src/modules/mavlink/streams/GPS2_RAW.hpp | 6 +- src/modules/mavlink/streams/GPS_RAW_INT.hpp | 7 +- 23 files changed, 328 insertions(+), 66 deletions(-) create mode 100644 src/drivers/uavcannode/Publishers/MovingBaselineData.hpp rename src/drivers/uavcannode/Subscribers/{RTCMStream.hpp => MovingBaselineData.hpp} (57%) diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg index 66d431143b..1cdaca85ea 100644 --- a/msg/gps_inject_data.msg +++ b/msg/gps_inject_data.msg @@ -1,6 +1,9 @@ uint64 timestamp # time since system start (microseconds) -uint8 len # length of data -uint8 flags # LSB: 1=fragmented -uint8[182] data # data to write to GPS device (RTCM message) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint8 len # length of data +uint8 flags # LSB: 1=fragmented +uint8[300] data # data to write to GPS device (RTCM message) uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_gps.msg b/msg/sensor_gps.msg index 1130a353fa..a02300df03 100644 --- a/msg/sensor_gps.msg +++ b/msg/sensor_gps.msg @@ -38,3 +38,4 @@ 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]) +float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 6fcf068949..bd72eb6794 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 6fcf06894973240d45dc49d3b31565917dc8f2f6 +Subproject commit bd72eb6794e8fb4f2ed3e47311a14c2cec69f60b diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4be1a57176..2f679ca9b7 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -191,11 +191,12 @@ private: const Instance _instance; - uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; - uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; - gps_dump_s *_dump_to_device{nullptr}; - gps_dump_s *_dump_from_device{nullptr}; - gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled}; + uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; + uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; + gps_dump_s *_dump_to_device{nullptr}; + gps_dump_s *_dump_from_device{nullptr}; + gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled}; static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 /// and thus we wait until the first one publishes at least one message. @@ -214,6 +215,11 @@ private: */ void publishSatelliteInfo(); + /** + * Publish RTCM corrections + */ + void publishRTCMCorrections(uint8_t *data, size_t len); + /** * This is an abstraction for the poll on serial used. * @@ -383,6 +389,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) return gps->setBaudrate(data2); case GPSCallbackType::gotRTCMMessage: + gps->publishRTCMCorrections((uint8_t *)data1, (size_t)data2); gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::RTCM, false); break; @@ -504,13 +511,16 @@ void GPS::handleInjectDataTopic() if (_orb_inject_data_sub.copy(&msg)) { - /* Write the message to the gps device. Note that the message could be fragmented. - * But as we don't write anywhere else to the device during operation, we don't - * need to assemble the message first. - */ - injectData(msg.data, msg.len); + // Prevent injection of data from self + if (msg.device_id != get_device_id()) { + /* Write the message to the gps device. Note that the message could be fragmented. + * But as we don't write anywhere else to the device during operation, we don't + * need to assemble the message first. + */ + injectData(msg.data, msg.len); - ++_last_rate_rtcm_injection_count; + ++_last_rate_rtcm_injection_count; + } } } } while (updated && num_injections < max_num_injections); @@ -549,6 +559,8 @@ int GPS::setBaudrate(unsigned baud) case 460800: speed = B460800; break; + case 921600: speed = B921600; break; + default: PX4_ERR("ERR: unknown baudrate: %d", baud); return -EINVAL; @@ -713,6 +725,20 @@ GPS::run() } else { ubx_mode = GPSDriverUBX::UBXMode::MovingBase; } + + } else if (gps_ubx_mode == 2) { + ubx_mode = GPSDriverUBX::UBXMode::MovingBase; + + } else if (gps_ubx_mode == 3) { + if (_instance == Instance::Main) { + ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1; + + } else { + ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; + } + + } else if (gps_ubx_mode == 4) { + ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; } } @@ -869,7 +895,8 @@ GPS::run() int helper_ret; unsigned receive_timeout = TIMEOUT_5HZ; - if (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) { + if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) + || (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1)) { /* The MB rover will wait as long as possible to compute a navigation solution, * possibly lowering the navigation rate all the way to 1 Hz while doing so. */ receive_timeout = TIMEOUT_1HZ; @@ -1115,6 +1142,41 @@ GPS::publishSatelliteInfo() } } +void +GPS::publishRTCMCorrections(uint8_t *data, size_t len) +{ + gps_inject_data_s gps_inject_data{}; + + gps_inject_data.timestamp = hrt_absolute_time(); + gps_inject_data.device_id = get_device_id(); + + size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); + + if (len > capacity) { + gps_inject_data.flags = 1; //LSB: 1=fragmented + + } else { + gps_inject_data.flags = 0; + } + + size_t written = 0; + + while (written < len) { + + gps_inject_data.len = len - written; + + if (gps_inject_data.len > capacity) { + gps_inject_data.len = capacity; + } + + memcpy(gps_inject_data.data, &data[written], gps_inject_data.len); + + _gps_inject_data_pub.publish(gps_inject_data); + + written = written + gps_inject_data.len; + } +} + int GPS::custom_command(int argc, char *argv[]) { diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index ba5dd5f69b..ee4c0f0625 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -76,14 +76,19 @@ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7); * dual GPS without heading. * * The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output - * heading information, whereas the secondary will act as moving base, sending RTCM on UART2 to - * the rover GPS. + * heading information, whereas the secondary will act as moving base. + * Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the + * F9P units are connected to each other. + * Modes 3 and 4 only require UART1 on each F9P connected to the Autopilot or Can Node. UART RX DMA is required. * RTK is still possible with this setup. * * @min 0 * @max 1 * @value 0 Default - * @value 1 Heading + * @value 1 Heading (Rover With Moving Base UART1 Connected To Autopilot, UART2 Connected To Moving Base) + * @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover) + * @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600) + * @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600) * * @reboot_required true * @group GPS @@ -95,7 +100,6 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0); * Heading/Yaw offset for dual antenna GPS * * Heading offset angle for dual antenna GPS setups that support heading estimation. - * (currently only for the Trimble MB-Two). * * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in * front. The offset angle increases clockwise. diff --git a/src/drivers/uavcan/beep.cpp b/src/drivers/uavcan/beep.cpp index 1e4a9818da..51ba5714fc 100644 --- a/src/drivers/uavcan/beep.cpp +++ b/src/drivers/uavcan/beep.cpp @@ -45,6 +45,7 @@ UavcanBeep::UavcanBeep(uavcan::INode &node) : _beep_pub(node), _timer(node) { + _beep_pub.setPriority(uavcan::TransferPriority::Default); } int UavcanBeep::init() diff --git a/src/drivers/uavcan/safety_state.cpp b/src/drivers/uavcan/safety_state.cpp index 3e293c22da..12fef90da7 100644 --- a/src/drivers/uavcan/safety_state.cpp +++ b/src/drivers/uavcan/safety_state.cpp @@ -43,6 +43,7 @@ UavcanSafetyState::UavcanSafetyState(uavcan::INode &node) : _safety_state_pub(node), _timer(node) { + _safety_state_pub.setPriority(uavcan::TransferPriority::Default); } int UavcanSafetyState::init() diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 88e796cfc5..b6eb0509c3 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -98,7 +98,7 @@ UavcanGnssBridge::init() return res; } - _pub_rtcm.setPriority(uavcan::TransferPriority::OneHigherThanLowest); + _pub_rtcm.setPriority(uavcan::TransferPriority::NumericallyMax); return res; } @@ -134,7 +134,7 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &msg, uint8_t fix_type, const float (&pos_cov)[9], const float (&vel_cov)[9], - const bool valid_pos_cov, const bool valid_vel_cov) + const bool valid_pos_cov, const bool valid_vel_cov, + const float heading, const float heading_offset, + const float heading_accuracy) { sensor_gps_s report{}; report.device_id = get_device_id(); @@ -407,8 +427,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.vdop = msg.pdop; } - report.heading = NAN; - report.heading_offset = NAN; + report.heading = heading; + report.heading_offset = heading_offset; + report.heading_accuracy = heading_accuracy; publish(msg.getSrcNodeID().get(), &report); } @@ -455,12 +476,11 @@ void UavcanGnssBridge::handleInjectDataTopic() bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len) { - using uavcan::equipment::gnss::RTCMStream; + using ardupilot::gnss::MovingBaselineData; perf_count(_rtcm_perf); - RTCMStream msg; - msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3; + MovingBaselineData msg; const size_t capacity = msg.data.capacity(); size_t written = 0; diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 00d364a158..620eedfa7f 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include @@ -87,7 +87,9 @@ private: void process_fixx(const uavcan::ReceivedDataStructure &msg, uint8_t fix_type, const float (&pos_cov)[9], const float (&vel_cov)[9], - const bool valid_pos_cov, const bool valid_vel_cov); + const bool valid_pos_cov, const bool valid_vel_cov, + const float heading, const float heading_offset, + const float heading_accuracy); void handleInjectDataTopic(); bool injectData(const uint8_t *data, size_t data_len); @@ -113,7 +115,7 @@ private: uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; - uavcan::Publisher _pub_rtcm; + uavcan::Publisher _pub_rtcm; uint64_t _last_gnss_auxiliary_timestamp{0}; float _last_gnss_auxiliary_hdop{0.0f}; diff --git a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp index ca0a672306..9a68744436 100644 --- a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp +++ b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::power::BatteryInfo::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(battery_status)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::MiddleLower); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp index 86bb65cd15..7780bd85f0 100644 --- a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp @@ -63,6 +63,8 @@ public: if (param_get(rot, &val) == PX4_OK) { _rotation = get_rot_matrix((enum Rotation)val); } + + this->setPriority(uavcan::TransferPriority::Default); } void PrintInfo() override diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp index 5e4f610e09..61623f8942 100644 --- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::gnss::Fix2::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + } void PrintInfo() override { @@ -118,6 +120,26 @@ public: fix2.covariance.push_back(gps.s_variance_m_s); fix2.covariance.push_back(gps.s_variance_m_s); + uavcan::equipment::gnss::ECEFPositionVelocity ecefpositionvelocity{}; + ecefpositionvelocity.velocity_xyz[0] = NAN; + ecefpositionvelocity.velocity_xyz[1] = NAN; + ecefpositionvelocity.velocity_xyz[2] = NAN; + + // Use ecef_position_velocity for now... There is no heading field + if (!isnan(gps.heading)) { + ecefpositionvelocity.velocity_xyz[0] = gps.heading; + + if (!isnan(gps.heading_offset)) { + ecefpositionvelocity.velocity_xyz[1] = gps.heading_offset; + } + + if (!isnan(gps.heading_accuracy)) { + ecefpositionvelocity.velocity_xyz[2] = gps.heading_accuracy; + } + + fix2.ecef_position_velocity.push_back(ecefpositionvelocity); + } + uavcan::Publisher::broadcast(fix2); // ensure callback is registered diff --git a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp index 4e421ae3fc..6b48fc099c 100644 --- a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp +++ b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_mag)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::Default); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp b/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp new file mode 100644 index 0000000000..b23cc24ac9 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class MovingBaselineDataPub : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + MovingBaselineDataPub(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(ardupilot::gnss::MovingBaselineData::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(gps_inject_data)), + uavcan::Publisher(node) + { + this->setPriority(uavcan::TransferPriority::NumericallyMax); + } + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + ardupilot::gnss::MovingBaselineData::getDataTypeFullName(), + id()); + } + } + + void BroadcastAnyUpdates() override + { + using ardupilot::gnss::MovingBaselineData; + + // gps_inject_data -> ardupilot::gnss::MovingBaselineData + gps_inject_data_s inject_data; + + if (uORB::SubscriptionCallbackWorkItem::update(&inject_data)) { + // Prevent republishing rtcm data we received from uavcan + if (inject_data.device_id > uavcan::NodeID::Max) { + ardupilot::gnss::MovingBaselineData movingbaselinedata{}; + + const size_t capacity = movingbaselinedata.data.capacity(); + size_t written = 0; + int result = 0; + + while ((result >= 0) && written < inject_data.len) { + size_t chunk_size = inject_data.len - written; + + if (chunk_size > capacity) { + chunk_size = capacity; + } + + for (size_t i = 0; i < chunk_size; ++i) { + movingbaselinedata.data.push_back(inject_data.data[written]); + written += 1; + } + + result = uavcan::Publisher::broadcast(movingbaselinedata); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + + movingbaselinedata.data.clear(); + } + } + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 06e0fe451f..129bd32c21 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::range_sensor::Measurement::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(distance_sensor), instance), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::Default); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Publishers/RawAirData.hpp b/src/drivers/uavcannode/Publishers/RawAirData.hpp index 98035e3a46..c5253b4f6e 100644 --- a/src/drivers/uavcannode/Publishers/RawAirData.hpp +++ b/src/drivers/uavcannode/Publishers/RawAirData.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::air_data::RawAirData::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(differential_pressure)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::Default); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Publishers/SafetyButton.hpp b/src/drivers/uavcannode/Publishers/SafetyButton.hpp index 59b4ee6a3b..69fad19e2a 100644 --- a/src/drivers/uavcannode/Publishers/SafetyButton.hpp +++ b/src/drivers/uavcannode/Publishers/SafetyButton.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(ardupilot::indication::Button::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::Default); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Publishers/StaticPressure.hpp b/src/drivers/uavcannode/Publishers/StaticPressure.hpp index baab9c123e..5b7d745493 100644 --- a/src/drivers/uavcannode/Publishers/StaticPressure.hpp +++ b/src/drivers/uavcannode/Publishers/StaticPressure.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::air_data::StaticPressure::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_baro)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::Default); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Publishers/StaticTemperature.hpp b/src/drivers/uavcannode/Publishers/StaticTemperature.hpp index 5216488e98..3a80c25dd3 100644 --- a/src/drivers/uavcannode/Publishers/StaticTemperature.hpp +++ b/src/drivers/uavcannode/Publishers/StaticTemperature.hpp @@ -53,7 +53,9 @@ public: UavcanPublisherBase(uavcan::equipment::air_data::StaticTemperature::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_baro)), uavcan::Publisher(node) - {} + { + this->setPriority(uavcan::TransferPriority::MiddleLower); + } void PrintInfo() override { diff --git a/src/drivers/uavcannode/Subscribers/RTCMStream.hpp b/src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp similarity index 57% rename from src/drivers/uavcannode/Subscribers/RTCMStream.hpp rename to src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp index f05b90fe64..80a2af1c42 100644 --- a/src/drivers/uavcannode/Subscribers/RTCMStream.hpp +++ b/src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp @@ -35,34 +35,35 @@ #include "UavcanSubscriberBase.hpp" -#include +#include +#include #include #include namespace uavcannode { -class RTCMStream; +class MovingBaselineData; -typedef uavcan::MethodBinder&)> - RTCMStreamBinder; +typedef uavcan::MethodBinder&)> + MovingBaselineDataBinder; -class RTCMStream : +class MovingBaselineData : public UavcanSubscriberBase, - private uavcan::Subscriber + private uavcan::Subscriber { public: - RTCMStream(uavcan::INode &node) : - UavcanSubscriberBase(uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID), - uavcan::Subscriber(node) + MovingBaselineData(uavcan::INode &node) : + UavcanSubscriberBase(ardupilot::gnss::MovingBaselineData::DefaultDataTypeID), + uavcan::Subscriber(node) {} bool init() { - if (start(RTCMStreamBinder(this, &RTCMStream::callback)) < 0) { - PX4_ERR("uavcan::equipment::gnss::RTCMStream subscription failed"); + if (start(MovingBaselineDataBinder(this, &MovingBaselineData::callback)) < 0) { + PX4_ERR("ardupilot::gnss::MovingBaselineData subscription failed"); return false; } @@ -72,23 +73,34 @@ public: void PrintInfo() const override { printf("\t%s:%d -> %s\n", - uavcan::equipment::gnss::RTCMStream::getDataTypeFullName(), - uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID, + ardupilot::gnss::MovingBaselineData::getDataTypeFullName(), + ardupilot::gnss::MovingBaselineData::DefaultDataTypeID, _gps_inject_data_pub.get_topic()->o_name); } private: - void callback(const uavcan::ReceivedDataStructure &msg) + void callback(const uavcan::ReceivedDataStructure &msg) { - gps_inject_data_s gps_inject_data{}; + // Don't republish a message from ourselves + if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) { + gps_inject_data_s gps_inject_data{}; - gps_inject_data.len = msg.data.size(); + gps_inject_data.len = msg.data.size(); - //gps_inject_data.flags = gps_rtcm_data_msg.flags; - memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len); + memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len); - gps_inject_data.timestamp = hrt_absolute_time(); - _gps_inject_data_pub.publish(gps_inject_data); + gps_inject_data.timestamp = hrt_absolute_time(); + + union device::Device::DeviceId device_id; + + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; + device_id.devid_s.address = msg.getSrcNodeID().get(); + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN; + + gps_inject_data.device_id = device_id.devid; + + _gps_inject_data_pub.publish(gps_inject_data); + } } uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index de4db64e3e..92120b111e 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -46,13 +46,14 @@ #include "Publishers/MagneticFieldStrength2.hpp" #include "Publishers/RangeSensorMeasurement.hpp" #include "Publishers/RawAirData.hpp" +#include "Publishers/MovingBaselineData.hpp" #include "Publishers/SafetyButton.hpp" #include "Publishers/StaticPressure.hpp" #include "Publishers/StaticTemperature.hpp" #include "Subscribers/BeepCommand.hpp" #include "Subscribers/LightsCommand.hpp" -#include "Subscribers/RTCMStream.hpp" +#include "Subscribers/MovingBaselineData.hpp" using namespace time_literals; @@ -303,13 +304,14 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _publisher_list.add(new MagneticFieldStrength2(this, _node)); _publisher_list.add(new RangeSensorMeasurement(this, _node)); _publisher_list.add(new RawAirData(this, _node)); + _publisher_list.add(new MovingBaselineDataPub(this, _node)); _publisher_list.add(new SafetyButton(this, _node)); _publisher_list.add(new StaticPressure(this, _node)); _publisher_list.add(new StaticTemperature(this, _node)); _subscriber_list.add(new BeepCommand(_node)); _subscriber_list.add(new LightsCommand(_node)); - _subscriber_list.add(new RTCMStream(_node)); + _subscriber_list.add(new MovingBaselineData(_node)); for (auto &subscriber : _subscriber_list) { subscriber->init(); diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index 754740780a..3c2e06067d 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -90,7 +90,11 @@ private: msg.yaw = 36000; // Use 36000 for north. } else { - msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees + msg.yaw = math::degrees(matrix::wrap_2pi(gps.heading)) * 100.0f; // centidegrees + } + + if (PX4_ISFINITE(gps.heading_accuracy)) { + msg.hdg_acc = math::degrees(gps.heading_accuracy) * 1e5f; // Heading / track uncertainty in degE5 } } diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index 80c17eccf8..8c3aeb12c9 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -85,14 +85,17 @@ private: msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm msg.v_acc = gps.epv * 1e3f; // altitude uncertainty in mm msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm - msg.hdg_acc = math::degrees(gps.c_variance_rad) * 1e5f; // Heading / track uncertainty in degE5 if (PX4_ISFINITE(gps.heading)) { if (fabsf(gps.heading) < FLT_EPSILON) { msg.yaw = 36000; // Use 36000 for north. } else { - msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees + msg.yaw = math::degrees(matrix::wrap_2pi(gps.heading)) * 100.0f; // centidegrees + } + + if (PX4_ISFINITE(gps.heading_accuracy)) { + msg.hdg_acc = math::degrees(gps.heading_accuracy) * 1e5f; // Heading / track uncertainty in degE5 } }