From 3e21efb72147b349d5c3a389bbe70b705ccfd20d Mon Sep 17 00:00:00 2001
From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Date: Tue, 8 Mar 2022 15:47:11 -0900
Subject: [PATCH] ina228: fix sign error on CURRENT reading (#19296)

---
 src/drivers/power_monitor/ina228/ina228.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp
index 8edd8acd9d..581dcbdd70 100644
--- a/src/drivers/power_monitor/ina228/ina228.cpp
+++ b/src/drivers/power_monitor/ina228/ina228.cpp
@@ -124,7 +124,12 @@ int INA228::read(uint8_t address, int32_t &data)
 	const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 1);
 
 	if (ret == PX4_OK) {
-		data = swap32(received_bytes) >> ((32 - 24) + 4);
+		data = swap32(received_bytes) >> ((32 - 24) + 4); // Convert to 20bit value
+
+		// Handle negative 20bit twos complement
+		if (data & 0x80000) {
+			data = -((0x000FFFFF & ~data) + 1);
+		}
 
 	} else {
 		perf_count(_comms_errors);