From 3674229adefd825575d1e0eae42d2c2f9a72aff0 Mon Sep 17 00:00:00 2001
From: alanbjohnston <alan.b.johnston@gmail.com>
Date: Tue, 23 Jul 2019 11:35:35 -0400
Subject: [PATCH] twosToInt

---
 afsk/main.c | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/afsk/main.c b/afsk/main.c
index ae2f715c..2aa7e9dc 100644
--- a/afsk/main.c
+++ b/afsk/main.c
@@ -112,7 +112,7 @@ struct SensorData read_sensor_data(struct SensorConfig sensor) {
     wiringPiI2CWriteReg16(sensor.fd, INA219_REG_CALIBRATION, sensor.calValue);
     int valuec1  = wiringPiI2CReadReg16(sensor.fd, INA219_REG_CURRENT);
     uint16_t valuec2 = (uint16_t)valuec1;
-    int twos = ((2 ^ 16) - valuec2) * (-1);
+    int twos = twosToInt(valuec2, 16);
     float valuec3  = (float)(valuec2);
     data.current  = valuec2 / (float)sensor.currentDivider;
     printf("****** valuec1 %d   valuec2 %d  valuec3 %f  current %f  two's %d \n", valuec1, valuec2, valuec3, data.current, twos);
@@ -416,3 +416,14 @@ int get_tlm(int tlm[][5]) {
 
   return 0;
 }
+
+int twosToInt( val, len) {   // Convert twos compliment to integer
+	
+      printf("##############    val: %d", val);
+	
+      if(val & (1 << len - 1))
+         val = val - (1 << len);
+      printf("len: %d  return: %d \n", len, val);
+
+      return(val);
+}