added I2C current read

cw/cw_main.c

@@ -29,6 +29,7 @@
 #include <string.h>
 #include <unistd.h>
 #include <errno.h>
+#include <wiringPiI2C.h>
 
 #define MAX_MESSAGE_LENGTH (197)
 
@@ -80,6 +81,27 @@ int main(void)
          fprintf(stderr, "ERROR: Unable to enter TX mode\n");
          exit(EXIT_FAILURE);
     }
+   // Read current from I2C bus
+    int devId = 0x40; // +X Panel current
+    int i2cDevice = wiringPiI2CSetup (devId) ;
+    printf("\n\nCurrent setup result: %d\n", i2cDevice);
+    printf("Read: %d\n", wiringPiI2CRead(i2cDevice)) ;
+
+    int result = wiringPiI2CWriteReg16(i2cDevice, 0x05, 4096);
+    printf("Write result: %d\n", result);
+    
+    float current = 0.05 * wiringPiI2CReadReg16(i2cDevice, 0x04);
+    printf("Current: %f\n\n\n", current);
+   
+    // Read temperature from I2C bus
+    devId = 0x48; // temp 
+    i2cDevice = wiringPiI2CSetup (devId);
+    printf("\n\nTemp setup result: %d\n", i2cDevice);
+    int tempValue = wiringPiI2CRead(i2cDevice); 
+    printf("Read: %x\n", tempValue);
+    uint8_t upper = tempValue >> 8;
+    uint8_t lower = tempValue && 0xff;
+    printf("upper: %x lower: %x\n", upper, lower);  
 
     config_cw();
 
@@ -127,9 +149,23 @@ int main(void)
         int tlm_3a = (int)((vbat * 10) - 65.5);
     	
         printf("TLM 3A = %d \n", tlm_3a);
+
+       // Read current from I2C bus
+        int devId = 0x40; // +X Panel current
+        int i2cDevice = wiringPiI2CSetup (devId) ;
+        printf("\n\nI2C result: %d\n", i2cDevice);
+        printf("Read: %d\n", wiringPiI2CRead(i2cDevice)) ;
+
+        int result = wiringPiI2CWriteReg16(i2cDevice, 0x05, 4096);
+        printf("Write result: %d\n", result);
     
-            msg_length = encode_tlm(&packet[0], channel, // add a channel with dummy data to buffer
+        int currentValue = wiringPiI2CReadReg16(i2cDevice, 0x04);
-		 tlm_3a, channel+1, channel+2, channel+3,
+        printf("Current: %d\n\n\n", currentValue);
+        int tlm_1b = (int) (98.5 - currentValue/400);
+        printf("TLM 1B = %d \n\n", tlm_1b);
+
+         msg_length = encode_tlm(&packet[0], channel, // add a channel with dummy data to buffer
+		 tlm_3a, tlm_1b, channel+2, channel+3,
 			 (MAX_MESSAGE_LENGTH + 1));
        
             printf("\nINFO: Sending TLM channel %d \n", channel);