diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index 86af45774c2c..753413554966 100644 --- a/boards/ark/can-gps/default.px4board +++ b/boards/ark/can-gps/default.px4board @@ -14,6 +14,7 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION=y CONFIG_UAVCANNODE_GNSS_FIX=y CONFIG_UAVCANNODE_LIGHTS_COMMAND=y CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index b74e74a63078..eade2dc338e6 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/boards/ark/can-rtk-gps/default.px4board @@ -14,6 +14,7 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION=y CONFIG_UAVCANNODE_GNSS_FIX=y CONFIG_UAVCANNODE_LIGHTS_COMMAND=y CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y diff --git a/src/drivers/uavcannode/Kconfig b/src/drivers/uavcannode/Kconfig index 1e504ae201f6..982ab703fa20 100644 --- a/src/drivers/uavcannode/Kconfig +++ b/src/drivers/uavcannode/Kconfig @@ -29,6 +29,10 @@ if DRIVERS_UAVCANNODE bool "Include flow measurement" default n + config UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION + bool "Include Global Navigation Solution" + default n + config UAVCANNODE_GNSS_FIX bool "Include GNSS fix" default n diff --git a/src/drivers/uavcannode/Publishers/GlobalNavigationSolution.hpp b/src/drivers/uavcannode/Publishers/GlobalNavigationSolution.hpp new file mode 100644 index 000000000000..611734a1e8dd --- /dev/null +++ b/src/drivers/uavcannode/Publishers/GlobalNavigationSolution.hpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 GlobalNavigationSolution : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + GlobalNavigationSolution(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::navigation::GlobalNavigationSolution::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_local_position)), + 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::navigation::GlobalNavigationSolution::getDataTypeFullName(), + id()); + } + } + + void BroadcastAnyUpdates() override + { + using uavcan::navigation::GlobalNavigationSolution; + + // vehicle_local_position -> uavcan::navigation::GlobalNavigationSolution + vehicle_local_position_s localposition; + + if (uORB::SubscriptionCallbackWorkItem::update(&localposition)) { + uavcan::navigation::GlobalNavigationSolution navigationsolution{}; + + //navigationsolution.timestamp = 0; // get uavcan monotonic time? + + if (localposition.xy_global) { + navigationsolution.latitude = localposition.ref_lat; + navigationsolution.longitude = localposition.ref_lon; + } + + if (localposition.z_global) { + //navigationsolution.height_ellipsoid = localposition.alt_ellipsoid; + navigationsolution.height_msl = localposition.ref_alt; + } + + //navigationsolution.height_agl + //navigationsolution.height_baro + + //navigationsolution.qnh_hpa + + //navigationsolution.orientation_xyzw + + //navigationsolution.pose_covariance + + navigationsolution.linear_velocity_body[0] = localposition.vx; + navigationsolution.linear_velocity_body[1] = localposition.vy; + navigationsolution.linear_velocity_body[2] = localposition.vz; + + //navigationsolution.angular_velocity_body + + navigationsolution.linear_acceleration_body[0] = localposition.ax; + navigationsolution.linear_acceleration_body[1] = localposition.ay; + navigationsolution.linear_acceleration_body[2] = localposition.az; + + //navigationsolution.velocity_covariance + + uavcan::Publisher::broadcast(navigationsolution); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 40f1b8316ce0..a1af55ef8535 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -56,6 +56,10 @@ #include "Publishers/HygrometerMeasurement.hpp" #endif // UAVCANNODE_HYGROMETER_MEASUREMENT +#if defined(CONFIG_UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION) +#include "Publishers/GlobalNavigationSolution.hpp" +#endif // CONFIG_UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION + #if defined(CONFIG_UAVCANNODE_GNSS_FIX) #include "Publishers/GnssFix2.hpp" #include "Publishers/GnssAuxiliary.hpp" @@ -370,6 +374,10 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _publisher_list.add(new HygrometerMeasurement(this, _node)); #endif // UAVCANNODE_HYGROMETER_MEASUREMENT +#if defined(CONFIG_UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION) + _publisher_list.add(new GlobalNavigationSolution(this, _node)); +#endif // CONFIG_UAVCANNODE_GLOBAL_NAVIGATION_SOLUTION + #if defined(CONFIG_UAVCANNODE_GNSS_FIX) _publisher_list.add(new GnssFix2(this, _node)); _publisher_list.add(new GnssAuxiliary(this, _node));