drivers/uavcan: make firmware server available on both CAN1/CAN2 always (if enabled)

- uavcan firmware server no longer shuts down when arming (nodes might restart in flight)
 - always handle UAVCAN parameters with or without the FW server active
 - remove legacy ESC enumeration in FW server

ROMFS/px4fmu_common/init.d/rcS

@@ -291,16 +291,10 @@ else
 		# Start core UAVCAN module.
 		if uavcan start
 		then
-			if param greater UAVCAN_ENABLE 1
-			then
-				# Start UAVCAN firmware update server and dynamic node ID allocation server.
-				uavcan start fw
-
 			if param greater UAVCAN_ENABLE 2
 			then
 				set OUTPUT_MODE uavcan_esc
 			fi
-			fi
 		else
 			tune_control play error
 		fi

src/drivers/uavcan/uavcan_main.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -46,32 +46,37 @@
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
-#include "beep.hpp"
-#include "rgbled.hpp"
-#include "safety_state.hpp"
-#include "uavcan_driver.hpp"
-#include "uavcan_servers.hpp"
-#include "allocator.hpp"
 #include "actuators/esc.hpp"
 #include "actuators/hardpoint.hpp"
 #include "actuators/servo.hpp"
+#include "allocator.hpp"
+#include "beep.hpp"
+#include "rgbled.hpp"
+#include "safety_state.hpp"
 #include "sensors/sensor_bridge.hpp"
+#include "uavcan_driver.hpp"
+#include "uavcan_servers.hpp"
+
+#include <lib/drivers/device/Device.hpp>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <lib/perf/perf_counter.h>
 
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
-#include <uavcan/protocol/param/GetSet.hpp>
 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
+#include <uavcan/protocol/param/GetSet.hpp>
 #include <uavcan/protocol/RestartNode.hpp>
 
-#include <lib/drivers/device/device.h>
+#include <uORB/Publication.hpp>
-#include <lib/mixer_module/mixer_module.hpp>
-#include <lib/perf/perf_counter.h>
-
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 using namespace time_literals;
 
@@ -178,18 +183,15 @@ public:
 
 	uavcan::Node<>	&get_node() { return _node; }
 
-	int		teardown();
-
 	void		print_info();
 
 	void		shrink();
 
-	void		hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
-
 	static UavcanNode	*instance() { return _instance; }
 	static int		 getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
-	int			 fw_server(eServerAction action);
+
-	void			 attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
+	void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
+
 	int			 list_params(int remote_node_id);
 	int			 save_params(int remote_node_id);
 	int			 set_param(int remote_node_id, const char *name, char *value);
@@ -198,16 +200,11 @@ public:
 
 	static void busevent_signal_trampoline();
 
-protected:
-	void Run() override;
 private:
+	void Run() override;
+
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
-	void		node_spin_once();
-
-	int		start_fw_server();
-	int		stop_fw_server();
-	int		request_fw_check();
 
 	int		print_params(uavcan::protocol::param::GetSet::Response &resp);
 	int		get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
@@ -219,10 +216,6 @@ private:
 	void enable_idle_throttle_when_armed(bool value);
 
 	px4::atomic_bool	_task_should_exit{false};	///< flag to indicate to tear down the CAN driver
-	px4::atomic<int>	_fw_server_action{None};
-	int			 _fw_server_status{-1};
-
-	bool			_is_armed{false};		///< the arming status of the actuators on the bus
 
 	unsigned		_output_count{0};		///< number of actuators currently available
 
@@ -232,30 +225,28 @@ private:
 
 	uavcan::Node<>			_node;				///< library instance
 	pthread_mutex_t			_node_mutex;
-	px4_sem_t			_server_command_sem;
+
 	UavcanEscController		_esc_controller;
 	UavcanServoController		_servo_controller;
 	UavcanMixingInterfaceESC 	_mixing_interface_esc{_node_mutex, _esc_controller};
 	UavcanMixingInterfaceServo 	_mixing_interface_servo{_node_mutex, _servo_controller};
 	UavcanHardpointController	_hardpoint_controller;
-	UavcanBeep			_beep_controller;
 	UavcanSafetyState         	_safety_state_controller;
 	UavcanRGBController             _rgbled_controller;
+
 	uavcan::GlobalTimeSyncMaster	_time_sync_master;
 	uavcan::GlobalTimeSyncSlave	_time_sync_slave;
 	uavcan::NodeStatusMonitor	_node_status_monitor;
 
-	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
+	uavcan::NodeInfoRetriever   _node_info_retriever;
 
-	ITxQueueInjector		*_tx_injector{nullptr};
+	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
 
 	bool 				_idle_throttle_when_armed{false};
 	int32_t 			_idle_throttle_when_armed_param{0};
 
-	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	perf_counter_t			_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
-
+	perf_counter_t			_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
-	perf_counter_t			_cycle_perf;
-	perf_counter_t			_interval_perf;
 
 	void handle_time_sync(const uavcan::TimerEvent &);
 
@@ -274,7 +265,58 @@ private:
 		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
 
 	void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+
+	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
+
+	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
+	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	/*
+	 * The MAVLink parameter bridge needs to know the maximum parameter index
+	 * of each node so that clients can determine when parameter listings have
+	 * finished. We do that by querying a node's entire parameter set whenever
+	 * a parameter message is received for a node with a zero _param_count,
+	 * and storing the count here. If a node doesn't respond, or doesn't have
+	 * any parameters, its count will stay at zero and we'll try to query the
+	 * set again next time.
+	 *
+	 * The node's UAVCAN ID is used as the index into the _param_counts array.
+	 */
+	uint8_t _param_counts[128] {};
+	bool _count_in_progress{false};
+	uint8_t _count_index{0};
+
+	bool _param_in_progress{false};
+	uint8_t _param_index{0};
+	bool _param_list_in_progress{false};
+	bool _param_list_all_nodes{false};
+	uint8_t _param_list_node_id{1};
+
+	uint32_t _param_dirty_bitmap[4] {};
+	uint8_t _param_save_opcode{0};
+
+	bool _cmd_in_progress{false};
+
+	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
 	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
 	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
 
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
+	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
+	void param_count(uavcan::NodeID node_id);
+	void param_opcode(uavcan::NodeID node_id);
+
+	uint8_t get_next_active_node_id(uint8_t base);
+	uint8_t get_next_dirty_node_id(uint8_t base);
+	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
+	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
+	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
+
+	bool _check_fw{false};
+
+	UavcanServers   *_servers{nullptr};
 };

src/drivers/uavcan/uavcan_module.hpp

@@ -66,18 +66,6 @@
 // ioctl interface
 #define _UAVCAN_IOC(_n)               (_IOC(_UAVCAN_IOCBASE, _n))
 #define _UAVCAN_IOCBASE               (0x4000)                        // IOCTL base for module UAVCAN
-/*
- * Query if node identification is in progress. Returns:
- *      EINVAL - not applicable in the current operating mode
- *      ETIME  - network discovery complete
- *      OK (0) - network discovery in progress
- */
-#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)
-/*
- * Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing.
- * The pointer may be invalidated once the call returns.
- */
-#define UAVCAN_IOCS_HARDPOINT_SET       _UAVCAN_IOC(10)
 
 // public prototypes
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);

src/drivers/uavcan/uavcan_servers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -34,33 +34,18 @@
 #pragma once
 
 #include <px4_platform_common/px4_config.h>
-#include "uavcan_driver.hpp"
-#include <drivers/device/Device.hpp>
-#include <lib/perf/perf_counter.h>
-#include <uORB/Publication.hpp>
-#include <uORB/topics/uavcan_parameter_value.h>
-#include <uORB/topics/vehicle_command_ack.h>
-
-#include <uavcan/node/sub_node.hpp>
-#include <uavcan/protocol/node_status_monitor.hpp>
-#include <uavcan/protocol/param/GetSet.hpp>
-#include <uavcan/protocol/param/ExecuteOpcode.hpp>
 
 #include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
-#include <uavcan/protocol/node_info_retriever.hpp>
-#include <uavcan_posix/basic_file_server_backend.hpp>
-#include <uavcan/protocol/firmware_update_trigger.hpp>
 #include <uavcan/protocol/file_server.hpp>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan_posix/basic_file_server_backend.hpp>
 #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
 #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
 #include <uavcan_posix/firmware_version_checker.hpp>
-#include <uavcan/equipment/esc/RawCommand.hpp>
-#include <uavcan/equipment/indication/BeepCommand.hpp>
-#include <uavcan/protocol/enumeration/Begin.hpp>
-#include <uavcan/protocol/enumeration/Indication.hpp>
 
 #include "uavcan_module.hpp"
-#include "uavcan_virtual_can_driver.hpp"
 
 /**
  * @file uavcan_servers.hpp
@@ -74,152 +59,29 @@
 /**
  * A UAVCAN Server Sub node.
  */
-class __EXPORT UavcanServers
+class UavcanServers
 {
-	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
-
-	static constexpr unsigned StackSize  = 6000;
-	static constexpr unsigned Priority  =  120;
-
-	static constexpr unsigned VirtualIfaceBlockAllocationQuota =  80;
-
-	static constexpr float BeepFrequencyGenericIndication       = 1000.0F;
-	static constexpr float BeepFrequencySuccess                 = 2000.0F;
-	static constexpr float BeepFrequencyError                   = 100.0F;
-
 public:
-	UavcanServers(uavcan::INode &main_node);
+	UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever);
+	~UavcanServers() = default;
 
-	virtual		~UavcanServers();
+	int init();
-
-	static int      start(uavcan::INode &main_node);
-	static int      stop();
-
-	uavcan::SubNode<> &get_node() { return _subnode; }
-
-	static UavcanServers *instance() { return _instance; }
-
-	/*
-	 *  Set the main node's pointer to to the injector
-	 *  This is a work around as main_node.getDispatcher().remeveRxFrameListener();
-	 *  would require a dynamic cast and rtti is not enabled.
-	 */
-	void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
-
-	void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
 
 	bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
 
 private:
-	pthread_t         _subnode_thread{-1};
-	pthread_mutex_t   _subnode_mutex{};
-	px4::atomic_bool  _subnode_thread_should_exit{false};
 
-	int		init();
+	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
-
+	void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
-	pthread_addr_t	run(pthread_addr_t);
+	int copyFw(const char *dst, const char *src);
-
-	static UavcanServers	*_instance;            ///< singleton pointer
-
-	VirtualCanDriver _vdriver;
-
-	uavcan::SubNode<>  _subnode;
-	uavcan::INode      &_main_node;
 
 	uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
 	uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
 	uavcan_posix::FirmwareVersionChecker _fw_version_checker;
 	uavcan::dynamic_node_id_server::CentralizedServer _server_instance;  ///< server singleton pointer
 	uavcan_posix::BasicFileServerBackend _fileserver_backend;
-	uavcan::NodeInfoRetriever   _node_info_retriever;
 	uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
 	uavcan::BasicFileServer _fw_server;
 
-	/*
+	uavcan::NodeInfoRetriever &_node_info_retriever;
-	 * The MAVLink parameter bridge needs to know the maximum parameter index
-	 * of each node so that clients can determine when parameter listings have
-	 * finished. We do that by querying a node's entire parameter set whenever
-	 * a parameter message is received for a node with a zero _param_count,
-	 * and storing the count here. If a node doesn't respond, or doesn't have
-	 * any parameters, its count will stay at zero and we'll try to query the
-	 * set again next time.
-	 *
-	 * The node's UAVCAN ID is used as the index into the _param_counts array.
-	 */
-	uint8_t _param_counts[128] = {};
-	bool _count_in_progress = false;
-	uint8_t _count_index = 0;
-
-	bool _param_in_progress = false;
-	uint8_t _param_index = 0;
-	bool _param_list_in_progress = false;
-	bool _param_list_all_nodes = false;
-	uint8_t _param_list_node_id = 1;
-
-	uint32_t _param_dirty_bitmap[4] = {};
-	uint8_t _param_save_opcode = 0;
-
-	bool _cmd_in_progress = false;
-
-	// uORB topic handle for MAVLink parameter responses
-	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
-	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
-
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)>
-		ExecuteOpcodeCallback;
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
-
-	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
-	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
-	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
-	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
-
-	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
-	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
-	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
-	void param_count(uavcan::NodeID node_id);
-	void param_opcode(uavcan::NodeID node_id);
-
-	uint8_t get_next_active_node_id(uint8_t base);
-	uint8_t get_next_dirty_node_id(uint8_t base);
-	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
-	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
-	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
-
-	void beep(float frequency);
-
-	bool _mutex_inited = false;
-	volatile bool _check_fw = false;
-
-	// ESC enumeration
-	bool _esc_enumeration_active = false;
-	uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
-	uint8_t _esc_enumeration_index = 0;
-	uint8_t _esc_count = 0;
-
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)>
-		EnumerationBeginCallback;
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
-		EnumerationIndicationCallback;
-	void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
-	void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
-	void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
-	void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
-
-	uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
-	uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback>
-	_enumeration_indication_sub;
-	uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
-	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
-	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
-
-	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
-	void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
-	int copyFw(const char *dst, const char *src);
 };