cal: transmit table to CAN bus

6 files changed, 132 insertions, 28 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)
+}
diff --git a/sw/cal/csv.go b/sw/cal/csv.go
index bb5cd01..cdd5b44 100644
--- a/sw/cal/csv.go
+++ b/sw/cal/csv.go
@@ -1,33 +1,33 @@
 package main
 
 import (
-	"os"
+	"encoding/csv"
 	"fmt"
 	"io"
+	"os"
 	"strconv"
-	"encoding/csv"
 )
 
-func parseTable(filename string) (map[int32]uint16, error) {
+func parseTable(filename string) (Table, error) {
 	f, err := os.Open(filename)
 	if err != nil {
 		eprintf("%v\n", err)
 	}
 	defer f.Close()
 
-	tbl := make(map[int32]uint16)
+	var tbl Table
 	rdr := csv.NewReader(f)
 	for {
-		err := parseRow(rdr, tbl)
+		err := parseRow(rdr, &tbl)
 		if err == io.EOF {
 			return tbl, nil
 		} else if err != nil {
-			return nil, fmt.Errorf("%s:%v", filename, err)
+			return Table{}, fmt.Errorf("%s:%v", filename, err)
 		}
 	}
 }
 
-func parseRow(rdr *csv.Reader, tbl map[int32]uint16) error {
+func parseRow(rdr *csv.Reader, tbl *Table) error {
 	row, err := rdr.Read()
 	if err != nil {
 		return err
@@ -46,10 +46,9 @@ func parseRow(rdr *csv.Reader, tbl map[int32]uint16) error {
 		line, col := rdr.FieldPos(1)
 		return fmt.Errorf("%d:%d: %v", line, col, err)
 	}
-	if _, ok := tbl[int32(key)]; ok {
+	if err := tbl.Insert(int32(key), uint16(val)); err != nil {
 		line, col := rdr.FieldPos(0)
-		return fmt.Errorf("%d: %d: duplicate key %d", line, col, key)
+		return fmt.Errorf("%d:%d: %v", line, col, err)
 	}
-	tbl[int32(key)] = uint16(val)
 	return nil
 }
diff --git a/sw/cal/dbc.go b/sw/cal/dbc.go
index 53ca852..7aa61bd 100644
--- a/sw/cal/dbc.go
+++ b/sw/cal/dbc.go
@@ -9,7 +9,7 @@ import (
 )
 
 // Extract signals from the DBC file.
-func parseSignals(filename string, names []string) ([]dbc.SignalDef, error) {
+func parseSignals(filename string, names map[int]string) (map[int]dbc.SignalDef, error) {
 	// Parse DBC file
 	msgDefs, err := parseDbcFile(filename)
 	if err != nil {
@@ -17,29 +17,27 @@ func parseSignals(filename string, names []string) ([]dbc.SignalDef, error) {
 	}
 
 	// Search for signals
-	sigPtrs := make([]*dbc.SignalDef, len(names))
+	signals := make(map[int]dbc.SignalDef)
 	for _, msg := range msgDefs {
-		for i := range names {
-			j := slices.IndexFunc(msg.Signals, func(sig dbc.SignalDef) bool { return sig.Name == dbc.Identifier(names[i]) })
-			if j < 0 {
+		for k, name := range names {
+			i := slices.IndexFunc(msg.Signals, func(sig dbc.SignalDef) bool { return sig.Name == dbc.Identifier(name) })
+			if i < 0 {
 				continue
 			}
-			if sigPtrs[i] != nil {
-				return nil, ErrDupSig{msg.Signals[j]}
+			if _, ok := signals[k]; ok {
+				return nil, ErrDupSig{msg.Signals[i]}
 			}
-			sigPtrs[i] = &msg.Signals[j]
+			signals[k] = msg.Signals[i]
 		}
 	}
 
 	// Check all signals are present
-	if i := slices.IndexFunc(sigPtrs, func(sp *dbc.SignalDef) bool { return sp == nil }); i >= 0 {
-		return nil, ErrNoSig{filename, names[i]}
-	}
-
-	// Dereference
-	signals := make([]dbc.SignalDef, len(sigPtrs))
-	for i := range sigPtrs {
-		signals[i] = *sigPtrs[i]
+	if len(signals) != len(names) {
+		for k, name := range names {
+			if _, ok := signals[k]; !ok {
+				return nil, ErrNoSig{filename, name}
+			}
+		}
 	}
 
 	return signals, nil
diff --git a/sw/cal/err.go b/sw/cal/err.go
index 452e5f0..26ef5df 100644
--- a/sw/cal/err.go
+++ b/sw/cal/err.go
@@ -32,3 +32,11 @@ type ErrNoSig struct {
 func (e ErrNoSig) Error() string {
 	return fmt.Sprintf("%s: no such signal '%s'", e.filename, e.signal)
 }
+
+type ErrDupKey struct {
+	key int32
+}
+
+func (e ErrDupKey) Error() string {
+	return fmt.Sprintf("duplicate key %d", e.key)
+}
diff --git a/sw/cal/main.go b/sw/cal/main.go
index a6e4b1d..c2b7209 100644
--- a/sw/cal/main.go
+++ b/sw/cal/main.go
@@ -6,6 +6,7 @@ import (
 	"os"
 
 	"go.einride.tech/can/pkg/dbc"
+	"go.einride.tech/can/pkg/socketcan"
 )
 
 const dev = "can0"
@@ -20,6 +21,10 @@ var (
 	dbcFilenameFlag = "dbc"
 	dbcFilename     = flag.String(dbcFilenameFlag, "", "DBC file name")
 
+	// SocketCAN device
+	canDevFlag = "can"
+	canDev     = flag.String(canDevFlag, "can0", "SocketCAN device")
+
 	// Signal names
 	tachSigFlag  = "tachSig"
 	speedSigFlag = "speedSig"
@@ -70,13 +75,25 @@ func main() {
 		eprintf("%v\n", err)
 	}
 
-	// Parse calibration tables
-	for _, filename := range tblFilenames {
+	// Open CAN connection
+	conn, err := socketcan.Dial("can", *canDev)
+	if err != nil {
+		eprintf("%v\n", err)
+	}
+	defer conn.Close()
+	tx := socketcan.NewTransmitter(conn)
+
+	// Write calibration tables to EEPROM
+	for k, filename := range tblFilenames {
 		fmt.Println("Parsing", filename)
 		tbl, err := parseTable(filename)
 		if err != nil {
 			eprintf("%v\n", err)
 		}
+
+		if err := writeTable(tx, tbl, k); err != nil {
+			eprintf("%v\n", err)
+		}
 	}
 
 	fmt.Println(sigDefs)
diff --git a/sw/cal/table.go b/sw/cal/table.go
new file mode 100644
index 0000000..20d21d0
--- /dev/null
+++ b/sw/cal/table.go
@@ -0,0 +1,28 @@
+package main
+
+import (
+	"fmt"
+	"slices"
+)
+
+const tabRows = 32
+
+type Table struct {
+	keys []int32
+	vals []uint16
+}
+
+func (t *Table) Insert(key int32, val uint16) error {
+	if len(t.keys) >= tabRows {
+		return fmt.Errorf("too many rows")
+	}
+
+	i, ok := slices.BinarySearch(t.keys, key)
+	if ok {
+		return ErrDupKey{key}
+	}
+	t.keys = slices.Insert(t.keys, i, key)
+	t.vals = slices.Insert(t.vals, i, val)
+
+	return nil
+}