From c6573ad10a8d96d744324937195cd50bb1442c26 Mon Sep 17 00:00:00 2001 From: Sam Anthony Date: Sat, 8 Nov 2025 18:11:23 -0500 Subject: cal: verify signal encoding after writing --- sw/cal/canbus/canbus.go | 140 +++++++++++++++++++++++++++++++++++++++++++ sw/cal/dbc.go | 38 ++++-------- sw/cal/err.go | 5 ++ sw/cal/main.go | 49 +++++++++------ sw/cal/signal.go | 155 ++++++++++++++++++++++++++++++++++++++++++++++++ sw/cal/table.go | 78 +++++++++++++++++++++--- 6 files changed, 412 insertions(+), 53 deletions(-) create mode 100644 sw/cal/canbus/canbus.go create mode 100644 sw/cal/signal.go (limited to 'sw') diff --git a/sw/cal/canbus/canbus.go b/sw/cal/canbus/canbus.go new file mode 100644 index 0000000..031c456 --- /dev/null +++ b/sw/cal/canbus/canbus.go @@ -0,0 +1,140 @@ +package canbus + +import ( + "context" + "net" + "strings" + "time" + + "go.einride.tech/can" + "go.einride.tech/can/pkg/socketcan" +) + +const timeout = 1 * time.Second + +type Bus struct { + conn net.Conn + receiver *socketcan.Receiver + transmitter *socketcan.Transmitter + + tx <-chan txChan + + rx <-chan can.Frame + rxErr <-chan error + + cancel context.CancelFunc +} + +type txChan struct { + request chan<- transmission + err <-chan error +} + +type transmission struct { + context.Context + can.Frame +} + +func Connect(dev string) (Bus, error) { + conn, err := socketcan.Dial("can", dev) + if err != nil { + return Bus{}, err + } + + receiver := socketcan.NewReceiver(conn) + transmitter := socketcan.NewTransmitter(conn) + tx := make(chan txChan) + rx := make(chan can.Frame) + rxErr := make(chan error) + ctx, cancel := context.WithCancel(context.Background()) + + go transmit(ctx, transmitter, tx) + go receive(ctx, receiver, rx, rxErr) + + return Bus{conn, receiver, transmitter, tx, rx, rxErr, cancel}, nil +} + +func transmit(ctx context.Context, transmitter *socketcan.Transmitter, tx chan<- txChan) { + defer close(tx) + + for { + reqc := make(chan transmission) + errc := make(chan error) + select { + case tx <- txChan{reqc, errc}: + req := <-reqc + errc <- req.transmit(transmitter) + close(reqc) + close(errc) + case <-ctx.Done(): + close(reqc) + close(errc) + return + } + } +} + +func (t transmission) transmit(transmitter *socketcan.Transmitter) error { + ctx, _ := context.WithTimeout(context.Background(), timeout) + for { + select { + case <-ctx.Done(): + return ctx.Err() + default: + } + err := transmitter.TransmitFrame(t.Context, t.Frame) + if err == nil { + return nil // success + } else if strings.HasSuffix(err.Error(), "no buffer space available") { + continue // retry + } else { + return err + } + } +} + +func receive(ctx context.Context, receiver *socketcan.Receiver, rx chan<- can.Frame, rxErr chan<- error) { + defer close(rx) + defer close(rxErr) + + for receiver.Receive() { + frame := receiver.Frame() + select { + case rx <- frame: + case <-ctx.Done(): + return + } + } + rxErr <- receiver.Err() +} + +func (b Bus) Close() { + b.cancel() + b.transmitter.Close() + b.receiver.Close() + b.conn.Close() +} + +func (b Bus) Send(ctx context.Context, frame can.Frame) error { + c := <-b.tx + c.request <- transmission{ctx, frame} + return <-c.err +} + +func (b Bus) Receive(ctx context.Context) (can.Frame, error) { + select { + case frame := <-b.rx: + return frame, nil + case <-ctx.Done(): + return can.Frame{}, ctx.Err() + } +} + +func (b Bus) TryReceive() (can.Frame, bool) { + select { + case frame := <-b.rx: + return frame, true + default: + return can.Frame{}, false + } +} diff --git a/sw/cal/dbc.go b/sw/cal/dbc.go index f746504..0a22a14 100644 --- a/sw/cal/dbc.go +++ b/sw/cal/dbc.go @@ -8,27 +8,8 @@ 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]SignalDef, error) { +func parseSignals(filename string, names map[uint8]string) ([]SignalDef, error) { // Parse DBC file msgDefs, err := parseDbcFile(filename) if err != nil { @@ -36,26 +17,27 @@ func parseSignals(filename string, names map[int]string) (map[int]SignalDef, err } // Search for signals - signals := make(map[int]SignalDef) + signals := make([]SignalDef, 0, len(names)) 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) }) if i < 0 { continue } - if _, ok := signals[k]; ok { - return nil, ErrDupSig{msg.Signals[i]} + fmt.Printf("Found signal %v\n", msg.Signals[i]) + sig, err := NewSignalDef(k, msg, msg.Signals[i]) + if err != nil { + return nil, err } - fmt.Println(msg.Signals[i]) - signals[k] = newSignalDef(msg, msg.Signals[i]) + signals = append(signals, sig) } } // Check all signals are present if len(signals) != len(names) { - for k, name := range names { - if _, ok := signals[k]; !ok { - return nil, ErrNoSig{filename, name} + for i := 0; i < len(signals); i++ { + if names[uint8(i)] != signals[i].name { + return nil, ErrNoSig{filename, names[uint8(i)]} } } } diff --git a/sw/cal/err.go b/sw/cal/err.go index 26ef5df..4ceb1cc 100644 --- a/sw/cal/err.go +++ b/sw/cal/err.go @@ -1,12 +1,17 @@ package main import ( + "errors" "fmt" "os" "go.einride.tech/can/pkg/dbc" ) +var ( + errWrongId = errors.New("wrong ID") +) + func eprintf(format string, a ...any) { weprintf(format, a...) os.Exit(1) diff --git a/sw/cal/main.go b/sw/cal/main.go index bfd0f11..503324f 100644 --- a/sw/cal/main.go +++ b/sw/cal/main.go @@ -3,10 +3,22 @@ package main import ( "flag" "fmt" + "math" "os" + "time" "go.einride.tech/can/pkg/dbc" - "go.einride.tech/can/pkg/socketcan" + + "git.samanthony.xyz/can_gauge_interface/sw/cal/canbus" +) + +const ( + stdMask = 0x7FF + extMask = 0x1FFFFFFF + + timeout = 1 * time.Second + eepromWriteDelay = 5 * time.Millisecond + maxRetries = 8 ) type Signals struct { @@ -68,31 +80,33 @@ func main() { // Open CAN connection fmt.Println("Opening connection to", *canDev) - conn, err := socketcan.Dial("can", *canDev) + bus, err := canbus.Connect(*canDev) if err != nil { eprintf("%v\n", err) } - defer conn.Close() - tx := socketcan.NewTransmitter(conn) - defer tx.Close() + defer bus.Close() // Parse DBC file and transmit encoding of each signal - if err := sendEncodings(*dbcFilename, sigNames, tx); err != nil { + if err := sendEncodings(*dbcFilename, sigNames, bus); err != nil { eprintf("%v\n", err) } // Parse tables and transmit them - if err := sendTables(tblFilenames, tx); err != nil { + if err := sendTables(tblFilenames, bus); err != nil { eprintf("%v\n", err) } } // Return a map of non-empty strings keyed by their index in the given list. -func nonEmpty(ss ...string) map[int]string { - m := make(map[int]string) +func nonEmpty(ss ...string) map[uint8]string { + if len(ss) > math.MaxUint8 { + panic(len(ss)) + } + + m := make(map[uint8]string) for i := range ss { if ss[i] != "" { - m[i] = ss[i] + m[uint8(i)] = ss[i] } } return m @@ -113,19 +127,18 @@ func checkTablesProvided() error { } // Parse DBC file and transmit encoding of each signal using Signal Control frames. -func sendEncodings(dbcFilename string, sigNames map[int]string, tx *socketcan.Transmitter) error { +func sendEncodings(dbcFilename string, sigNames map[uint8]string, bus canbus.Bus) error { // Parse DBC file fmt.Println("Parsing", dbcFilename) - sigDefs, err := parseSignals(dbcFilename, sigNames) + sigs, err := parseSignals(dbcFilename, sigNames) if err != nil { return err } // 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 { + for _, sig := range sigs { + fmt.Println("Sending signal encoding", sig) + if err := sig.SendEncoding(bus); err != nil { return err } } @@ -134,7 +147,7 @@ func sendEncodings(dbcFilename string, sigNames map[int]string, tx *socketcan.Tr } // Parse each table and transmit them using Table Control frames. -func sendTables(tblFilenames map[int]string, tx *socketcan.Transmitter) error { +func sendTables(tblFilenames map[uint8]string, bus canbus.Bus) error { for k, filename := range tblFilenames { fmt.Println("Parsing", filename) tbl, err := parseTable(filename) @@ -143,7 +156,7 @@ func sendTables(tblFilenames map[int]string, tx *socketcan.Transmitter) error { } fmt.Printf("Transmitting table %d: %s\n", k, filename) - if err := sendTable(tx, tbl, k); err != nil { + if err := tbl.Send(bus); err != nil { return err } } diff --git a/sw/cal/signal.go b/sw/cal/signal.go new file mode 100644 index 0000000..6591c50 --- /dev/null +++ b/sw/cal/signal.go @@ -0,0 +1,155 @@ +package main + +import ( + "context" + bin "encoding/binary" + "fmt" + "math" + + "go.einride.tech/can" + "go.einride.tech/can/pkg/dbc" + + "git.samanthony.xyz/can_gauge_interface/sw/cal/canbus" +) + +const ( + sigCtrlId = 0x1272100 + sigCtrlMask = 0x1FFFF00 + + exide = 1 << 31 +) + +type SignalDef struct { + index uint8 + id uint32 + isExtended bool + name string + start, size uint8 + isBigEndian bool + isSigned bool +} + +func NewSignalDef(index uint8, msg *dbc.MessageDef, sig dbc.SignalDef) (SignalDef, error) { + if sig.StartBit > math.MaxUint8 { + return SignalDef{}, fmt.Errorf("%s: start bit out of range: %d", sig.Name, sig.StartBit) + } + if sig.Size > math.MaxUint8 { + return SignalDef{}, fmt.Errorf("%s: size out of range: %d", sig.Name, sig.Size) + } + + return SignalDef{ + index, + uint32(msg.MessageID) & extMask, + msg.MessageID.IsExtended(), + string(sig.Name), + uint8(sig.StartBit), + uint8(sig.Size), + sig.IsBigEndian, + sig.IsSigned, + }, nil +} + +// Transmit a signal's encoding in a Signal Control frame so the Interface can store it in its EEPROM. +func (sig SignalDef) SendEncoding(bus canbus.Bus) error { + frame, err := sig.MarshalFrame() + if err != nil { + return err + } + + var retry int + for retry = 0; retry < maxRetries; retry++ { + // Write to EEPROM + ctx, _ := context.WithTimeout(context.Background(), timeout) + if err := bus.Send(ctx, frame); err != nil { + return err + } + + // Read back + request := sigCtrlRequest(sig.index) + ctx, _ = context.WithTimeout(context.Background(), timeout) + if err := bus.Send(ctx, request); err != nil { + return err + } + ctx, _ = context.WithTimeout(context.Background(), timeout) + reply, err := bus.Receive(ctx) + if err != nil { + return err + } + var rsig SignalDef + err = rsig.UnmarshalFrame(reply) + if err == errWrongId { + fmt.Println("Wrong ID", reply, "; expected", sigCtrlId) + continue + } else if err != nil { + return err + } + + // Verify + rsig.index = sig.index + rsig.name = sig.name + fmt.Println("Received signal encoding", rsig) + if rsig == sig { + fmt.Printf("Signal %d encoding OK\n", sig.index) + return nil // success + } else { + weprintf("Warning: signal %d verification failed; rewriting...\n", sig.index) + continue + } + } + // Max retries exceeded + return fmt.Errorf("signal %d verification failed", sig.index) +} + +// Construct a Signal Control REMOTE REQUEST frame. +func sigCtrlRequest(index uint8) can.Frame { + return can.Frame{ + ID: sigCtrlId | uint32(index&0xF), + IsRemote: true, + IsExtended: true, + } +} + +func (sig SignalDef) MarshalFrame() (can.Frame, error) { + var data [8]byte + if _, err := bin.Encode(data[0:4], bin.BigEndian, uint32(sig.id)&extMask); err != nil { + return can.Frame{}, err + } + if sig.isExtended { + data[0] |= 0x80 // EXIDE + } + data[4] = uint8(sig.start) + data[5] = uint8(sig.size) + if !sig.isBigEndian { + data[6] |= 0x80 + } + if sig.isSigned { + data[6] |= 0x40 + } + return can.Frame{ + ID: sigCtrlId | uint32(sig.index&0xF), + Length: 7, + Data: data, + IsExtended: true, + }, nil +} + +func (sig *SignalDef) UnmarshalFrame(frame can.Frame) error { + if !frame.IsExtended || frame.ID&sigCtrlMask != sigCtrlId { + return errWrongId + } + if frame.Length != 7 { + return fmt.Errorf("Signal Control frame has wrong DLC: %d", frame.Length) + } + sig.index = uint8(frame.ID & 0xF) + var id uint32 + if _, err := bin.Decode(frame.Data[0:4], bin.BigEndian, &id); err != nil { + return err + } + sig.id = id & extMask + sig.isExtended = id&exide != 0 + sig.start = frame.Data[4] + sig.size = frame.Data[5] + sig.isBigEndian = frame.Data[6]&0x80 == 0 + sig.isSigned = frame.Data[6]&0x40 != 0 + return nil +} diff --git a/sw/cal/table.go b/sw/cal/table.go index 20d21d0..610448a 100644 --- a/sw/cal/table.go +++ b/sw/cal/table.go @@ -1,28 +1,92 @@ package main import ( + "cmp" + "context" + bin "encoding/binary" "fmt" "slices" + "time" + + "go.einride.tech/can" + + "git.samanthony.xyz/can_gauge_interface/sw/cal/canbus" ) -const tabRows = 32 +const ( + tblCtrlId uint32 = 0x1272000 + maxTabRows = 32 +) type Table struct { - keys []int32 - vals []uint16 + sigIndex uint8 + rows []Row +} + +type Row struct { + key int32 + val uint16 } func (t *Table) Insert(key int32, val uint16) error { - if len(t.keys) >= tabRows { + if len(t.rows) >= maxTabRows { return fmt.Errorf("too many rows") } - i, ok := slices.BinarySearch(t.keys, key) + i, ok := slices.BinarySearchFunc(t.rows, key, cmpRowKey) if ok { return ErrDupKey{key} } - t.keys = slices.Insert(t.keys, i, key) - t.vals = slices.Insert(t.vals, i, val) + t.rows = slices.Insert(t.rows, i, Row{key, val}) + + return nil +} + +func cmpRowKey(row Row, key int32) int { + return cmp.Compare(row.key, key) +} + +// Transmit a table in Table Control frames so the Interface can store it in its EEPROM. +func (tbl Table) Send(bus canbus.Bus) error { + // Send populated rows + var i int + for i = 0; i < len(tbl.rows); i++ { + if err := sendRow(tbl.sigIndex, uint8(i), tbl.rows[i], bus); err != nil { + return err + } + time.Sleep(eepromWriteDelay) + } + + // Fill rest of table with last row + lastRow := tbl.rows[len(tbl.rows)-1] + for ; i < maxTabRows; i++ { + if err := sendRow(tbl.sigIndex, uint8(i), lastRow, bus); err != nil { + return err + } + time.Sleep(eepromWriteDelay) + } return nil } + +// Transmit a Table Control frame containing one row of a table. +func sendRow(sigIndex, rowIndex uint8, row Row, bus canbus.Bus) error { + // Serialize DATA FIELD + var data [8]byte + if _, err := bin.Encode(data[0:4], bin.BigEndian, row.key); err != nil { + return err + } + if _, err := bin.Encode(data[4:6], bin.BigEndian, row.val); err != nil { + return err + } + + // Construct ID and send frame + frame := can.Frame{ + ID: uint32(tblCtrlId) | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F), + Length: 6, + Data: data, + IsExtended: true, + } + ctx, _ := context.WithTimeout(context.Background(), timeout) + return bus.Send(ctx, frame) +} -- cgit v1.2.3