diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl index 993be80a62..b8f70ec5d8 160000 --- a/src/drivers/uavcan/libdronecan/dsdl +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -1 +1 @@ -Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 +Subproject commit b8f70ec5d811186cc848052c8417e63b148e8c3a diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index fb02f1d274..21e8282c7d 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -59,14 +59,17 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_ _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), + _sub_fix3(node), _sub_gnss_heading(node), _sub_moving_baseline_data(node), _pub_moving_baseline_data(node), _pub_rtcm_stream(node), - _channel_using_fix2(new bool[_max_channels]) + _channel_using_fix2(new bool[_max_channels]), + _channel_using_fix3(new bool[_max_channels]) { for (uint8_t i = 0; i < _max_channels; i++) { _channel_using_fix2[i] = false; + _channel_using_fix3[i] = false; } set_device_type(DRV_GPS_DEVTYPE_UAVCAN); @@ -75,6 +78,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_ UavcanGnssBridge::~UavcanGnssBridge() { delete [] _channel_using_fix2; + delete [] _channel_using_fix3; perf_free(_rtcm_stream_pub_perf); perf_free(_moving_baseline_data_pub_perf); perf_free(_moving_baseline_data_sub_perf); @@ -104,6 +108,13 @@ UavcanGnssBridge::init() return res; } + res = _sub_fix3.start(Fix3CbBinder(this, &UavcanGnssBridge::gnss_fix3_sub_cb)); + + if (res < 0) { + PX4_WARN("GNSS fix3 sub failed %i", res); + return res; + } + res = _sub_gnss_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssBridge::gnss_relative_sub_cb)); if (res < 0) { @@ -155,11 +166,11 @@ UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg) { - // Check to see if this node is also publishing a Fix2 message. + // Check to see if this node is also publishing a Fix2 or Fix3 message. // If so, ignore the old "Fix" message for this node. const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get()); - if (ch > -1 && _channel_using_fix2[ch]) { + if (ch > -1 && (_channel_using_fix2[ch] || _channel_using_fix3[ch])) { return; } @@ -184,6 +195,11 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure -1 && _channel_using_fix3[ch]) { + return; + } + if (ch > -1 && !_channel_using_fix2[ch]) { PX4_WARN("GNSS Fix2 msg detected for ch %d; disabling Fix msg for this node", ch); _channel_using_fix2[ch] = true; @@ -350,6 +366,167 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + using uavcan::equipment::gnss::Fix3; + + const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get()); + + if (ch > -1 && !_channel_using_fix3[ch]) { + PX4_WARN("GNSS Fix3 msg detected for ch %d; disabling Fix/Fix2 msgs for this node", ch); + _channel_using_fix3[ch] = true; + } + + sensor_gps_s sensor_gps{}; + + sensor_gps.device_id = make_uavcan_device_id(msg); + + // Register GPS capability with NodeInfoPublisher after first successful message + if (_node_info_publisher != nullptr) { + _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), + sensor_gps.device_id, + NodeInfoPublisher::DeviceCapability::GPS); + } + + sensor_gps.timestamp = hrt_absolute_time(); + + // Position - Fix3 uses 1e8 scaling for lat/lon, mm for altitudes + sensor_gps.latitude_deg = msg.latitude_deg_1e8 / 1e8; + sensor_gps.longitude_deg = msg.longitude_deg_1e8 / 1e8; + sensor_gps.altitude_msl_m = msg.altitude_msl / 1e3; // mm to m + sensor_gps.altitude_ellipsoid_m = msg.altitude_ellipsoid / 1e3; // mm to m + + sensor_gps.eph = (msg.eph >= 0) ? msg.eph : -1.0f; + sensor_gps.epv = (msg.epv >= 0) ? msg.epv : -1.0f; + + // Velocity + sensor_gps.vel_n_m_s = msg.vel_north; + sensor_gps.vel_e_m_s = msg.vel_east; + sensor_gps.vel_d_m_s = msg.vel_down; + sensor_gps.vel_m_s = (msg.ground_speed >= 0 && !std::isnan(msg.ground_speed)) ? + msg.ground_speed : matrix::Vector3f(msg.vel_north, msg.vel_east, msg.vel_down).norm(); + sensor_gps.vel_ned_valid = (msg.flags & Fix3::FLAGS_VEL_NED_VALID) != 0; + + // Speed accuracy + sensor_gps.s_variance_m_s = (msg.speed_accuracy >= 0) ? (msg.speed_accuracy * msg.speed_accuracy) : -1.0f; + + // Course over ground + if (!std::isnan(msg.cog)) { + sensor_gps.cog_rad = math::radians(msg.cog); + + } else { + sensor_gps.cog_rad = atan2f(sensor_gps.vel_e_m_s, sensor_gps.vel_n_m_s); + } + + // Course variance from velocity (same calculation as process_fixx) + if (sensor_gps.s_variance_m_s > 0) { + float vel_n = msg.vel_north; + float vel_e = msg.vel_east; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + float speed_sq = vel_n_sq + vel_e_sq; + + if (speed_sq > 0.01f) { // Only calculate if moving + sensor_gps.c_variance_rad = sensor_gps.s_variance_m_s / speed_sq; + + } else { + sensor_gps.c_variance_rad = -1.0f; + } + + } else { + sensor_gps.c_variance_rad = -1.0f; + } + + // Fix type mapping + sensor_gps.fix_type = msg.fix_type; + + // DOP values + sensor_gps.hdop = std::isnan(msg.hdop) ? 0.0f : msg.hdop; + sensor_gps.vdop = std::isnan(msg.vdop) ? 0.0f : msg.vdop; + + // Satellite count + sensor_gps.satellites_used = msg.sats_used; + + // Time handling + sensor_gps.timestamp_time_relative = 0; + + if ((msg.flags & Fix3::FLAGS_TIME_VALID) && msg.gnss_time_usec > 0) { + switch (msg.time_standard) { + case Fix3::TIME_STANDARD_UTC: + sensor_gps.time_utc_usec = msg.gnss_time_usec; + break; + + case Fix3::TIME_STANDARD_GPS: + if (msg.num_leap_seconds > 0) { + sensor_gps.time_utc_usec = msg.gnss_time_usec - (uint64_t)(msg.num_leap_seconds - 9) * 1000000ULL; + } + + break; + + case Fix3::TIME_STANDARD_TAI: + if (msg.num_leap_seconds > 0) { + sensor_gps.time_utc_usec = msg.gnss_time_usec - (uint64_t)(msg.num_leap_seconds + 10) * 1000000ULL; + } + + break; + + default: + break; + } + + // Set system clock if not already done + if (sensor_gps.time_utc_usec != 0 && (msg.fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) { + timespec ts{}; + ts.tv_sec = sensor_gps.time_utc_usec / 1000000ULL; + ts.tv_nsec = (sensor_gps.time_utc_usec % 1000000ULL) * 1000; + px4_clock_settime(CLOCK_REALTIME, &ts); + _system_clock_set = true; + } + } + + // Heading - Fix3 provides direct degrees + if ((msg.flags & Fix3::FLAGS_HEADING_VALID) && !std::isnan(msg.heading)) { + // Use RelPosHeading if available and we have RTK Fixed solution + if (_rel_heading_valid && (msg.fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) { + sensor_gps.heading = _rel_heading; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = _rel_heading_accuracy; + + _rel_heading = NAN; + _rel_heading_accuracy = NAN; + _rel_heading_valid = false; + + } else { + sensor_gps.heading = math::radians(msg.heading); + sensor_gps.heading_offset = NAN; // Fix3 doesn't have offset field + sensor_gps.heading_accuracy = std::isnan(msg.heading_accuracy) ? NAN : math::radians(msg.heading_accuracy); + } + + } else { + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = NAN; + } + + // Quality metrics - Fix3 has dedicated fields + sensor_gps.noise_per_ms = msg.noise; + sensor_gps.automatic_gain_control = msg.agc; + sensor_gps.jamming_indicator = msg.jamming_indicator; + sensor_gps.jamming_state = msg.jamming_state; + sensor_gps.spoofing_state = msg.spoofing_state; + sensor_gps.authentication_state = msg.auth_state; + + // System errors + sensor_gps.system_error = msg.system_errors; + + // RTCM info + sensor_gps.selected_rtcm_instance = _selected_rtcm_instance; + sensor_gps.rtcm_injection_rate = _rtcm_injection_rate; + + publish(msg.getSrcNodeID().get(), &sensor_gps); +} + void UavcanGnssBridge::gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg) { diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 72fe9b3453..fb2cc79f40 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -37,6 +37,7 @@ * UAVCAN <--> ORB bridge for GNSS messages: * uavcan.equipment.gnss.Fix (deprecated, but still supported for backward compatibility) * uavcan.equipment.gnss.Fix2 + * uavcan.equipment.gnss.Fix3 (preferred, uses direct EPH/EPV fields) * * @author Pavel Kirienko * @author Andrew Chambers @@ -55,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -84,6 +86,7 @@ private: void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); + void gnss_fix3_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg); void moving_baseline_data_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -114,6 +117,10 @@ private: void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > Fix2CbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + Fix3CbBinder; + typedef uavcan::MethodBinder TimerCbBinder; @@ -131,6 +138,7 @@ private: uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; + uavcan::Subscriber _sub_fix3; uavcan::Subscriber _sub_gnss_heading; // Used for MSM7 logging for PPK workflows @@ -152,6 +160,7 @@ private: bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg + bool *_channel_using_fix3; ///< Flag for whether each channel is using Fix3 (takes priority over Fix2 and Fix) bool _publish_rtcm_stream{false}; bool _publish_moving_baseline_data{false}; diff --git a/src/drivers/uavcannode/Publishers/GnssFix3.hpp b/src/drivers/uavcannode/Publishers/GnssFix3.hpp new file mode 100644 index 0000000000..18ad93a065 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/GnssFix3.hpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2024 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 + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class GnssFix3 : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + GnssFix3(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::gnss::Fix3::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)), + uavcan::Publisher(node) + { + this->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + } + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::gnss::Fix3::getDataTypeFullName(), + id()); + } + } + + void BroadcastAnyUpdates() override + { + using uavcan::equipment::gnss::Fix3; + + // sensor_gps -> uavcan::equipment::gnss::Fix3 + sensor_gps_s gps; + + if (uORB::SubscriptionCallbackWorkItem::update(&gps)) { + uavcan::equipment::gnss::Fix3 fix3{}; + + // Time + fix3.gnss_time_usec = gps.time_utc_usec; + fix3.time_standard = Fix3::TIME_STANDARD_UTC; + fix3.num_leap_seconds = 0; // Unknown + + // Position - scale to 1e8 and mm + fix3.latitude_deg_1e8 = (int64_t)(gps.latitude_deg * 1e8); + fix3.longitude_deg_1e8 = (int64_t)(gps.longitude_deg * 1e8); + fix3.altitude_msl = (int32_t)(gps.altitude_msl_m * 1e3); // m to mm + fix3.altitude_ellipsoid = (int32_t)(gps.altitude_ellipsoid_m * 1e3); // m to mm + + // Velocity + fix3.vel_north = gps.vel_n_m_s; + fix3.vel_east = gps.vel_e_m_s; + fix3.vel_down = gps.vel_d_m_s; + fix3.ground_speed = gps.vel_m_s; + fix3.eph = gps.eph; + fix3.epv = gps.epv; + + // Speed accuracy - convert variance to std dev + fix3.speed_accuracy = (gps.s_variance_m_s > 0) ? sqrtf(gps.s_variance_m_s) : -1.0f; + + // Heading - convert rad to deg + if (!std::isnan(gps.heading)) { + fix3.heading = math::degrees(gps.heading); + fix3.heading_accuracy = std::isnan(gps.heading_accuracy) ? NAN : math::degrees(gps.heading_accuracy); + + } else { + fix3.heading = NAN; + fix3.heading_accuracy = NAN; + } + + // Course over ground - convert rad to deg + fix3.cog = math::degrees(gps.cog_rad); + + // DOP + fix3.hdop = gps.hdop; + fix3.vdop = gps.vdop; + fix3.pdop = (gps.hdop > gps.vdop) ? gps.hdop : gps.vdop; // Approximate PDOP + + // Fix type + fix3.fix_type = gps.fix_type; + + // Fix quality - derive from fix type if not available + switch (gps.fix_type) { + case sensor_gps_s::FIX_TYPE_NONE: + fix3.fix_quality = 0; + break; + + case sensor_gps_s::FIX_TYPE_2D: + fix3.fix_quality = 30; + break; + + case sensor_gps_s::FIX_TYPE_3D: + fix3.fix_quality = 60; + break; + + case sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL: + fix3.fix_quality = 75; + break; + + case sensor_gps_s::FIX_TYPE_RTK_FLOAT: + fix3.fix_quality = 85; + break; + + case sensor_gps_s::FIX_TYPE_RTK_FIXED: + fix3.fix_quality = 100; + break; + + default: + fix3.fix_quality = 0; + break; + } + + // Satellite counts + fix3.sats_used = gps.satellites_used; + fix3.sats_visible = gps.satellites_used; // We don't have visible count separately + + // Differential correction age - not available in sensor_gps + fix3.diff_age = 0xFFFF; // Unavailable + + // Signal quality metrics + fix3.noise = gps.noise_per_ms; + fix3.agc = gps.automatic_gain_control; + fix3.jamming_state = gps.jamming_state; + fix3.jamming_indicator = gps.jamming_indicator; + fix3.spoofing_state = gps.spoofing_state; + fix3.auth_state = gps.authentication_state; + + // System errors + fix3.system_errors = gps.system_error; + + // Flags + fix3.flags = 0; + + if (gps.vel_ned_valid) { + fix3.flags |= Fix3::FLAGS_VEL_NED_VALID; + } + + if (!std::isnan(gps.heading)) { + fix3.flags |= Fix3::FLAGS_HEADING_VALID; + } + + if (gps.time_utc_usec != 0) { + fix3.flags |= Fix3::FLAGS_TIME_VALID; + } + + if (gps.fix_type >= sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL) { + fix3.flags |= Fix3::FLAGS_DIFF_CORRECTIONS; + } + + // Assume receiver is healthy if we're getting data + fix3.flags |= Fix3::FLAGS_RECEIVER_HEALTHY; + + // Armable if we have at least a 3D fix + if (gps.fix_type >= sensor_gps_s::FIX_TYPE_3D) { + fix3.flags |= Fix3::FLAGS_ARMABLE; + } + + uavcan::Publisher::broadcast(fix3); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 6d0799fc27..c546cfd0b1 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -58,6 +58,7 @@ #if defined(CONFIG_UAVCANNODE_GNSS_FIX) #include "Publishers/GnssFix2.hpp" +#include "Publishers/GnssFix3.hpp" #include "Publishers/GnssAuxiliary.hpp" #endif // CONFIG_UAVCANNODE_GNSS_FIX @@ -379,7 +380,8 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events #endif // UAVCANNODE_HYGROMETER_MEASUREMENT #if defined(CONFIG_UAVCANNODE_GNSS_FIX) - _publisher_list.add(new GnssFix2(this, _node)); + _publisher_list.add(new GnssFix3(this, _node)); + _publisher_list.add(new GnssFix2(this, _node)); // Keep Fix2 for backwards compatibility _publisher_list.add(new GnssAuxiliary(this, _node)); #endif // CONFIG_UAVCANNODE_GNSS_FIX