UAVCAN: Read servo telemetry over CAN bus and publish to uorb

msg/CMakeLists.txt

@@ -212,6 +212,8 @@ set(msg_files
 	SensorTemp.msg
 	SensorUwb.msg
 	SensorAirflow.msg
+	ServoReport.msg
+	ServoStatus.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg

msg/ServoReport.msg

@@ -0,0 +1,25 @@
+uint64 timestamp					# time since system start (microseconds)
+uint8 servo_actuator_id					# actuator id of current servo (in most cases 1-8)
+uint8 servo_node_id					# Node ID of device on CAN bus
+float32 servo_position					# meter or radian
+float32 servo_force					# Newton or Newton metre
+float32 servo_speed					# meter per second or radian per second
+uint8 servo_power_rating_pct				# 0 - unloaded, 100 - full load
+
+uint8 servo_function					# servo output function
+
+
+float32 servo_temperature				# in kelvin
+uint8 servo_temperature_error_flags
+
+uint8 ERROR_FLAG_OVERHEATING = 1
+uint8 ERROR_FLAG_OVERCOOLING = 2
+
+float32 servo_voltage					# Volts
+float32 servo_current					# Amps
+uint8 servo_power_error_flags
+
+uint8 ERROR_FLAG_OVERVOLTAGE  = 1
+uint8 ERROR_FLAG_UNDERVOLTAGE = 2
+uint8 ERROR_FLAG_OVERCURRENT  = 4
+uint8 ERROR_FLAG_UNDERCURRENT = 8

msg/ServoStatus.msg

@@ -0,0 +1,28 @@
+uint64 timestamp					# time since system start (microseconds)
+uint8 CONNECTED_SERVO_MAX = 8				# The number of SERVOs supported via UAVCAN
+
+uint8 SERVO_CONNECTION_TYPE_PPM = 0			# Traditional PPM SERVO
+uint8 SERVO_CONNECTION_TYPE_SERIAL = 1			# Serial Bus connected SERVO
+uint8 SERVO_CONNECTION_TYPE_ONESHOT = 2			# One Shot PPM
+uint8 SERVO_CONNECTION_TYPE_I2C = 3			# I2C
+uint8 SERVO_CONNECTION_TYPE_CAN = 4			# CAN-Bus
+uint8 SERVO_CONNECTION_TYPE_DSHOT = 5			# DShot
+
+uint16 counter  					# incremented by the writing thread everytime new data is stored
+uint16 temperature_counter 				# Incremented when new temperature data is stored
+uint16 power_counter 					# Incremented when new power data is stored
+uint8 servo_count					# number of connected servo's
+
+uint8 servo_connectiontype				# how SERVOs connected to the system. Currently only CAN supports servo telemtry
+
+uint8 servo_online_flags				# Bitmask indicating which SERVO is online/offline
+# servo_online_flags bit 0 : Set to 1 if SERVO0 is online
+# servo_online_flags bit 1 : Set to 1 if SERVO1 is online
+# servo_online_flags bit 2 : Set to 1 if SERVO2 is online
+# servo_online_flags bit 3 : Set to 1 if SERVO3 is online
+# servo_online_flags bit 4 : Set to 1 if SERVO4 is online
+# servo_online_flags bit 5 : Set to 1 if SERVO5 is online
+# servo_online_flags bit 6 : Set to 1 if SERVO6 is online
+# servo_online_flags bit 7 : Set to 1 if SERVO7 is online
+
+ServoReport[8] servo

src/drivers/uavcan/actuators/servo.cpp

@@ -39,9 +39,56 @@ using namespace time_literals;
 
 UavcanServoController::UavcanServoController(uavcan::INode &node) :
 	_node(node),
-	_uavcan_pub_array_cmd(node)
+	_uavcan_pub_array_cmd(node),
+	_uavcan_sub_status(node),
+	_uavcan_sub_temperature(node),
+	_uavcan_sub_circuit_status(node)
 {
 	_uavcan_pub_array_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
+	memset(_last_temperature, 0, sizeof(_last_temperature)); // Set bytes to 0
+	memset(_last_temperature_error_flag, 0, sizeof(_last_temperature_error_flag));
+	memset(_last_voltage, 0, sizeof(_last_voltage));
+	memset(_last_current, 0, sizeof(_last_current));
+	memset(_last_power_error_flag, 0, sizeof(_last_power_error_flag));
+}
+
+int
+UavcanServoController::init()
+{
+	// Servo status subscription
+	int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanServoController::servo_status_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("Servo status sub failed %i", res);
+		return res;
+	}
+
+	// Servo temperature subscription
+	res = _uavcan_sub_temperature.start(TemperatureCbBinder(this, &UavcanServoController::servo_temperature_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("Servo Temperature sub failed %i", res);
+		return res;
+	}
+
+	// Servo circuit status subscription
+	res = _uavcan_sub_circuit_status.start(CircuitStatusCbBinder(this,
+					       &UavcanServoController::servo_circuit_status_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("Servo circuit status sub failed %i", res);
+		return res;
+	}
+
+	_servo_status_pub.advertise(); // advertise to ensure messages are logged
+
+	return res; // return if both subscriptions are successful
+}
+
+void
+UavcanServoController::set_servo_count(uint8_t count)
+{
+	_servo_count = count;
 }
 
 void
@@ -60,3 +107,82 @@ UavcanServoController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned
 
 	_uavcan_pub_array_cmd.broadcast(msg);
 }
+
+void
+UavcanServoController::servo_temperature_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::device::Temperature>
+		&msg)
+{
+	if (msg.device_id < servo_status_s::CONNECTED_SERVO_MAX) {
+
+		_last_temperature[msg.device_id] = msg.temperature;
+		_last_temperature_error_flag[msg.device_id] = msg.error_flags;
+		_servo_temperature_counter += 1; // Keep counter to monitor callbacks
+	}
+}
+
+void
+UavcanServoController::servo_circuit_status_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::power::CircuitStatus>
+		&msg)
+{
+	if (msg.circuit_id < servo_status_s::CONNECTED_SERVO_MAX) {
+
+		_last_voltage[msg.circuit_id] = msg.voltage;
+		_last_current[msg.circuit_id] = msg.current;
+		_last_power_error_flag[msg.circuit_id] = msg.error_flags;
+		_servo_power_counter += 1; // Keep counter to monitor callbacks
+	}
+}
+
+void
+UavcanServoController::servo_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::actuator::Status>
+		&msg)
+{
+	if (msg.actuator_id < servo_status_s::CONNECTED_SERVO_MAX) {
+		auto &ref = _servo_status.servo[msg.actuator_id];
+
+		ref.timestamp              = hrt_absolute_time();
+		ref.servo_node_id          = msg.getSrcNodeID().get();
+		ref.servo_actuator_id      = msg.actuator_id;
+		ref.servo_position         = msg.position;
+		ref.servo_force            = msg.force;
+		ref.servo_speed            = msg.speed;
+		ref.servo_power_rating_pct = msg.power_rating_pct;
+
+		// Add servo temperature data
+		ref.servo_temperature 	   = _last_temperature[msg.actuator_id];
+		ref.servo_temperature_error_flags = _last_temperature_error_flag[msg.actuator_id];
+
+		// Add servo power data
+		ref.servo_voltage 	   = _last_voltage[msg.actuator_id];
+		ref.servo_current 	   = _last_current[msg.actuator_id];
+		ref.servo_power_error_flags = _last_power_error_flag[msg.actuator_id];
+
+		_servo_status.counter += 1;
+		_servo_status.temperature_counter = _servo_temperature_counter;
+		_servo_status.power_counter = _servo_power_counter;
+		_servo_status.servo_count = _servo_count;
+		_servo_status.servo_connectiontype = servo_status_s::SERVO_CONNECTION_TYPE_CAN;
+		_servo_status.servo_online_flags = check_servos_status();
+		_servo_status.timestamp = hrt_absolute_time();
+		_servo_status_pub.publish(_servo_status);
+	}
+}
+
+uint8_t
+UavcanServoController::check_servos_status()
+{
+	int servo_status_flags = 0;
+	const hrt_abstime now = hrt_absolute_time();
+
+	for (int index = 0; index < servo_status_s::CONNECTED_SERVO_MAX; index++) {
+
+		if (_servo_status.servo[index].timestamp > 0 && now - _servo_status.servo[index].timestamp < 1200_ms) {
+			servo_status_flags |= (1 << index);
+		}
+
+	}
+
+	return servo_status_flags;
+}

src/drivers/uavcan/actuators/servo.hpp

@@ -36,9 +36,12 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/actuator/Status.hpp>
 #include <uavcan/equipment/actuator/ArrayCommand.hpp>
+#include <uavcan/equipment/device/Temperature.hpp>
+#include <uavcan/equipment/power/CircuitStatus.hpp>
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/servo_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
 
@@ -48,13 +51,76 @@ public:
 	static constexpr int MAX_ACTUATORS = 8;
 	static constexpr unsigned MAX_RATE_HZ = 50;
 	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 6;	///< 0..31, inclusive, 0 - highest, 31 - lowest
+	static_assert(uavcan::equipment::actuator::ArrayCommand::FieldTypes::commands::MaxSize >= MAX_ACTUATORS,
+		      "Too many actuators");
 
 	UavcanServoController(uavcan::INode &node);
 	~UavcanServoController() = default;
 
+	int init();
+	void set_servo_count(uint8_t count);
 	void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
 
+	servo_status_s &servo_status() { return _servo_status; }
+
+
 private:
+
+	/**
+	 * Servo status message reception will be reported via this callback.
+	 */
+	void servo_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::actuator::Status> &msg);
+
+	/**
+	 * Servo temperature message reception will be reported via this callback.
+	 */
+	void servo_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::device::Temperature> &msg);
+
+	/**
+	 * Servo circuit status message reception will be reported via this callback.
+	 */
+	void servo_circuit_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::CircuitStatus> &msg);
+
+
+	/**
+	 * Checks all the servo freshness based on timestamp, if an servo exceeds the timeout then is flagged offline.
+	 */
+	uint8_t check_servos_status();
+
+	typedef uavcan::MethodBinder<UavcanServoController *,
+		void (UavcanServoController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::actuator::Status>&)>
+		StatusCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanServoController *,
+		void (UavcanServoController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::device::Temperature>&)>
+		TemperatureCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanServoController *,
+		void (UavcanServoController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::power::CircuitStatus>&)>
+		CircuitStatusCbBinder;
+
+	servo_status_s	_servo_status{};
+	uint8_t _servo_count{0};
+
+	uORB::PublicationMulti<servo_status_s> _servo_status_pub{ORB_ID(servo_status)};
+
+	/*
+	 * libuavcan related things
+	 */
+
 	uavcan::INode								&_node;
 	uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand> _uavcan_pub_array_cmd;
+	uavcan::Subscriber<uavcan::equipment::actuator::Status, StatusCbBinder>	_uavcan_sub_status;
+	uavcan::Subscriber<uavcan::equipment::device::Temperature, TemperatureCbBinder> _uavcan_sub_temperature;
+	uavcan::Subscriber<uavcan::equipment::power::CircuitStatus, CircuitStatusCbBinder> _uavcan_sub_circuit_status;
+
+
+	float _last_temperature[servo_status_s::CONNECTED_SERVO_MAX];
+	uint8_t _last_temperature_error_flag[servo_status_s::CONNECTED_SERVO_MAX];
+	float _last_voltage[servo_status_s::CONNECTED_SERVO_MAX];
+	float _last_current[servo_status_s::CONNECTED_SERVO_MAX];
+	uint8_t _last_power_error_flag[servo_status_s::CONNECTED_SERVO_MAX];
+	int _servo_temperature_counter{0};
+	int _servo_power_counter{0};
+
 };

src/drivers/uavcan/uavcan_main.cpp

@@ -528,7 +528,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 
 #endif
 
-	// Actuators
+	// ESCs
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
 	int32_t uavcan_enable = -1;
 	(void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable);
@@ -544,6 +544,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 
 #endif
 
+	// Servos
+#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
+	ret = _servo_controller.init();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+#endif
+
+
 #if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER)
 	ret = _hardpoint_controller.init();
 
@@ -1135,6 +1146,22 @@ void UavcanMixingInterfaceESC::mixerChanged()
 	_esc_controller.set_rotor_count(rotor_count);
 }
 
+void UavcanMixingInterfaceServo::mixerChanged()
+{
+	int servo_count = 0;
+
+	for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
+		servo_count += _mixing_output.isFunctionSet(i);
+
+		if (i < servo_status_s::CONNECTED_SERVO_MAX) {
+			_servo_controller.servo_status().servo[i].servo_function = (uint8_t)_mixing_output.outputFunction(i);
+		}
+
+	}
+
+	_servo_controller.set_servo_count(servo_count);
+}
+
 bool UavcanMixingInterfaceServo::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {

src/drivers/uavcan/uavcan_main.hpp

@@ -165,6 +165,8 @@ public:
 	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
+	void mixerChanged() override;
+
 	MixingOutput &mixingOutput() { return _mixing_output; }
 
 protected:

src/modules/logger/logged_topics.cpp

@@ -126,6 +126,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("sensor_selection");
 	add_topic("sensors_status_imu", 200);
 	add_optional_topic("spoilers_setpoint", 1000);
+	add_optional_topic("servo_status", 10);
 	add_topic("system_power", 500);
 	add_optional_topic("takeoff_status", 1000);
 	add_optional_topic("tecs_status", 200);