From 86f3f0c8e3958b93a869f5e16acf606dc0c300e5 Mon Sep 17 00:00:00 2001
From: Hendrik van Wyk <gh@hpdvanwyk.com>
Date: Fri, 25 Sep 2020 15:03:26 +0200
Subject: [PATCH] Fix scaling to more closely match the Victron documentation.

We were decoding the scale as unsigned while it is signed. We were also
ignoring the fact that the sign of the scale determines the signedness of
the value it scales.
---
 mk2driver/mk2.go | 75 ++++++++++++++++++++++++++++++++----------------
 1 file changed, 50 insertions(+), 25 deletions(-)

diff --git a/mk2driver/mk2.go b/mk2driver/mk2.go
index c8d0fc8..7a34be7 100644
--- a/mk2driver/mk2.go
+++ b/mk2driver/mk2.go
@@ -4,7 +4,6 @@ import (
 	"errors"
 	"fmt"
 	"io"
-	"math"
 	"sync"
 	"time"
 
@@ -14,6 +13,7 @@ import (
 type scaling struct {
 	scale     float64
 	offset    float64
+	signed    bool
 	supported bool
 }
 
@@ -233,6 +233,13 @@ func (m *mk2Ser) reqScaleFactor(in byte) {
 	m.sendCommand(cmd)
 }
 
+func int16Abs(in int16) uint16 {
+	if in < 0 {
+		return uint16(-in)
+	}
+	return uint16(in)
+}
+
 // Decode the scale factor frame.
 func (m *mk2Ser) scaleDecode(frame []byte) {
 	tmp := scaling{}
@@ -240,27 +247,26 @@ func (m *mk2Ser) scaleDecode(frame []byte) {
 	if len(frame) < 6 {
 		tmp.supported = false
 		logrus.Warnf("Skiping scaling factors for: %d", m.scaleCount)
-	} else if len(frame) == 6 {
-		tmp.supported = true
-		scl := uint16(frame[2])<<8 + uint16(frame[1])
-		ofs := int16(uint16(frame[4])<<8 + uint16(frame[3]))
-
-		tmp.offset = float64(ofs)
-		if scl >= 0x4000 {
-			tmp.scale = math.Abs(1 / (0x8000 - float64(scl)))
-		} else {
-			tmp.scale = math.Abs(float64(scl))
-		}
 	} else {
 		tmp.supported = true
-		scl := uint16(frame[2])<<8 + uint16(frame[1])
-		ofs := int16(uint16(frame[5])<<8 + uint16(frame[4]))
-
-		tmp.offset = float64(ofs)
-		if scl >= 0x4000 {
-			tmp.scale = math.Abs(1 / (0x8000 - float64(scl)))
+		var scl int16
+		var ofs int16
+		if len(frame) == 6 {
+			scl = int16(frame[2])<<8 + int16(frame[1])
+			ofs = int16(uint16(frame[4])<<8 + uint16(frame[3]))
 		} else {
-			tmp.scale = math.Abs(float64(scl))
+			scl = int16(frame[2])<<8 + int16(frame[1])
+			ofs = int16(uint16(frame[5])<<8 + uint16(frame[4]))
+		}
+		if scl < 0 {
+			tmp.signed = true
+		}
+		tmp.offset = float64(ofs)
+		scale := int16Abs(scl)
+		if scale >= 0x4000 {
+			tmp.scale = 1 / (0x8000 - float64(scale))
+		} else {
+			tmp.scale = float64(scale)
 		}
 	}
 	m.scales = append(m.scales, tmp)
@@ -292,6 +298,20 @@ func (m *mk2Ser) versionDecode(frame []byte) {
 	}
 }
 
+// Decode with correct signedness and apply scale
+func (m *mk2Ser) applyScaleAndSign(data []byte, scale int) float64 {
+	var value float64
+	if !m.scales[scale].supported {
+		return 0
+	}
+	if m.scales[scale].signed {
+		value = getSigned(data)
+	} else {
+		value = getUnsigned16(data)
+	}
+	return m.applyScale(value, scale)
+}
+
 // Apply scaling to float
 func (m *mk2Ser) applyScale(value float64, scale int) float64 {
 	if !m.scales[scale].supported {
@@ -305,6 +325,11 @@ func getSigned(data []byte) float64 {
 	return float64(int16(data[0]) + int16(data[1])<<8)
 }
 
+// Convert bytes->int16->float
+func getUnsigned16(data []byte) float64 {
+	return float64(uint16(data[0]) + uint16(data[1])<<8)
+}
+
 // Convert bytes->uint32->float
 func getUnsigned(data []byte) float64 {
 	return float64(uint32(data[0]) + uint32(data[1])<<8 + uint32(data[2])<<16)
@@ -312,7 +337,7 @@ func getUnsigned(data []byte) float64 {
 
 // Decodes DC frame.
 func (m *mk2Ser) dcDecode(frame []byte) {
-	m.info.BatVoltage = m.applyScale(getSigned(frame[5:7]), ramVarVBat)
+	m.info.BatVoltage = m.applyScaleAndSign(frame[5:7], ramVarVBat)
 
 	usedC := m.applyScale(getUnsigned(frame[7:10]), ramVarIBat)
 	chargeC := m.applyScale(getUnsigned(frame[10:13]), ramVarIBat)
@@ -329,10 +354,10 @@ func (m *mk2Ser) dcDecode(frame []byte) {
 
 // Decodes AC frame.
 func (m *mk2Ser) acDecode(frame []byte) {
-	m.info.InVoltage = m.applyScale(getSigned(frame[5:7]), ramVarVMains)
-	m.info.InCurrent = m.applyScale(getSigned(frame[7:9]), ramVarIMains)
-	m.info.OutVoltage = m.applyScale(getSigned(frame[9:11]), ramVarVInverter)
-	m.info.OutCurrent = m.applyScale(getSigned(frame[11:13]), ramVarIInverter)
+	m.info.InVoltage = m.applyScaleAndSign(frame[5:7], ramVarVMains)
+	m.info.InCurrent = m.applyScaleAndSign(frame[7:9], ramVarIMains)
+	m.info.OutVoltage = m.applyScaleAndSign(frame[9:11], ramVarVInverter)
+	m.info.OutCurrent = m.applyScaleAndSign(frame[11:13], ramVarIInverter)
 
 	if frame[13] == 0xff {
 		m.info.InFrequency = 0
@@ -348,7 +373,7 @@ func (m *mk2Ser) acDecode(frame []byte) {
 
 // Decode charge state of battery.
 func (m *mk2Ser) stateDecode(frame []byte) {
-	m.info.ChargeState = m.applyScale(getSigned(frame[1:3]), ramVarChargeState)
+	m.info.ChargeState = m.applyScaleAndSign(frame[1:3], ramVarChargeState)
 	m.updateReport()
 }