uavcan escs: Modified esc_status reporting to have _rotor_count published.

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 @@ public:
 
 	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 @@ private:
 	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 @@ private:
 	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 @@ private:
 	// 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;