Martino Ferrari
90 lines
2.5 KiB
Go
90 lines
2.5 KiB
Go
package modbus
|
|
|
|
import (
|
|
"fmt"
|
|
"math"
|
|
)
|
|
|
|
// orderWords returns the registers in big-word-first order, reversing them when
|
|
// the register declares little word order. The slice is copied so the caller's
|
|
// data is left untouched.
|
|
func (r Register) orderWords(words []uint16) []uint16 {
|
|
if !r.littleWordOrder() {
|
|
return words
|
|
}
|
|
out := make([]uint16, len(words))
|
|
for i, w := range words {
|
|
out[len(words)-1-i] = w
|
|
}
|
|
return out
|
|
}
|
|
|
|
// decode converts the raw registers into the register's numeric value and
|
|
// applies scale/offset. The result type is int64 or float64 per dataType.
|
|
func (r Register) decode(words []uint16) (any, error) {
|
|
w := r.orderWords(words)
|
|
var raw float64
|
|
var rawInt int64
|
|
switch r.encoding() {
|
|
case "uint16":
|
|
rawInt = int64(w[0])
|
|
raw = float64(w[0])
|
|
case "int16":
|
|
rawInt = int64(int16(w[0]))
|
|
raw = float64(int16(w[0]))
|
|
case "uint32":
|
|
u := uint32(w[0])<<16 | uint32(w[1])
|
|
rawInt = int64(u)
|
|
raw = float64(u)
|
|
case "int32":
|
|
u := uint32(w[0])<<16 | uint32(w[1])
|
|
rawInt = int64(int32(u))
|
|
raw = float64(int32(u))
|
|
case "float32":
|
|
u := uint32(w[0])<<16 | uint32(w[1])
|
|
raw = float64(math.Float32frombits(u))
|
|
case "float64":
|
|
u := uint64(w[0])<<48 | uint64(w[1])<<32 | uint64(w[2])<<16 | uint64(w[3])
|
|
raw = math.Float64frombits(u)
|
|
default:
|
|
return nil, fmt.Errorf("modbus: unknown encoding %q", r.Encoding)
|
|
}
|
|
|
|
// Integer fast-path: no scaling/offset, integer encoding.
|
|
if r.scale() == 1 && r.Offset == 0 {
|
|
switch r.encoding() {
|
|
case "uint16", "int16", "uint32", "int32":
|
|
return rawInt, nil
|
|
}
|
|
}
|
|
return raw*r.scale() + r.Offset, nil
|
|
}
|
|
|
|
// encode converts a value destined for a Write back into raw registers,
|
|
// inverting scale/offset. Only the holding-register encodings are writable.
|
|
func (r Register) encode(value float64) ([]uint16, error) {
|
|
v := (value - r.Offset) / r.scale()
|
|
var words []uint16
|
|
switch r.encoding() {
|
|
case "uint16":
|
|
words = []uint16{uint16(int64(math.Round(v)))}
|
|
case "int16":
|
|
words = []uint16{uint16(int16(int64(math.Round(v))))}
|
|
case "uint32":
|
|
u := uint32(int64(math.Round(v)))
|
|
words = []uint16{uint16(u >> 16), uint16(u)}
|
|
case "int32":
|
|
u := uint32(int32(int64(math.Round(v))))
|
|
words = []uint16{uint16(u >> 16), uint16(u)}
|
|
case "float32":
|
|
u := math.Float32bits(float32(v))
|
|
words = []uint16{uint16(u >> 16), uint16(u)}
|
|
case "float64":
|
|
u := math.Float64bits(v)
|
|
words = []uint16{uint16(u >> 48), uint16(u >> 32), uint16(u >> 16), uint16(u)}
|
|
default:
|
|
return nil, fmt.Errorf("modbus: unknown encoding %q", r.Encoding)
|
|
}
|
|
return r.orderWords(words), nil
|
|
}
|