uavcan_v1: Add Publisher base; Gnss Publisher

src/drivers/uavcan_v1/Actuators/EscClient.hpp

@@ -61,6 +61,7 @@ public:
 	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
 	static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX;
 
+	/// TODO: derive from Publisher class to handle port-ID setting via parameter
 	UavcanEscController(CanardInstance &ins, const CanardPortID &baseID) :
 		_canard_instance(ins), _base_port_id(baseID) {};
 

src/drivers/uavcan_v1/ParamManager.hpp

@@ -62,16 +62,15 @@ public:
 	bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value);
 
 private:
-	/// TODO:
-	/// use qsort() to order alphabetically by UAVCAN name
-	/// copy over Ken's parameter find/get/set code
-	const UavcanParamBinder _uavcan_params[7] {
-		{"uavcan.pub.esc.0.id",   "UCAN1_ESC0_PID"},
-		{"uavcan.pub.servo.0.id", "UCAN1_SERVO0_PID"},
+
+	const UavcanParamBinder _uavcan_params[8] {
+		{"uavcan.pub.esc.0.id",   "UCAN1_ESC_PUB"},
+		{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
+		{"uavcan.pub.gps.0.id",   "UCAN1_GPS_PUB"},
+		{"uavcan.sub.esc.0.id",   "UCAN1_ESC_PID"},
 		{"uavcan.sub.gps.0.id",   "UCAN1_GPS0_PID"},
 		{"uavcan.sub.gps.1.id",   "UCAN1_GPS1_PID"},
 		{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_PID"},
 		{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_PID"},
-		{"uavcan.sub.esc.0.id",   "UCAN1_ESC_PID"},
 	};
 };

src/drivers/uavcan_v1/Publishers/Gnss.hpp

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Gnss.hpp
+ *
+ * Defines basic functionality of UAVCAN v1 GNSS publisher
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+// DS-15 Specification Messages
+#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
+#include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>
+
+#include "Publisher.hpp"
+
+class UavcanGnssPublication : public UavcanPublication
+{
+public:
+	UavcanGnssPublication(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
+		UavcanPublication(ins, pmgr, uavcan_pname)
+	{
+
+	};
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() override
+	{
+		if (_gps_sub.updated() && _port_id > 0) {
+			sensor_gps_s gps {};
+			_gps_sub.update(&gps);
+
+			reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
+			geo.latitude = gps.lat;
+			geo.longitude = gps.lon;
+			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast<double>(gps.alt) };
+
+			uint8_t geo_payload_buffer[reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			CanardTransfer transfer = {
+				.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id,
+				.payload_size   = reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				.payload        = &geo_payload_buffer,
+			};
+
+			int32_t result = reg_drone_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer,
+					 &transfer.payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = canardTxPush(&_canard_instance, &transfer);
+			}
+
+			/// TODO: Also publish DilutionOfPrecision, ...?
+			reg_drone_service_gnss_DilutionOfPrecision_0_1 dop {
+				.geometric = NAN,
+				.position = NAN,
+				.horizontal = gps.hdop,
+				.vertical = gps.vdop,
+				.time = (float)gps.time_utc_usec / 1.e6f, /// ?
+				.northing = NAN,
+				.easting = NAN,
+			};
+
+			uint8_t dop_payload_buffer[reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			CanardPortID _port_id_2 = static_cast<CanardPortID>((uint16_t)_port_id + 1U);
+
+			CanardTransfer transfer2 = {
+				.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id_2, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id_2,
+				.payload_size   = reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				.payload        = &dop_payload_buffer,
+			};
+
+			result = reg_drone_service_gnss_DilutionOfPrecision_0_1_serialize_(&dop, dop_payload_buffer, &transfer2.payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id_2;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = canardTxPush(&_canard_instance, &transfer2);
+			}
+		}
+	};
+
+private:
+
+	/// TODO: Allow >1 instance
+	uORB::Subscription _gps_sub{ORB_ID(sensor_gps)};
+	CanardTransferID _transfer_id_2 {0};
+};

src/drivers/uavcan_v1/Publishers/Publisher.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Publisher.hpp
+ *
+ * Defines basic functionality of UAVCAN v1 publisher class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/parameters/param.h>
+
+#include <uavcan/_register/Access_1_0.h>
+
+#include "../CanardInterface.hpp"
+#include "../ParamManager.hpp"
+
+class UavcanPublication
+{
+public:
+	UavcanPublication(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
+		_canard_instance(ins), _param_manager(pmgr), _uavcan_param(uavcan_pname) { };
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() = 0;
+
+	CanardPortID id() { return _port_id; };
+
+	void updateParam()
+	{
+		// Set _port_id from _uavcan_param
+		uavcan_register_Value_1_0 value;
+		_param_manager.GetParamByName(_uavcan_param, value);
+		int32_t new_id = value.integer32.value.elements[0];
+
+		if (_port_id != new_id) {
+			if (new_id == 0) {
+				PX4_INFO("Disabling publication of %s", _uavcan_param);
+
+			} else {
+				_port_id = (CanardPortID)new_id;
+				PX4_INFO("Enabling %s on port %d", _uavcan_param, _port_id);
+			}
+		}
+	};
+
+	void printInfo()
+	{
+		if (_port_id > 0) {
+			PX4_INFO("Enabled %s on port %d", _uavcan_param, _port_id);
+		}
+	}
+
+protected:
+	CanardInstance &_canard_instance;
+	UavcanParamManager &_param_manager;
+	CanardRxSubscription _canard_sub;
+	const char *_uavcan_param; // Port ID parameter
+
+	CanardPortID _port_id {0};
+	CanardTransferID _transfer_id {0};
+};

src/drivers/uavcan_v1/Subscribers/Gnss.hpp

@@ -46,10 +46,10 @@
 
 #include "Subscriber.hpp"
 
-class UavcanGpsSubscription : public UavcanSubscription
+class UavcanGnssSubscription : public UavcanSubscription
 {
 public:
-	UavcanGpsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
+	UavcanGnssSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
 		UavcanSubscription(ins, pmgr, uavcan_pname) { };
 
 	void subscribe() override

src/drivers/uavcan_v1/Subscribers/Subscriber.hpp

@@ -42,22 +42,10 @@
 #pragma once
 
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/atomic.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
 
 #include <lib/parameters/param.h>
 
 #include <uavcan/_register/Access_1_0.h>
-#include <uavcan/_register/List_1_0.h>
-#include <uavcan/_register/Value_1_0.h>
-#include <uavcan/primitive/Empty_1_0.h>
-
-//Quick and Dirty PNP imlementation only V1 for now as well
-#include <uavcan/node/ID_1_0.h>
-#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
-#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
 
 #include "../CanardInterface.hpp"
 #include "../ParamManager.hpp"

src/drivers/uavcan_v1/Uavcan.cpp

@@ -83,6 +83,10 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 
 	PX4_INFO("main _canard_instance = %p", &_canard_instance);
 
+	for (auto &publisher : _publishers) {
+		publisher->updateParam();
+	}
+
 	for (auto &subscriber : _subscribers) {
 		subscriber->updateParam();
 	}
@@ -229,6 +233,12 @@ void UavcanNode::Run()
 		// update parameters from storage
 		updateParams();
 
+		for (auto &publisher : _publishers) {
+			// Have the publisher update its associated port-id parameter
+			// Setting to 0 disable publication
+			publisher->updateParam();
+		}
+
 		for (auto &subscriber : _subscribers) {
 			// Have the subscriber update its associated port-id parameter
 			// If the port-id changes, (re)start the subscription
@@ -240,8 +250,6 @@ void UavcanNode::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 
-
-
 	if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s &&
 	    _node_register_setup != CANARD_NODE_ID_UNSET &&
 	    _node_register_request_index == _node_register_last_received_index + 1) {
@@ -278,6 +286,11 @@ void UavcanNode::Run()
 	// send uavcan::node::Heartbeat_1_0 @ 1 Hz
 	sendHeartbeat();
 
+	// Check all publishers
+	for (auto &publisher : _publishers) {
+		publisher->update();
+	}
+
 	// Transmitting
 	// Look at the top of the TX queue.
 	for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
@@ -381,6 +394,10 @@ void UavcanNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
 
+	for (auto &publisher : _publishers) {
+		publisher->printInfo();
+	}
+
 	for (auto &subscriber : _subscribers) {
 		subscriber->printInfo();
 	}

src/drivers/uavcan_v1/Uavcan.hpp

@@ -77,11 +77,15 @@
 #define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_
 
 #include "CanardInterface.hpp"
+
 #include "Publishers/Publisher.hpp"
+#include "Publishers/Gnss.hpp"
+
 #include "Subscribers/Subscriber.hpp"
 #include "Subscribers/Battery.hpp"
 #include "Subscribers/Esc.hpp"
 #include "Subscribers/Gnss.hpp"
+
 #include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
 
 /**
@@ -230,15 +234,20 @@ private:
 	)
 
 	UavcanParamManager _param_manager;
-	UavcanGpsSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"};
-	UavcanGpsSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"};
-	UavcanBmsSubscription _bms0_sub {_canard_instance, _param_manager, "uavcan.sub.bms.0.id"};
-	UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"};
-	UavcanEscSubscription _esc_sub  {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"};
+
+	UavcanGnssPublication _gps_pub {_canard_instance, _param_manager, "uavcan.pub.gps.0.id"};
+
+	UavcanPublication *_publishers[1] {&_gps_pub};
+
+	UavcanGnssSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"};
+	UavcanGnssSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"};
+	UavcanBmsSubscription  _bms0_sub {_canard_instance, _param_manager, "uavcan.sub.bms.0.id"};
+	UavcanBmsSubscription  _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"};
+	UavcanEscSubscription  _esc_sub  {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"};
 
 	UavcanSubscription *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
 
-	UavcanEscController _esc_controller {_canard_instance, 22};
+	UavcanEscController _esc_controller {_canard_instance, 22}; //// TODO
 
 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
 };

src/drivers/uavcan_v1/parameters.c

@@ -86,11 +86,13 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0);
  */
 PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);
 
-
-PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
-PARAM_DEFINE_INT32(UCAN1_SERVO0_PID, 0);
+// Subscription Port IDs
 PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
-PARAM_DEFINE_INT32(UCAN1_ESC_PID, 0);
+
+// Publication Port IDs
+PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
+PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0);
+PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);