Simplified esc_status healthiness logic.

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

src/drivers/uavcan/actuators/esc.cpp

@@ -233,14 +233,13 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 
 uint8_t UavcanEscController::check_escs_status()
 {
-	int esc_status_flags = 255;
+	int esc_status_flags = 0;
+
 
 	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
-		if (_esc_status.esc[index].timestamp == 0) {
-			esc_status_flags &= ~(1 << index);
 
-		} else if (hrt_elapsed_time(&_esc_status.esc[index].timestamp) > 800000.0f) {
+		if (_esc_status.esc[index].timestamp > 0 && hrt_elapsed_time(&_esc_status.esc[index].timestamp) < 800000.0f) {
-			esc_status_flags &= ~(1 << index);
+			esc_status_flags |= (1 << index);
 		}
 
 	}

src/drivers/uavcan/uavcan_main.cpp

@@ -1125,9 +1125,9 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 					_mixers->groups_required(_groups_required);
 					PX4_INFO("Groups required %d", _groups_required);
 
-					_rotor_count = _mixers->get_multirotor_count();
+					int rotor_count = _mixers->get_multirotor_count();
-					_esc_controller.set_rotor_count(_rotor_count);
+					_esc_controller.set_rotor_count(rotor_count);
-					PX4_INFO("Number of rotors %d", _rotor_count);
+					PX4_INFO("Number of rotors %d", rotor_count);
 				}
 			}
 		}

src/drivers/uavcan/uavcan_main.hpp

@@ -216,8 +216,6 @@ 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;

src/modules/commander/Commander.cpp

@@ -1622,16 +1622,10 @@ Commander::run()
 			}
 		}
 
-		if (orb_exists(ORB_ID(esc_status), 0) == PX4_OK) {
+		if (esc_status_sub.updated()) {
-
+			/* ESCs status changed */
-			if (esc_status_sub.updated()) {
+			esc_status_sub.copy(&esc_status);
-				/* ESCs status changed */
+			esc_status_check();
-				esc_status_sub.copy(&esc_status);
-				esc_status_check();
-			}
-
-		} else {
-			status_flags.condition_escs_error = false;
 		}
 
 		estimator_check(&status_changed);
@@ -4377,25 +4371,27 @@ void Commander::esc_status_check()
 	char esc_fail_msg[50];
 	esc_fail_msg[0] = '\0';
 
-	int online_bitmask = (1 << (esc_status.esc_count)) - 1;
+	int online_bitmask = (1 << esc_status.esc_count) - 1;
 
 	// Check if ALL the ESCs are online
 	if (online_bitmask == esc_status.esc_online_flags) {
 		status_flags.condition_escs_error = false;
 		_last_esc_online_flags = esc_status.esc_online_flags;
-	}
 
-	// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded from uavcan_main
+	} else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0)  {
-	else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0)  {
+
+		// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
+
 		status_flags.condition_escs_error = true;
-	}
 
-	// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing  Offline ESCs warning messages at boot
+	} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
-	else if (esc_status.esc_online_flags < _last_esc_online_flags) {
+
+		// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
 
 		for (int index = 0; index < esc_status.esc_count; index++) {
-			if ((bool)!(esc_status.esc_online_flags & (1 << index))) {
+			if ((esc_status.esc_online_flags & (1 << index)) == 0) {
 				snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
+				esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
 			}
 		}