diff --git a/src/main/java/io/mapsmessaging/state/mavlink/listener/SysStatusListener.java b/src/main/java/io/mapsmessaging/state/mavlink/listener/SysStatusListener.java index 5bc29d680..397094af8 100644 --- a/src/main/java/io/mapsmessaging/state/mavlink/listener/SysStatusListener.java +++ b/src/main/java/io/mapsmessaging/state/mavlink/listener/SysStatusListener.java @@ -19,6 +19,8 @@ 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; @@ -26,11 +28,8 @@ 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(); } -} \ No newline at end of file +} diff --git a/src/main/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacket.java b/src/main/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacket.java index ab87cc468..88195d0d2 100644 --- a/src/main/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacket.java +++ b/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 fields) { @@ -155,4 +164,4 @@ private long getUnsignedInt(Map fields, String key) { return ((Number) value).longValue() & 0xFFFFFFFFL; } -} \ No newline at end of file +} diff --git a/src/test/java/io/mapsmessaging/state/mavlink/listener/SysStatusListenerHealthTest.java b/src/test/java/io/mapsmessaging/state/mavlink/listener/SysStatusListenerHealthTest.java new file mode 100644 index 000000000..622555d0b --- /dev/null +++ b/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; + } +} diff --git a/src/test/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacketHealthTest.java b/src/test/java/io/mapsmessaging/state/mavlink/packet/SysStatusPacketHealthTest.java new file mode 100644 index 000000000..ce9538a74 --- /dev/null +++ b/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 healthFields) { + Map fields = new HashMap<>(healthFields); + ProcessedFrame frame = mock(ProcessedFrame.class); + when(frame.getFields()).thenReturn(fields); + when(frame.isValid()).thenReturn(true); + return new SysStatusPacket(frame); + } +}