cal: transmit table to CAN bus

1 files changed, 54 insertions, 0 deletions

diff --git a/sw/cal/can.go b/sw/cal/can.go
new file mode 100644
index 0000000..53f00a0
--- /dev/null
+++ b/sw/cal/can.go
@@ -0,0 +1,54 @@
+package main
+
+import (
+	"context"
+	bin "encoding/binary"
+
+	"go.einride.tech/can"
+	"go.einride.tech/can/pkg/socketcan"
+)
+
+const (
+
+	// CAN IDs
+	tblCtrlId = 0x1272000
+	sigCtrlId = 0x1272100
+)
+
+// Write a calibration table to the EEPROM.
+func writeTable(tx *socketcan.Transmitter, tbl Table, sig int) error {
+	var i int
+	for i = 0; i < len(tbl.keys); i++ {
+		err := writeRow(tx, tbl.keys[i], tbl.vals[i], sig, i)
+		if err != nil {
+			return err
+		}
+	}
+	for ; i < tabRows; i++ {
+		err := writeRow(tx, tbl.keys[len(tbl.keys)-1], tbl.vals[len(tbl.keys)-1], sig, i)
+		if err != nil {
+			return err
+		}
+	}
+	return nil
+}
+
+func writeRow(tx *socketcan.Transmitter, key int32, val uint16, sig int, row int) error {
+	var data [8]byte
+	_, err := bin.Encode(data[0:4], bin.BigEndian, key)
+	if err != nil {
+		return err
+	}
+	_, err = bin.Encode(data[4:6], bin.BigEndian, val)
+	if err != nil {
+		return err
+	}
+
+	frame := can.Frame{
+		ID:         uint32(tblCtrlId) | uint32((sig<<5)&0xE0) | uint32(row&0x1F),
+		Length:     6,
+		Data:       data,
+		IsExtended: true,
+	}
+	return tx.TransmitFrame(context.Background(), frame)
+}