diff --git a/afsk/main.c b/afsk/main.c
index 7da0a81c..cce7a415 100644
--- a/afsk/main.c
+++ b/afsk/main.c
@@ -139,7 +139,8 @@ struct SensorData read_sensor_data(struct SensorConfig sensor) {
     data.voltage  =  ((float)(value >> 3) * 4) / 1000;
 	
 //    data.power   = (float) wiringPiI2CReadReg16(sensor.fd, INA219_REG_POWER) * (float) sensor.powerMultiplier;
-	
+    wiringPiI2CWriteReg16(sensor.fd, INA219_REG_CALIBRATION, sensor.calValue);
+    wiringPiI2CWriteReg16(sensor.fd, INA219_REG_CONFIG, sensor.config);		
     wiringPiI2CWriteReg16(sensor.fd, INA219_REG_CALIBRATION, sensor.calValue);
     wiringPiI2CWrite(sensor.fd, INA219_REG_POWER);
     delay(1); // Max 12-bit conversion time is 586us per sample