From 55fe0c3c7bcd0243493976195a1e4d3283e2bfb7 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Thu, 29 Dec 2016 22:41:38 +0100 Subject: Dig deeper --- cmd/experimental/main.go | 10 +++++++--- direction_string.go | 16 ++++++++++++++++ id_string.go | 14 +++++++++----- protocol.go | 42 ++++++++++++++++++++++++++++++++---------- protocol.txt | 6 +++--- protocol2.go | 40 +++++++++++++++++++++++++++++++++------- 6 files changed, 100 insertions(+), 28 deletions(-) create mode 100644 direction_string.go diff --git a/cmd/experimental/main.go b/cmd/experimental/main.go index 506cd2e..991d910 100644 --- a/cmd/experimental/main.go +++ b/cmd/experimental/main.go @@ -106,18 +106,22 @@ func onNotify(c *gatt.Characteristic, b []byte, err error) { } id, payload := anki.SplitMsg(b) switch id { + case anki.VehicleMsgV2CLaneChangeUpdate: + lu := anki.VehicleMsgLaneChangeUpdate{} + anki.Decode(b, &lu) + log.Printf("%+v", lu) case anki.VehicleMsgV2CStatusUpdate: su := anki.VehicleMsgStatusUpdate{} anki.Decode(b, &su) - log.Printf("%+v\n", su) + log.Printf("%+v", su) case anki.VehicleMsgV2CLocalizationTransitionUpdate: tu := anki.VehicleMsgLocalizationTransitionUpdate{} anki.Decode(b, &tu) - log.Printf("%+v\n", tu) + log.Printf("%+v", tu) case anki.VehicleMsgV2CLocalizationPositionUpdate: pu := anki.VehicleLocalizationPositionUpdate{} anki.Decode(b, &pu) - log.Printf("%+v\n", pu) + log.Printf("%+v", pu) default: log.Printf("ID: %v | % X\n", id, payload) } diff --git a/direction_string.go b/direction_string.go new file mode 100644 index 0000000..276d157 --- /dev/null +++ b/direction_string.go @@ -0,0 +1,16 @@ +// Code generated by "stringer -type=Direction"; DO NOT EDIT + +package anki + +import "fmt" + +const _Direction_name = "ForwardReverse" + +var _Direction_index = [...]uint8{0, 7, 14} + +func (i Direction) String() string { + if i >= Direction(len(_Direction_index)-1) { + return fmt.Sprintf("Direction(%d)", i) + } + return _Direction_name[_Direction_index[i]:_Direction_index[i+1]] +} diff --git a/id_string.go b/id_string.go index efd0daa..7058aea 100644 --- a/id_string.go +++ b/id_string.go @@ -4,7 +4,7 @@ package anki import "fmt" -const _ID_name = "VehicleMsgC2VDisconnectVehicleMsgC2VPingRequestVehicleMsgV2CPingResponseVehicleMsgC2VVersionRequestVehicleMsgV2CVersionResponseVehicleMsgC2VBatteryLevelRequestVehicleMsgV2CBatteryLevelResponseVehicleMsgC2VSetLightsVehicleMsgC2VSetSpeedVehicleMsgC2VChangeLaneVehicleMsgC2VCancelLaneChangeVehicleMsgV2CLocalizationPositionUpdateVehicleMsgV2CLocalizationTransitionUpdateVehicleMsgV2CLocalizationIntersectionUpdateVehicleMsgV2cVehicleDelocalizedVehicleMsgC2VSetOffsetFromRoadCenterVehicleMsgV2COffsetFromRoadCenterUpdateVehicleMsgC2VTurnVehicleMsgC2VLightsPatternVehicleMsgV2CSpeedUpdateVehicleMsgV2CStatusUpdateVehicleMsgC2VSetConfigParamsVehicleMsgV2CCollisionDetectedVehicleMsgV2CCycleOvertimeVehicleMsgC2VSDKMode" +const _ID_name = "VehicleMsgC2VDisconnectVehicleMsgC2VPingRequestVehicleMsgV2CPingResponseVehicleMsgC2VVersionRequestVehicleMsgV2CVersionResponseVehicleMsgC2VBatteryLevelRequestVehicleMsgV2CBatteryLevelResponseVehicleMsgC2VSetLightsVehicleMsgC2VSetSpeedVehicleMsgC2VChangeLaneVehicleMsgC2VCancelLaneChangeVehicleMsgV2CLocalizationPositionUpdateVehicleMsgV2CLocalizationTransitionUpdateVehicleMsgV2CLocalizationIntersectionUpdateVehicleMsgV2cVehicleDelocalizedVehicleMsgC2VSetOffsetFromRoadCenterVehicleMsgV2COffsetFromRoadCenterUpdateVehicleMsgC2VTurnVehicleMsgC2VLightsPatternVehicleMsgV2CSpeedUpdateVehicleMsgV2CStatusUpdateVehicleMsgV2CLaneChangeUpdateVehicleMsgV2CDelocAutoRecoveryEnteredVehicleMsgV2CDelocAutoRecoverySuccessVehicleMsgC2VSetConfigParamsVehicleMsgV2CCollisionDetectedVehicleMsgV2CCycleOvertimeVehicleMsgC2VSDKModeVehicleMsgV2CDebug" var _ID_map = map[ID]string{ 13: _ID_name[0:23], @@ -28,10 +28,14 @@ var _ID_map = map[ID]string{ 51: _ID_name[533:559], 54: _ID_name[559:583], 63: _ID_name[583:608], - 69: _ID_name[608:636], - 77: _ID_name[636:666], - 134: _ID_name[666:692], - 144: _ID_name[692:712], + 65: _ID_name[608:637], + 67: _ID_name[637:674], + 68: _ID_name[674:711], + 69: _ID_name[711:739], + 77: _ID_name[739:769], + 134: _ID_name[769:795], + 144: _ID_name[795:815], + 201: _ID_name[815:833], } func (i ID) String() string { diff --git a/protocol.go b/protocol.go index e23143c..8662cf5 100644 --- a/protocol.go +++ b/protocol.go @@ -3,6 +3,8 @@ package anki import ( "bytes" "encoding/binary" + "fmt" + "log" ) // VehicleMsgMaxSize = 20 @@ -72,7 +74,10 @@ func SplitMsg(b []byte) (id ID, payload []byte) { func Encode(v interface{}) []byte { buf := new(bytes.Buffer) - binary.Write(buf, binary.LittleEndian, v) + err := binary.Write(buf, binary.LittleEndian, v) + if err != nil { + log.Println("Encode", err) + } b := buf.Bytes()[:] z := make([]byte, len(b)) copy(z, b) // FIXME workaround "cgo argument has Go pointer to Go pointer" @@ -81,7 +86,10 @@ func Encode(v interface{}) []byte { func Decode(b []byte, v interface{}) { buf := bytes.NewBuffer(b) - binary.Read(buf, binary.LittleEndian, v) + err := binary.Read(buf, binary.LittleEndian, v) + if err != nil { + log.Println("Decode", err) + } } type VehicleMsgVersionResponse struct { @@ -103,7 +111,7 @@ type VehicleMsgSDKMode struct { Flags uint8 } -func (v *VehicleMsgSDKMode) Set(on, flags uint8) { +func (v *VehicleMsgSDKMode) Set(on uint8, flags uint8) { v.Size = uint8(binary.Size(v) - 1) v.MsgID = VehicleMsgC2VSDKMode v.On = on @@ -175,13 +183,24 @@ func (v *VehicleMsgChangeLane) Set(hspeed, haccel uint16, offset float32) { v.Offset = offset } +type Flags uint8 + const ( - ParseflagsMaskNumBits = 0x0f - ParseflagsMaskInvertedColor = 0x80 - ParseflagsMaskReverseParsing = 0x40 - ParseflagsMaskReverseDriving = 0x20 + ParseflagsMaskNumBits Flags = 0x0f + ParseflagsMaskInvertedColor Flags = 0x80 + ParseflagsMaskReverseParsing Flags = 0x40 + ParseflagsMaskReverseDriving Flags = 0x20 ) +func (f Flags) String() string { + return fmt.Sprintf("{Bits: %d InvColor: %t RevParse: %t RevDrive: %t}", + f&ParseflagsMaskNumBits, + f&ParseflagsMaskInvertedColor != 0, + f&ParseflagsMaskReverseParsing != 0, + f&ParseflagsMaskReverseDriving != 0, + ) +} + type VehicleLocalizationPositionUpdate struct { Size uint8 MsgID ID @@ -189,7 +208,7 @@ type VehicleLocalizationPositionUpdate struct { RoadPieceID uint8 Offset float32 // from road center mm Speed uint16 // mm/sec - ParsingFlags uint8 + ParsingFlags Flags // ACK commands received LastRecvLaneChangeCmdID uint8 @@ -198,9 +217,12 @@ type VehicleLocalizationPositionUpdate struct { LastDesiredSpeed uint16 // mm/sec } +//go:generate stringer -type=Direction +type Direction uint8 + // VehicleDrivingDirection const ( - Forward uint8 = iota + Forward Direction = iota Reverse ) @@ -211,7 +233,7 @@ type VehicleMsgLocalizationTransitionUpdate struct { RoadPieceIDXPrev uint8 Offset float32 // from road center mm - Direction uint8 // driving direction + Direction Direction // driving direction // ACK commands received LastRecvLaneChangeCmdID uint8 diff --git a/protocol.txt b/protocol.txt index f6b94bb..3cbd3c0 100644 --- a/protocol.txt +++ b/protocol.txt @@ -42,7 +42,7 @@ 0x3e 62 CarMsgWriteFlashResponse 0x3f 63 CarMsgStatusUpdate seen in stream 0x40 64 CarMsgSetTestMode -0x41 65 CarMsgLaneChangeUpdate +0x41 65 CarMsgLaneChangeUpdate seen on track 0x42 66 CarMsgMergeReactionUpdate 0x43 67 CarMsgDelocAutoRecoveryEntered seen on track 0x44 68 CarMsgDelocAutoRecoverySuccess @@ -72,6 +72,6 @@ 0x8f 143 CarMsgSDLogging 0x90 144 SDK Mode seen in SDK -0xc9 201 Debug? +0xc9 201 Debug? seen on track 0xcf 207 CarMsgMessageCycleCount -0xd0 208 CarMsgSteeringDiagnosticMetrics seen on track +0xd0 208 CarMsgSteeringDiagnosticMetrics diff --git a/protocol2.go b/protocol2.go index dbffd22..333b871 100644 --- a/protocol2.go +++ b/protocol2.go @@ -1,16 +1,20 @@ package anki const ( - VehicleMsgV2CSpeedUpdate ID = 0x36 - VehicleMsgV2CStatusUpdate ID = 0x3f - VehicleMsgV2CCollisionDetected ID = 0x4d - VehicleMsgV2CCycleOvertime ID = 0x86 + VehicleMsgV2CSpeedUpdate ID = 0x36 + VehicleMsgV2CStatusUpdate ID = 0x3f + VehicleMsgV2CLaneChangeUpdate ID = 0x41 + VehicleMsgV2CDelocAutoRecoveryEntered ID = 0x43 + VehicleMsgV2CDelocAutoRecoverySuccess ID = 0x44 + VehicleMsgV2CCollisionDetected ID = 0x4d + VehicleMsgV2CCycleOvertime ID = 0x86 + VehicleMsgV2CDebug ID = 0xc9 ) // 07 36 20 03 98 3A 2A 00 cchhh type VehicleMsgSpeedUpdate struct { Size uint8 - MsgID uint8 + MsgID ID DesiredSpeed uint16 // mm/sec Accel uint16 // mm/sec² CurrentSpeed uint16 // mm/sec @@ -19,17 +23,28 @@ type VehicleMsgSpeedUpdate struct { // 05 3F 01 00 01 00 type VehicleMsgStatusUpdate struct { Size uint8 - MsgID uint8 + MsgID ID IsOnTrack uint8 IsOnCharger uint8 HasLowBattery uint8 HasChargedBattery uint8 } +// ID(65) | 00 00 99 42 00 00 99 42 00 00 F4 01 81 +type VehicleMsgLaneChangeUpdate struct { + Size uint8 + MsgID ID + CurrentOffsetFromRoadCenter float32 + TargetOffsetFromRoadCenter float32 + HorizontalSpeed uint16 + VerticalSpeed uint16 + LaneChangeID uint8 +} + // 03 4D 01 00 type VehicleMsgCollisionDetected struct { Size uint8 - MsgID uint8 + MsgID ID WasSideOnCollision uint8 WasFrontBackCollision uint8 } @@ -42,3 +57,14 @@ type VehicleMsgCycleOvertime struct { AverageCycleTime uint32 // µsec MaxCycleTime uint32 // µsec } + +// ID(201) | 04 15 03 00 00 +// case 0: read char +// case 1: read char +// case 2: read uint16 +// case 3: read uint16: Debug message (s16) from vehicle +// case 4: read uint32 +// case 5: read uint32 +// case 6: read float: Debug message (float) from vehicle +// case 7: read char: Vehicle Error: Error triggered on vehicle +// default: Invalid CMD_DEBUG type -- cgit v1.2.3