From c991503e33f7e46d54dc0f24d406ebbd0c3784a6 Mon Sep 17 00:00:00 2001 From: Nicholas Thompson Date: Sat, 19 Sep 2020 18:38:00 +0200 Subject: [PATCH] Add mode-2 to scale factors --- mk2driver/mk2.go | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/mk2driver/mk2.go b/mk2driver/mk2.go index ba0af6e..c8d0fc8 100644 --- a/mk2driver/mk2.go +++ b/mk2driver/mk2.go @@ -236,9 +236,21 @@ func (m *mk2Ser) reqScaleFactor(in byte) { // Decode the scale factor frame. func (m *mk2Ser) scaleDecode(frame []byte) { tmp := scaling{} + logrus.Infof("Scale frame(%d): 0x%x", len(frame), frame) 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])