initial commit. Ensure that the DroneCAN esc module can receive the S…

…tatusExtended message

Initial working logging of the extended ESC status message of one motor

update DroneCAN extended status message to have multiple entries

add messages and start process of logging dronecan node status information

working node status logging

cleanup

last bits of cleanup. Get rid of magic numbers and variable names

update logic for adding to node status

run formatting on uavcan esc

final updates. Update naming to be consistent with esc status extended. update node status logging with appropriate names

update comments

fix name of dronecan_esc_status_extended in logged topics

msg/CMakeLists.txt

@@ -74,6 +74,10 @@ set(msg_files
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	DistanceSensorModeChangeRequest.msg
+	DronecanEscStatusExtended.msg
+	DronecanEscStatusExtendedData.msg
+	DronecanNodeStatus.msg
+	DronecanNodeStatusData.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg

msg/DronecanEscStatusExtended.msg

@@ -0,0 +1,5 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 CONNECTED_ESC_MAX = 8					# The number of ESCs supported. To be consistent with ESC Status, limit it to 8.
+
+DronecanEscStatusExtendedData[8] extended_esc_status_data	# An array of up to CONNECTED_ESC_MAX DronecanEscStatusExtendedData instances

msg/DronecanEscStatusExtendedData.msg

@@ -0,0 +1,9 @@
+uint64 timestamp					# time since system start (microseconds)
+
+# From the StatusExtended.uavcan message
+uint8 input_percent					# Input command to ESC, in percent, which is commanded using the setpoint messages. Range 0% to 100%.
+uint8 output_percent					# Output command from ESC to motor, in percent. Range 0% to 100%.
+int16 motor_temperature_deg_c  				# Temperature of connected motor, in Celsius. Range is -256 to +255 C.
+uint16 motor_angle             				# Measured angle of connected angle sensor, in degrees. Range is 0 to 360.
+uint32 status_flags           				# Manufacturer-specific status flags currently active.
+uint8 esc_index               				# Index of currently reporting ESC.

msg/DronecanNodeStatus.msg

@@ -0,0 +1,5 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 MAX_NODE_STATUSES_LOGGED = 8				# Hold up to 8 nodes worth of Node Status data in the logs. Limited to 8 to avoid needlessly large log files.
+
+DronecanNodeStatusData[8] node_status_id			# An array of up to MAX_NODE_STATUSES_LOGGED DronecanNodeStatusData instances

msg/DronecanNodeStatusData.msg

@@ -0,0 +1,41 @@
+uint64 timestamp					# time since system start (microseconds)
+
+# From the uavcan.protocol.NodeStatus message
+uint32 uptime_sec					# Node uptime
+
+uint16 node_id			# The node ID which this data comes from
+
+#
+# Abstract node health.
+#
+uint8 HEALTH_OK         = 0     # The node is functioning properly.
+uint8 HEALTH_WARNING    = 1     # A critical parameter went out of range or the node encountered a minor failure.
+uint8 HEALTH_ERROR      = 2     # The node encountered a major failure.
+uint8 HEALTH_CRITICAL   = 3     # The node suffered a fatal malfunction.
+uint8 health
+
+#
+# Current mode.
+#
+# Mode OFFLINE can be actually reported by the node to explicitly inform other network
+# participants that the sending node is about to shutdown. In this case other nodes will not
+# have to wait OFFLINE_TIMEOUT_MS before they detect that the node is no longer available.
+#
+# Reserved values can be used in future revisions of the specification.
+#
+uint8 MODE_OPERATIONAL      = 0         # Normal operating mode.
+uint8 MODE_INITIALIZATION   = 1         # Initialization is in progress; this mode is entered immediately after startup.
+uint8 MODE_MAINTENANCE      = 2         # E.g. calibration, the bootloader is running, etc.
+uint8 MODE_SOFTWARE_UPDATE  = 3         # New software/firmware is being loaded.
+uint8 MODE_OFFLINE          = 7         # The node is no longer available.
+uint8 mode
+
+#
+# Not used currently, keep zero when publishing, ignore when receiving.
+#
+uint8 sub_mode
+
+#
+# Optional, vendor-specific node status code, e.g. a fault code or a status bitmask.
+#
+uint16 vendor_specific_status_code

src/drivers/uavcan/actuators/esc.cpp

@@ -48,7 +48,8 @@ using namespace time_literals;
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
-	_uavcan_sub_status(node)
+	_uavcan_sub_status(node),
+	_uavcan_sub_status_extended(node)
 {
 	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority
 }
@@ -64,7 +65,16 @@ UavcanEscController::init()
 		return res;
 	}
 
+	//ESC Status Extended subscription
+	res = _uavcan_sub_status_extended.start(StatusExtendedCbBinder(this, &UavcanEscController::esc_status_extended_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("ESC status extended sub failed %i", res);
+		return res;
+	}
+
 	_esc_status_pub.advertise();
+	_status_extended_pub.advertise();  //Make sure people are listening
 
 	return res;
 }
@@ -154,6 +164,32 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	}
 }
 
+void
+UavcanEscController::esc_status_extended_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &received_status_extended_msg)
+{
+	//Make sure it's an ESC we can handle
+	if (received_status_extended_msg.esc_index < dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) {
+		//Grab the ESC we're talking about
+		auto &esc_reference = _status_extended.extended_esc_status_data[received_status_extended_msg.esc_index];
+
+		//Fill in the data
+		esc_reference.input_percent = received_status_extended_msg.input_pct;
+		esc_reference.output_percent = received_status_extended_msg.output_pct;
+		esc_reference.motor_temperature_deg_c = received_status_extended_msg.motor_temperature_degC;
+		esc_reference.motor_angle = received_status_extended_msg.motor_angle;
+		esc_reference.status_flags = received_status_extended_msg.status_flags;
+		esc_reference.esc_index = received_status_extended_msg.esc_index;
+		esc_reference.timestamp = hrt_absolute_time();
+
+		//Make sure to update the timestamp of our top level status
+		_status_extended.timestamp = hrt_absolute_time();
+
+		//Put the data into the world
+		_status_extended_pub.publish(_status_extended);
+	}
+}
+
 uint8_t
 UavcanEscController::check_escs_status()
 {

src/drivers/uavcan/actuators/esc.hpp

@@ -47,12 +47,14 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
+#include <uavcan/equipment/esc/StatusExtended.hpp>
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
+#include <uORB/topics/dronecan_esc_status_extended.h>
 
 class UavcanEscController
 {
@@ -86,6 +88,12 @@ class UavcanEscController
 	 */
 	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
 
+	/**
+	 * ESC status extended message reception will be reported via this callback.
+	 */
+	void esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>
+					&received_status_extended_msg);
+
 	/**
 	 * Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
 	 */
@@ -94,12 +102,19 @@ class UavcanEscController
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
 
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)>
+		StatusExtendedCbBinder;
+
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
 
 	esc_status_s	_esc_status{};
+	dronecan_esc_status_extended_s _status_extended{};
+
 
 	uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
+	uORB::Publication<dronecan_esc_status_extended_s> _status_extended_pub{ORB_ID(dronecan_esc_status_extended)};
 
 	uint8_t		_rotor_count{0};
 
@@ -110,6 +125,7 @@ class UavcanEscController
 	uavcan::INode								&_node;
 	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
+	uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, StatusExtendedCbBinder>	_uavcan_sub_status_extended;
 
 	/*
 	 * ESC states

src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp

@@ -10,6 +10,8 @@
 #include <uavcan/node/subscriber.hpp>
 #include <uavcan/node/timer.hpp>
 #include <uavcan/protocol/NodeStatus.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/dronecan_node_status.h>
 #include <cassert>
 #include <cstdlib>
 
@@ -77,8 +79,14 @@ class UAVCAN_EXPORT NodeStatusMonitor
 
     Subscriber<protocol::NodeStatus, NodeStatusCallback> sub_;
 
+    dronecan_node_status_s	_node_status{};
+    uORB::Publication<dronecan_node_status_s> _node_status_pub{ORB_ID(dronecan_node_status)};
+
     TimerEventForwarder<TimerCallback> timer_;
 
+    uint8_t _module_id_to_logging_index[dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED]; //Only handle as many node statuses as the DronecanNodeStatus message tells us we can
+    uint8_t _module_ids_being_logged = 0; //Keep track of how many node statuses are in play
+
     struct Entry
     {
         NodeStatus status;
@@ -119,6 +127,43 @@ class UAVCAN_EXPORT NodeStatusMonitor
         entry = new_entry_value;
     }
 
+    bool loggingModuleIdNodeStatus(uint8_t reporting_node_id)
+    {
+        for(uint8_t array_index = 0; array_index < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED; array_index++){
+            if(_module_id_to_logging_index[array_index] == reporting_node_id){
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    bool addModuleIdToLoggedStatuses(uint8_t module_id_to_add)
+    {
+        if(_module_ids_being_logged < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED){
+            _module_id_to_logging_index[_module_ids_being_logged] = module_id_to_add;
+            _module_ids_being_logged++;
+
+            return true;
+        }
+
+        return false;
+    }
+
+    uint8_t getModuleIdLogIndex(uint8_t module_id)
+    {
+        uint8_t return_array_index = 0;
+
+        for(uint8_t array_index = 0; array_index < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED; array_index++){
+            if(_module_id_to_logging_index[array_index] == module_id){
+                return_array_index = array_index;
+                break;
+            }
+        }
+
+        return return_array_index;
+    }
+
     void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
     {
         Entry new_entry;
@@ -130,6 +175,33 @@ class UAVCAN_EXPORT NodeStatusMonitor
         changeNodeStatus(msg.getSrcNodeID(), new_entry);
 
         handleNodeStatusMessage(msg);
+
+        uint8_t received_node_id = msg.getSrcNodeID().get();
+
+        //Check to see if we're logging this ID already. Use it to determine our next steps
+        bool already_logging_this_id = loggingModuleIdNodeStatus(received_node_id);
+
+        //Check to see if we're already logging this module ID, if not, try to add it to our list of module IDs to log.
+        //If we are, deal with publishing a new message
+        if(!already_logging_this_id){
+            addModuleIdToLoggedStatuses(received_node_id);
+        }else if(already_logging_this_id){
+            //Make a new message with the data we've got
+            auto &node_status_message = _node_status.node_status_id[getModuleIdLogIndex(received_node_id)];
+
+            //Fill in the actual node's status information
+            node_status_message.timestamp = hrt_absolute_time();
+            node_status_message.uptime_sec = msg.uptime_sec;
+            node_status_message.node_id = received_node_id;
+            node_status_message.health = msg.health;
+            node_status_message.mode = msg.mode;
+            node_status_message.sub_mode = msg.sub_mode;
+            node_status_message.vendor_specific_status_code = msg.vendor_specific_status_code;
+
+            _node_status.timestamp = hrt_absolute_time();
+
+            _node_status_pub.publish(_node_status);
+        }
     }
 
     void handleTimerEvent(const TimerEvent&)
@@ -197,6 +269,7 @@ class UAVCAN_EXPORT NodeStatusMonitor
         {
             timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent));
             timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
+            _node_status_pub.advertise();
         }
         return res;
     }

src/modules/logger/logged_topics.cpp

@@ -59,6 +59,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("config_overrides");
 	add_topic("cpuload");
 	add_topic("distance_sensor_mode_change_request");
+	add_optional_topic("dronecan_esc_status_extended", 250);
+	add_optional_topic("dronecan_node_status", 250);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");