Agent/system health mask fix

src/main/java/io/mapsmessaging/state/mavlink/listener/SysStatusListener.java

@@ -19,18 +19,17 @@
 
 package io.mapsmessaging.state.mavlink.listener;
 
+import static io.mapsmessaging.state.mavlink.packet.MavlinkMessageIds.SYS_STATUS;
+
 import io.mapsmessaging.state.drone.core.TwinManager;
 import io.mapsmessaging.state.drone.core.TwinUpdateContext;
 import io.mapsmessaging.state.drone.drone.DroneTwin;
 import io.mapsmessaging.state.drone.model.BatteryState;
 import io.mapsmessaging.state.drone.model.SystemState;
 import io.mapsmessaging.state.mavlink.packet.MavlinkPacket;
 import io.mapsmessaging.state.mavlink.packet.SysStatusPacket;
-
 import java.time.Instant;
 
-import static io.mapsmessaging.state.mavlink.packet.MavlinkMessageIds.SYS_STATUS;
-
 public class SysStatusListener implements Listener {
 
   public static final int LISTENER_ID = SYS_STATUS;
@@ -43,29 +42,22 @@ public SysStatusListener(TwinManager twinManager) {
 
   @Override
   public void handle(String twinId, MavlinkPacket pkt, TwinUpdateContext context) {
-
-    if (!(pkt instanceof SysStatusPacket packet)) {
-      return;
-    }
-
-    if (!packet.isValid()) {
+    if (!(pkt instanceof SysStatusPacket packet) || !packet.isValid()) {
       return;
     }
 
-    Instant now = (context != null && context.getReceivedTime() != null)
-        ? context.getReceivedTime()
-        : Instant.now();
-
-    twinManager.updateTwin(twinId, twin -> {
-      DroneTwin drone = (DroneTwin) twin;
-
-      updateBatteryState(drone, packet);
-      updateSystemState(drone, packet);
-
-      drone.setPowerUpdatedAt(now);
-      drone.setOperationalUpdatedAt(now);
-
-    }, context);
+    Instant now = context != null && context.getReceivedTime() != null ? context.getReceivedTime() : Instant.now();
+
+    twinManager.updateTwin(
+        twinId,
+        twin -> {
+          DroneTwin drone = (DroneTwin) twin;
+          updateBatteryState(drone, packet);
+          updateSystemState(drone, packet);
+          drone.setPowerUpdatedAt(now);
+          drone.setOperationalUpdatedAt(now);
+        },
+        context);
   }
 
   private void updateBatteryState(DroneTwin drone, SysStatusPacket packet) {
@@ -77,11 +69,9 @@ private void updateBatteryState(DroneTwin drone, SysStatusPacket packet) {
     if (!Double.isNaN(packet.getVoltageVolts())) {
       batteryState.setVoltageVolts(packet.getVoltageVolts());
     }
-
     if (!Double.isNaN(packet.getCurrentAmps())) {
       batteryState.setCurrentAmps(packet.getCurrentAmps());
     }
-
     if (!Double.isNaN(packet.getRemainingPercent())) {
       batteryState.setPercentage(packet.getRemainingPercent());
     }
@@ -99,10 +89,9 @@ private void updateSystemState(DroneTwin drone, SysStatusPacket packet) {
       systemState.setCpuLoadPercent(packet.getLoadPercent());
     }
 
-    boolean healthy = packet.getOnboardControlSensorsEnabled() == packet.getOnboardControlSensorsHealth();
+    boolean healthy = packet.areEnabledSensorsHealthy() && packet.areEnabledExtendedSensorsHealthy();
     systemState.setHealthy(healthy);
     systemState.setStatusMessage(buildStatusMessage(packet, healthy));
-
     drone.setSystemState(systemState);
   }
 
@@ -111,6 +100,14 @@ private String buildStatusMessage(SysStatusPacket packet, boolean healthy) {
       return "System health nominal";
     }
 
-    return "System health degraded";
+    long unhealthySensors = packet.getUnhealthyEnabledSensorsMask();
+    long unhealthyExtendedSensors = packet.getUnhealthyEnabledSensorsExtendedMask();
+    if (unhealthyExtendedSensors == 0) {
+      return "System health degraded: unhealthy enabled sensors 0x" + Long.toHexString(unhealthySensors).toUpperCase();
+    }
+    if (unhealthySensors == 0) {
+      return "System health degraded: unhealthy enabled extended sensors 0x" + Long.toHexString(unhealthyExtendedSensors).toUpperCase();
+    }
+    return "System health degraded: unhealthy enabled sensors 0x" + Long.toHexString(unhealthySensors).toUpperCase() + ", extended 0x" + Long.toHexString(unhealthyExtendedSensors).toUpperCase();
   }
-}
+}

src/main/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacket.java

@@ -19,12 +19,11 @@
 
 package io.mapsmessaging.state.mavlink.packet;
 
-import io.mapsmessaging.mavlink.ProcessedFrame;
-import lombok.Getter;
+import static io.mapsmessaging.state.mavlink.packet.MavlinkMessageIds.SYS_STATUS;
 
+import io.mapsmessaging.mavlink.ProcessedFrame;
 import java.util.Map;
-
-import static io.mapsmessaging.state.mavlink.packet.MavlinkMessageIds.SYS_STATUS;
+import lombok.Getter;
 
 @Getter
 public class SysStatusPacket extends MavlinkPacket {
@@ -35,6 +34,7 @@ public class SysStatusPacket extends MavlinkPacket {
 
   private final long onboardControlSensorsPresentExtended;
   private final long onboardControlSensorsEnabledExtended;
+  private final long onboardControlSensorsHealthExtended;
 
   private final double loadPercent;
   private final double voltageVolts;
@@ -59,6 +59,7 @@ public SysStatusPacket(ProcessedFrame frame) {
 
     this.onboardControlSensorsPresentExtended = getUnsignedInt(fields, "onboard_control_sensors_present_extended");
     this.onboardControlSensorsEnabledExtended = getUnsignedInt(fields, "onboard_control_sensors_enabled_extended");
+    this.onboardControlSensorsHealthExtended = getUnsignedInt(fields, "onboard_control_sensors_health_extended");
 
     this.loadPercent = getLoadPercent(fields);
     this.voltageVolts = getVoltageVolts(fields);
@@ -80,15 +81,23 @@ public int getMessageId() {
   }
 
   public boolean areEnabledSensorsHealthy() {
-    return (onboardControlSensorsEnabled & ~onboardControlSensorsHealth) == 0;
+    return getUnhealthyEnabledSensorsMask() == 0;
+  }
+
+  public boolean areEnabledExtendedSensorsHealthy() {
+    return getUnhealthyEnabledSensorsExtendedMask() == 0;
+  }
+
+  public long getUnhealthyEnabledSensorsMask() {
+    return onboardControlSensorsEnabled & ~onboardControlSensorsHealth;
+  }
+
+  public long getUnhealthyEnabledSensorsExtendedMask() {
+    return onboardControlSensorsEnabledExtended & ~onboardControlSensorsHealthExtended;
   }
 
   public boolean hasCommunicationErrors() {
-    return errorsComm > 0
-        || errorsCount1 > 0
-        || errorsCount2 > 0
-        || errorsCount3 > 0
-        || errorsCount4 > 0;
+    return errorsComm > 0 || errorsCount1 > 0 || errorsCount2 > 0 || errorsCount3 > 0 || errorsCount4 > 0;
   }
 
   private double getLoadPercent(Map<String, Object> fields) {
@@ -155,4 +164,4 @@ private long getUnsignedInt(Map<String, Object> fields, String key) {
 
     return ((Number) value).longValue() & 0xFFFFFFFFL;
   }
-}
+}

src/test/java/io/mapsmessaging/state/mavlink/listener/SysStatusListenerHealthTest.java

@@ -0,0 +1,87 @@
+/*
+ *
+ *  Copyright [ 2020 - 2024 ] Matthew Buckton
+ *  Copyright [ 2024 - 2026 ] MapsMessaging B.V.
+ *
+ *  Licensed under the Apache License, Version 2.0 with the Commons Clause
+ *  (the "License"); you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at:
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *      https://commonsclause.com/
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+package io.mapsmessaging.state.mavlink.listener;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+import io.mapsmessaging.state.drone.core.TwinManager;
+import io.mapsmessaging.state.drone.drone.DroneTwin;
+import io.mapsmessaging.state.mavlink.packet.SysStatusPacket;
+import org.junit.jupiter.api.Test;
+
+class SysStatusListenerHealthTest {
+
+  @Test
+  void handle_whenAllEnabledSensorsAreHealthy_marksSystemHealthy() {
+    DroneTwin droneTwin = new DroneTwin("test-drone");
+    TwinManager twinManager = new TwinManager();
+    twinManager.registerTwin(droneTwin, null);
+
+    SysStatusPacket packet = packet(true, true, 0, 0);
+    new SysStatusListener(twinManager).handle(droneTwin.getTwinId(), packet, null);
+
+    assertTrue(droneTwin.getSystemState().getHealthy());
+    assertEquals("System health nominal", droneTwin.getSystemState().getStatusMessage());
+  }
+
+  @Test
+  void handle_whenEnabledSensorIsUnhealthy_marksSystemDegradedWithMask() {
+    DroneTwin droneTwin = new DroneTwin("test-drone");
+    TwinManager twinManager = new TwinManager();
+    twinManager.registerTwin(droneTwin, null);
+
+    SysStatusPacket packet = packet(false, true, 0x20, 0);
+    new SysStatusListener(twinManager).handle(droneTwin.getTwinId(), packet, null);
+
+    assertFalse(droneTwin.getSystemState().getHealthy());
+    assertEquals("System health degraded: unhealthy enabled sensors 0x20", droneTwin.getSystemState().getStatusMessage());
+  }
+
+  @Test
+  void handle_whenEnabledExtendedSensorIsUnhealthy_marksSystemDegradedWithExtendedMask() {
+    DroneTwin droneTwin = new DroneTwin("test-drone");
+    TwinManager twinManager = new TwinManager();
+    twinManager.registerTwin(droneTwin, null);
+
+    SysStatusPacket packet = packet(true, false, 0, 0x02);
+    new SysStatusListener(twinManager).handle(droneTwin.getTwinId(), packet, null);
+
+    assertFalse(droneTwin.getSystemState().getHealthy());
+    assertEquals("System health degraded: unhealthy enabled extended sensors 0x2", droneTwin.getSystemState().getStatusMessage());
+  }
+
+  private SysStatusPacket packet(boolean basicHealthy, boolean extendedHealthy, long basicMask, long extendedMask) {
+    SysStatusPacket packet = mock(SysStatusPacket.class);
+    when(packet.isValid()).thenReturn(true);
+    when(packet.getLoadPercent()).thenReturn(Double.NaN);
+    when(packet.getVoltageVolts()).thenReturn(Double.NaN);
+    when(packet.getCurrentAmps()).thenReturn(Double.NaN);
+    when(packet.getRemainingPercent()).thenReturn(Double.NaN);
+    when(packet.areEnabledSensorsHealthy()).thenReturn(basicHealthy);
+    when(packet.areEnabledExtendedSensorsHealthy()).thenReturn(extendedHealthy);
+    when(packet.getUnhealthyEnabledSensorsMask()).thenReturn(basicMask);
+    when(packet.getUnhealthyEnabledSensorsExtendedMask()).thenReturn(extendedMask);
+    return packet;
+  }
+}

src/test/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacketHealthTest.java

@@ -0,0 +1,66 @@
+/*
+ *
+ *  Copyright [ 2020 - 2024 ] Matthew Buckton
+ *  Copyright [ 2024 - 2026 ] MapsMessaging B.V.
+ *
+ *  Licensed under the Apache License, Version 2.0 with the Commons Clause
+ *  (the "License"); you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at:
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *      https://commonsclause.com/
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+package io.mapsmessaging.state.mavlink.packet;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+import io.mapsmessaging.mavlink.ProcessedFrame;
+import java.util.HashMap;
+import java.util.Map;
+import org.junit.jupiter.api.Test;
+
+class SysStatusPacketHealthTest {
+
+  @Test
+  void areEnabledSensorsHealthy_whenHealthContainsAdditionalDisabledSensors_returnsTrue() {
+    SysStatusPacket packet = packet(Map.of("onboard_control_sensors_enabled", 0x03, "onboard_control_sensors_health", 0x07));
+
+    assertTrue(packet.areEnabledSensorsHealthy());
+    assertEquals(0L, packet.getUnhealthyEnabledSensorsMask());
+  }
+
+  @Test
+  void areEnabledSensorsHealthy_whenEnabledSensorIsUnhealthy_returnsFalse() {
+    SysStatusPacket packet = packet(Map.of("onboard_control_sensors_enabled", 0x07, "onboard_control_sensors_health", 0x03));
+
+    assertFalse(packet.areEnabledSensorsHealthy());
+    assertEquals(0x04L, packet.getUnhealthyEnabledSensorsMask());
+  }
+
+  @Test
+  void areEnabledExtendedSensorsHealthy_usesExtendedHealthMask() {
+    SysStatusPacket packet = packet(Map.of("onboard_control_sensors_enabled_extended", 0x03, "onboard_control_sensors_health_extended", 0x01));
+
+    assertFalse(packet.areEnabledExtendedSensorsHealthy());
+    assertEquals(0x02L, packet.getUnhealthyEnabledSensorsExtendedMask());
+  }
+
+  private SysStatusPacket packet(Map<String, Object> healthFields) {
+    Map<String, Object> fields = new HashMap<>(healthFields);
+    ProcessedFrame frame = mock(ProcessedFrame.class);
+    when(frame.getFields()).thenReturn(fields);
+    when(frame.isValid()).thenReturn(true);
+    return new SysStatusPacket(frame);
+  }
+}