UAVCAN: format uORB docu, disable not needed features, clean-up

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,33 @@
+# Servo report
+#
+# Contains information about a specific servo (position, force, speed, load).
+# Also contains auxiliary power data (voltage, current) and temperature.
+
+uint64 timestamp  # [us] Time since system start
+
+uint8 actuator_id  # [-] DroneCAN actuator id of servo. Set to 0 for non DroneCAN servos
+uint8 node_id      # [-] DroneCAN node id of servo. Set to 0 for non DroneCAN servos
+
+float32 position          # meter or radian
+float32 force             # Newton or Newton metre
+float32 speed             # meter per second or radian per second
+float32 power_rating_pct  # [%] [@range 0, 100] Servo load
+
+uint8 function  # [-] Servo output function
+
+uint16 temperature_counter  # [-] Incremented when temperature data got updated
+float32 temperature         # [degC] Servo temperature.
+
+uint8 temperature_error_flags  # [@enum ERROR_FLAG] Temperature error flags
+uint8 ERROR_FLAG_OVERHEATING = 1
+uint8 ERROR_FLAG_OVERCOOLING = 2
+
+uint16 power_counter  # [-] Incremented when power data got updated
+float32 voltage       # [V] Servo voltage
+float32 current       # [A] Servo current draw
+
+uint8 power_error_flags  # [@enum ERROR_FLAG] 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,21 @@
+# Servo status
+#
+# Overview message that contains general information about all servos.
+# In addition it contains the reports of all individual servos.
+
+uint64 timestamp  # [us] Time since system start
+
+uint8 count  # [-] Number of connected servos
+uint8 CONNECTED_SERVO_MAX = 8
+
+uint8 online_flags  # [-] Bitmask indicating which servo is online/offline
+# online_flags bit 0 : Set to 1 if SERVO0 is online
+# online_flags bit 1 : Set to 1 if SERVO1 is online
+# online_flags bit 2 : Set to 1 if SERVO2 is online
+# online_flags bit 3 : Set to 1 if SERVO3 is online
+# online_flags bit 4 : Set to 1 if SERVO4 is online
+# online_flags bit 5 : Set to 1 if SERVO5 is online
+# online_flags bit 6 : Set to 1 if SERVO6 is online
+# online_flags bit 7 : Set to 1 if SERVO7 is online
+
+ServoReport[8] servo

src/drivers/uavcan/actuators/servo.cpp

@@ -34,14 +34,77 @@
 #include "servo.hpp"
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <lib/atmosphere/atmosphere.h>
+
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0]))
 
 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)
 {
+	/* Ensure that future changes do not cause any out-of-bounds access */
+	static_assert(ARRAY_SIZE(_servo_status.servo) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_last_temperature) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_last_temperature_error_flag) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_last_voltage) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_last_current) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_last_power_error_flag) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_servo_temperature_counter) == servo_status_s::CONNECTED_SERVO_MAX);
+	static_assert(ARRAY_SIZE(_servo_power_counter) == servo_status_s::CONNECTED_SERVO_MAX);
+
 	_uavcan_pub_array_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
+
+	/* Init the arrays to zero */
+	memset(_last_temperature, 0, sizeof(_last_temperature));
+	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));
+	memset(_servo_temperature_counter, 0, sizeof(_servo_temperature_counter));
+	memset(_servo_power_counter, 0, sizeof(_servo_power_counter));
+}
+
+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 +123,93 @@ 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)
+{
+	for (int i = 0; i < servo_status_s::CONNECTED_SERVO_MAX; i++) {
+		auto &ref = _servo_status.servo[i];
+
+		const bool is_servo_online = _servo_status.online_flags & (1 << i);
+		const bool is_servo_matching = ref.node_id == msg.getSrcNodeID().get();
+
+		if (is_servo_online && is_servo_matching) {
+			_servo_temperature_counter[i] += 1;
+			_last_temperature[i] = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius;
+			_last_temperature_error_flag[i] = msg.error_flags;
+			break;
+		}
+	}
+}
+
+void
+UavcanServoController::servo_circuit_status_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::power::CircuitStatus>
+		&msg)
+{
+	for (int i = 0; i < servo_status_s::CONNECTED_SERVO_MAX; i++) {
+		auto &ref = _servo_status.servo[i];
+
+		const bool is_servo_online = _servo_status.online_flags & (1 << i);
+		const bool is_servo_matching = ref.node_id == msg.getSrcNodeID().get();
+
+		if (is_servo_online && is_servo_matching) {
+			_servo_power_counter[i] += 1;
+			_last_voltage[i] = msg.voltage;
+			_last_current[i] = msg.current;
+			_last_power_error_flag[i] = msg.error_flags;
+			break;
+		}
+	}
+}
+
+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.node_id = msg.getSrcNodeID().get();
+		ref.actuator_id = msg.actuator_id;
+		ref.position = msg.position;
+		ref.force = msg.force;
+		ref.speed = msg.speed;
+		ref.power_rating_pct = msg.power_rating_pct;
+
+		// Add servo temperature data
+		ref.temperature_counter = _servo_temperature_counter[msg.actuator_id];
+		ref.temperature = _last_temperature[msg.actuator_id];
+		ref.temperature_error_flags = _last_temperature_error_flag[msg.actuator_id];
+
+		// Add servo power data
+		ref.power_counter = _servo_power_counter[msg.actuator_id];
+		ref.voltage = _last_voltage[msg.actuator_id];
+		ref.current = _last_current[msg.actuator_id];
+		ref.power_error_flags = _last_power_error_flag[msg.actuator_id];
+
+		_servo_status.count = _servo_count;
+		_servo_status.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,75 @@ 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];
+	uint16_t _servo_temperature_counter[servo_status_s::CONNECTED_SERVO_MAX];
+	uint16_t _servo_power_counter[servo_status_s::CONNECTED_SERVO_MAX];
 };

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,21 @@ 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].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", 100);
 	add_topic("system_power", 500);
 	add_optional_topic("takeoff_status", 1000);
 	add_optional_topic("tecs_status", 200);