diff options
Diffstat (limited to 'sw')
| -rw-r--r-- | sw/cal/can.go | 138 | ||||
| -rw-r--r-- | sw/cal/signal.go | 5 | ||||
| -rw-r--r-- | sw/cal/table.go | 93 |
3 files changed, 82 insertions, 154 deletions
diff --git a/sw/cal/can.go b/sw/cal/can.go deleted file mode 100644 index 7869687..0000000 --- a/sw/cal/can.go +++ /dev/null @@ -1,138 +0,0 @@ -package main - -import ( - "context" - bin "encoding/binary" - "fmt" - "math" - "strings" - "time" - - "go.einride.tech/can" - "go.einride.tech/can/pkg/socketcan" -) - -const ( - tblCtrlId uint32 = 0x1272000 - sigCtrlId uint32 = 0x1272100 - - stdMask = 0x7FF - extMask = 0x1FFFFFFF - - timeout = 1 * time.Second - eepromWriteDelay = 5 * time.Millisecond -) - -// 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 - var i int - for i = 0; i < len(tbl.keys); i++ { - err := sendRow(tx, tbl.keys[i], tbl.vals[i], sig, i) - if err != nil { - return err - } - } - - // Fill rest of table with last row - for ; i < tabRows; i++ { - err := sendRow(tx, tbl.keys[len(tbl.keys)-1], tbl.vals[len(tbl.keys)-1], sig, i) - if err != nil { - return err - } - time.Sleep(eepromWriteDelay) - } - - return nil -} - -// Transmit a Table Control frame containing one row of a table. -func sendRow(tx *socketcan.Transmitter, key int32, val uint16, sig int, row int) error { - // Serialize DATA FIELD - var data [8]byte - if _, err := bin.Encode(data[0:4], bin.BigEndian, key); err != nil { - return err - } - if _, err := bin.Encode(data[4:6], bin.BigEndian, val); err != nil { - return err - } - - // Construct ID and send frame - frame := can.Frame{ - ID: uint32(tblCtrlId) | uint32((sig<<5)&0xE0) | uint32(row&0x1F), - Length: 6, - Data: data, - IsExtended: true, - } - ctx, _ := context.WithTimeout(context.Background(), timeout) - return transmit(tx, frame, ctx) -} - -// Transmit a CAN frame to the bus, retrying if the buffer is full. -func transmit(tx *socketcan.Transmitter, frame can.Frame, ctx context.Context) error { - for { - err := tx.TransmitFrame(ctx, frame) - if err == nil { - return nil // success - } else if strings.HasSuffix(err.Error(), "no buffer space available") { - continue // retry - } else { - return err // error, abort - } - } -} diff --git a/sw/cal/signal.go b/sw/cal/signal.go index 6591c50..446afb7 100644 --- a/sw/cal/signal.go +++ b/sw/cal/signal.go @@ -13,8 +13,8 @@ import ( ) const ( - sigCtrlId = 0x1272100 - sigCtrlMask = 0x1FFFF00 + sigCtrlId uint32 = 0x1272100 + sigCtrlMask uint32 = 0x1FFFF00 exide = 1 << 31 ) @@ -78,7 +78,6 @@ func (sig SignalDef) SendEncoding(bus canbus.Bus) error { var rsig SignalDef err = rsig.UnmarshalFrame(reply) if err == errWrongId { - fmt.Println("Wrong ID", reply, "; expected", sigCtrlId) continue } else if err != nil { return err diff --git a/sw/cal/table.go b/sw/cal/table.go index 610448a..7d63d8d 100644 --- a/sw/cal/table.go +++ b/sw/cal/table.go @@ -6,7 +6,6 @@ import ( bin "encoding/binary" "fmt" "slices" - "time" "go.einride.tech/can" @@ -14,8 +13,10 @@ import ( ) const ( - tblCtrlId uint32 = 0x1272000 - maxTabRows = 32 + tblCtrlId uint32 = 0x1272000 + tblCtrlMask uint32 = 0x1FFFF00 + + maxTabRows = 32 ) type Table struct { @@ -54,7 +55,6 @@ func (tbl Table) Send(bus canbus.Bus) error { 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 @@ -63,7 +63,6 @@ func (tbl Table) Send(bus canbus.Bus) error { if err := sendRow(tbl.sigIndex, uint8(i), lastRow, bus); err != nil { return err } - time.Sleep(eepromWriteDelay) } return nil @@ -71,22 +70,90 @@ func (tbl Table) Send(bus canbus.Bus) error { // 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 + frame, err := marshalTblCtrlFrame(sigIndex, rowIndex, row) + 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 := tblCtrlRequest(sigIndex, rowIndex) + 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 + } + rRow, rSigIndex, rRowIndex, err := unmarshalTblCtrlFrame(reply) + if err == errWrongId || rSigIndex != sigIndex || rRowIndex != rowIndex { + continue + } else if err != nil { + return err + } + + // Verify + if rRow == row { + fmt.Printf("Table %d row %d OK\n", sigIndex, rowIndex) + return nil // success + } else { + weprintf("Warning: table %d row %d verification failed; rewriting...\n", sigIndex, rowIndex) + continue + } + } + // Max retries exceeded + return fmt.Errorf("table %d row %d verification failed", sigIndex, rowIndex) +} + +func marshalTblCtrlFrame(sigIndex, rowIndex uint8, row Row) (can.Frame, error) { var data [8]byte if _, err := bin.Encode(data[0:4], bin.BigEndian, row.key); err != nil { - return err + return can.Frame{}, err } if _, err := bin.Encode(data[4:6], bin.BigEndian, row.val); err != nil { - return err + return can.Frame{}, err } - - // Construct ID and send frame - frame := can.Frame{ + return can.Frame{ ID: uint32(tblCtrlId) | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F), Length: 6, Data: data, IsExtended: true, + }, nil +} + +func unmarshalTblCtrlFrame(frame can.Frame) (row Row, sigIndex, rowIndex uint8, err error) { + if !frame.IsExtended || frame.ID&tblCtrlMask != tblCtrlId { + err = errWrongId + return + } + if frame.Length != 6 { + err = fmt.Errorf("Table Control frame has wrong DLC: %d", frame.Length) + return + } + sigIndex = uint8((frame.ID & 0xE0) >> 5) + rowIndex = uint8(frame.ID & 0x1F) + _, err = bin.Decode(frame.Data[0:4], bin.BigEndian, &row.key) + if err != nil { + return + } + _, err = bin.Decode(frame.Data[4:6], bin.BigEndian, &row.val) + return +} + +// Construct a Table Control REMOTE REQUEST frame. +func tblCtrlRequest(sigIndex, rowIndex uint8) can.Frame { + return can.Frame{ + ID: tblCtrlId | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F), + IsRemote: true, + IsExtended: true, } - ctx, _ := context.WithTimeout(context.Background(), timeout) - return bus.Send(ctx, frame) } |