cal: send Signal Control framess

3 files changed, 97 insertions, 12 deletions

diff --git a/sw/cal/can.go b/sw/cal/can.go
index 751ffd9..9feefa3 100644
--- a/sw/cal/can.go
+++ b/sw/cal/can.go
@@ -3,6 +3,8 @@ package main
 import (
 	"context"
 	bin "encoding/binary"
+	"fmt"
+	"math"
 	"strings"
 	"time"
 
@@ -11,12 +13,70 @@ import (
 )
 
 const (
-	tblCtrlId = 0x1272000
-	sigCtrlId = 0x1272100
+	tblCtrlId uint32 = 0x1272000
+	sigCtrlId uint32 = 0x1272100
 
-	timeout = 5 * time.Second
+	stdMask = 0x7FF
+	extMask = 0x1FFFFFFF
+
+	timeout = 1 * time.Second
 )
 
+// Transmit a signal's encoding in a Signal Control frame so the Interface can store it in its EEPROM.
+func sendEncoding(def SignalDef, sig int, tx *socketcan.Transmitter) error {
+	// Serialize DATA FIELD
+	var data [8]byte
+	if err := serializeEncodingData(def, &data); err != nil {
+		return err
+	}
+	fmt.Println(data)
+
+	// Construct ID and send frame
+	frame := can.Frame{
+		ID:         sigCtrlId | uint32(sig&0xF),
+		Length:     7,
+		Data:       data,
+		IsExtended: true,
+	}
+	ctx, _ := context.WithTimeout(context.Background(), timeout)
+	return transmit(tx, frame, ctx)
+}
+
+// Serialize the DATA FIELD of a Signal Control frame.
+func serializeEncodingData(def SignalDef, data *[8]byte) error {
+	// SigId field
+	if _, err := bin.Encode(data[0:4], bin.BigEndian, uint32(def.id)&extMask); err != nil {
+		return err
+	}
+	if def.id.IsExtended() {
+		data[0] |= 0x80 // EXIDE
+	}
+
+	// Start field
+	if def.start > math.MaxUint8 {
+		return fmt.Errorf("%s: start bit out of range: %d>%d", def.name, def.start, math.MaxUint8)
+	}
+	data[4] = uint8(def.start)
+
+	// Size field
+	if def.size > math.MaxUint8 {
+		return fmt.Errorf("%s: size out of range: %d>%d", def.name, def.size, math.MaxUint8)
+	}
+	data[5] = uint8(def.size)
+
+	// Order and sign flag bits
+	if def.isBigEndian {
+		data[6] |= 0x80
+	}
+	if def.isSigned {
+		data[6] |= 0x40
+	}
+
+	fmt.Println(data)
+
+	return nil
+}
+
 // Transmit a table in Table Control frames so the Interface can store it in its EEPROM.
 func sendTable(tx *socketcan.Transmitter, tbl Table, sig int) error {
 	// Send populated rows
@@ -43,12 +103,10 @@ func sendTable(tx *socketcan.Transmitter, tbl Table, sig int) error {
 func sendRow(tx *socketcan.Transmitter, key int32, val uint16, sig int, row int) error {
 	// Serialize DATA FIELD
 	var data [8]byte
-	_, err := bin.Encode(data[0:4], bin.BigEndian, key)
-	if err != nil {
+	if _, err := bin.Encode(data[0:4], bin.BigEndian, key); err != nil {
 		return err
 	}
-	_, err = bin.Encode(data[4:6], bin.BigEndian, val)
-	if err != nil {
+	if _, err := bin.Encode(data[4:6], bin.BigEndian, val); err != nil {
 		return err
 	}
 
diff --git a/sw/cal/dbc.go b/sw/cal/dbc.go
index 7aa61bd..f746504 100644
--- a/sw/cal/dbc.go
+++ b/sw/cal/dbc.go
@@ -8,8 +8,27 @@ import (
 	"go.einride.tech/can/pkg/dbc"
 )
 
+type SignalDef struct {
+	id          dbc.MessageID
+	name        string
+	start, size uint64
+	isBigEndian bool
+	isSigned    bool
+}
+
+func newSignalDef(msg *dbc.MessageDef, sig dbc.SignalDef) SignalDef {
+	return SignalDef{
+		msg.MessageID,
+		string(sig.Name),
+		sig.StartBit,
+		sig.Size,
+		sig.IsBigEndian,
+		sig.IsSigned,
+	}
+}
+
 // Extract signals from the DBC file.
-func parseSignals(filename string, names map[int]string) (map[int]dbc.SignalDef, error) {
+func parseSignals(filename string, names map[int]string) (map[int]SignalDef, error) {
 	// Parse DBC file
 	msgDefs, err := parseDbcFile(filename)
 	if err != nil {
@@ -17,7 +36,7 @@ func parseSignals(filename string, names map[int]string) (map[int]dbc.SignalDef,
 	}
 
 	// Search for signals
-	signals := make(map[int]dbc.SignalDef)
+	signals := make(map[int]SignalDef)
 	for _, msg := range msgDefs {
 		for k, name := range names {
 			i := slices.IndexFunc(msg.Signals, func(sig dbc.SignalDef) bool { return sig.Name == dbc.Identifier(name) })
@@ -27,7 +46,8 @@ func parseSignals(filename string, names map[int]string) (map[int]dbc.SignalDef,
 			if _, ok := signals[k]; ok {
 				return nil, ErrDupSig{msg.Signals[i]}
 			}
-			signals[k] = msg.Signals[i]
+			fmt.Println(msg.Signals[i])
+			signals[k] = newSignalDef(msg, msg.Signals[i])
 		}
 	}
 
diff --git a/sw/cal/main.go b/sw/cal/main.go
index d87dd64..bfd0f11 100644
--- a/sw/cal/main.go
+++ b/sw/cal/main.go
@@ -122,8 +122,15 @@ func sendEncodings(dbcFilename string, sigNames map[int]string, tx *socketcan.Tr
 	}
 
 	// Transmit Signal Control frames
+	for k, sigDef := range sigDefs {
+		fmt.Printf("Transmitting encoding of signal %d: %s\n", k, sigDef.name)
+		fmt.Println(sigDef)
+		if err := sendEncoding(sigDef, k, tx); err != nil {
+			return err
+		}
+	}
 
-	// TODO
+	return nil
 }
 
 // Parse each table and transmit them using Table Control frames.
@@ -135,7 +142,7 @@ func sendTables(tblFilenames map[int]string, tx *socketcan.Transmitter) error {
 			return err
 		}
 
-		fmt.Println("Transmitting", filename)
+		fmt.Printf("Transmitting table %d: %s\n", k, filename)
 		if err := sendTable(tx, tbl, k); err != nil {
 			return err
 		}