uavcan escs: Modified esc_status reporting to have _rotor_count publi…

…shed.

Signed-off-by: Claudio Micheli <claudio@auterion.com>

src/drivers/uavcan/actuators/esc.cpp

@@ -202,9 +202,6 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
 {
 	if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
-		_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
-		_esc_status.timestamp = hrt_absolute_time();
-
 		auto &ref = _esc_status.esc[msg.esc_index];
 
 		ref.esc_address = msg.getSrcNodeID().get();
@@ -220,6 +217,8 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 
 void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 {
+	_esc_status.timestamp = hrt_absolute_time();
+	_esc_status.esc_count = _rotor_count;
 	_esc_status.counter += 1;
 	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
 	_esc_status.esc_online_flags = UavcanEscController::check_escs_status();

src/drivers/uavcan/actuators/esc.hpp

@@ -68,6 +68,11 @@ class UavcanEscController
 
 	void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; }
 
+	/**
+	 * Sets the number of rotors
+	 */
+	void set_rotor_count(uint8_t count) { _rotor_count = count; }
+
 private:
 	/**
 	 * ESC status message reception will be reported via this callback.
@@ -85,6 +90,8 @@ class UavcanEscController
 	uint8_t check_escs_status();
 
 
+
+
 	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
 	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
 	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5;	///< 0..31, inclusive, 0 - highest, 31 - lowest
@@ -101,7 +108,7 @@ class UavcanEscController
 	esc_status_s	_esc_status = {};
 	orb_advert_t	_esc_status_pub = nullptr;
 	orb_advert_t _actuator_outputs_pub = nullptr;
-	hrt_abstime _last_received_msg[esc_status_s::CONNECTED_ESC_MAX] {0};
+	uint8_t		_rotor_count = 0;
 
 	/*
 	 * libuavcan related things

src/drivers/uavcan/uavcan_main.cpp

@@ -919,6 +919,7 @@ int UavcanNode::run()
 
 				new_output = true;
 			}
+
 		}
 
 		if (new_output) {
@@ -993,6 +994,7 @@ int UavcanNode::run()
 				_esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0);
 			}
 		}
+
 	}
 
 	orb_unsubscribe(params_sub);
@@ -1123,6 +1125,11 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 				} else {
 
 					_mixers->groups_required(_groups_required);
+					printf("Groups required %d \n", _groups_required);
+
+					_rotor_count = _mixers->get_multirotor_count();
+					_esc_controller.set_rotor_count(_rotor_count);
+					printf("Number of rotors %d \n", _rotor_count);
 				}
 			}
 		}

src/drivers/uavcan/uavcan_main.hpp

@@ -216,6 +216,8 @@ class UavcanNode : public cdev::CDev
 	// index into _poll_fds for each _control_subs handle
 	uint8_t				_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
 
+	uint8_t				_rotor_count = 0;
+
 	void handle_time_sync(const uavcan::TimerEvent &);
 
 	typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;

src/lib/mixer/mixer.h

@@ -254,6 +254,8 @@ class Mixer
 	 */
 	virtual void set_airmode(Airmode airmode) {};
 
+	virtual unsigned get_multirotor_count()  {return 0;}
+
 protected:
 	/** client-supplied callback used when fetching control values */
 	ControlCallback			_control_cb;
@@ -450,6 +452,8 @@ class MixerGroup : public Mixer
 
 	void 	set_airmode(Airmode airmode) override;
 
+	unsigned get_multirotor_count() override;
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -715,6 +719,8 @@ class MultirotorMixer : public Mixer
 
 	void 			set_airmode(Airmode airmode) override;
 
+	unsigned get_multirotor_count() override {return _rotor_count;}
+
 	union saturation_status {
 		struct {
 			uint16_t valid		: 1; // 0 - true when the saturation status is used

src/lib/mixer/mixer_group.cpp

@@ -198,6 +198,25 @@ MixerGroup::set_airmode(Airmode airmode)
 	}
 }
 
+unsigned
+MixerGroup::get_multirotor_count()
+{
+	Mixer	*mixer = _first;
+	unsigned rotor_count = 0;
+
+	while (mixer != nullptr) {
+
+		rotor_count = mixer->get_multirotor_count();
+
+		if (rotor_count > 0) {
+			break;
+
+		} else { mixer = mixer -> _next;}
+	}
+
+	return rotor_count;
+}
+
 uint16_t
 MixerGroup::get_saturation_status()
 {

src/modules/commander/state_machine_helper.h

@@ -51,7 +51,6 @@
 #include <uORB/topics/safety.h>
 #include <uORB/topics/commander_state.h>
 #include <uORB/topics/vehicle_status_flags.h>
-#include <uORB/topics/esc_status.h>
 
 typedef enum {
 	TRANSITION_DENIED = -1,