uavcan: don't init ESC if UAVCAN_ENABLE isn't set for ESC

src/drivers/uavcan/actuators/esc.cpp

@@ -81,6 +81,8 @@ UavcanEscController::init()
 		_param_handles[i] = param_find(param_name);
 	}
 
+	_initialized = true;
+
 	return res;
 }
 

src/drivers/uavcan/actuators/esc.hpp

@@ -69,6 +69,8 @@ public:
 
 	int init();
 
+	bool initialized() { return _initialized; };
+
 	void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs);
 
 	/**
@@ -97,6 +99,8 @@ private:
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
 
+	bool _initialized{};
+
 	esc_status_s	_esc_status{};
 
 	uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

src/drivers/uavcan/uavcan_main.cpp

@@ -529,10 +529,16 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 
 	// Actuators
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-	ret = _esc_controller.init();
+	int32_t uavcan_enable = -1;
+	(void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable);
 
-	if (ret < 0) {
-		return ret;
+	if (uavcan_enable > 2) {
+
+		ret = _esc_controller.init();
+
+		if (ret < 0) {
+			return ret;
+		}
 	}
 
 #endif
@@ -607,10 +613,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
 	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
 
-
-	int32_t uavcan_enable = 1;
-	(void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable);
-
 	if (uavcan_enable > 1) {
 		_servers = new UavcanServers(_node, _node_info_retriever);
 
@@ -1079,7 +1081,10 @@ void UavcanNode::publish_node_statuses()
 bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
-	_esc_controller.update_outputs(outputs, num_outputs);
+	if (_esc_controller.initialized()) {
+		_esc_controller.update_outputs(outputs, num_outputs);
+	}
+
 	return true;
 }