From 7e62f728fe33fd81ccadf11eca43d424a9260d09 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Fri, 30 Dec 2016 03:26:51 +0100 Subject: tweak around --- cmd/experimental/main.go | 78 ++++++++++++++++++++++++++++++++++-------------- protocol.go | 60 ++++++++++++++++++++++++++----------- protocol2.go | 25 +++++++++++----- 3 files changed, 115 insertions(+), 48 deletions(-) diff --git a/cmd/experimental/main.go b/cmd/experimental/main.go index d70c512..e82a366 100644 --- a/cmd/experimental/main.go +++ b/cmd/experimental/main.go @@ -59,23 +59,27 @@ func onConnect(p gatt.Peripheral, err error) { log.Println("Descriptor", d.UUID()) } */ + //log.Println("Properties", c.Properties()) if (c.Properties() & (gatt.CharNotify | gatt.CharIndicate)) != 0 { if err := p.SetNotifyValue(c, onNotify); err != nil { log.Println("Set notify", err) return } } - if (c.Properties() & gatt.CharWrite) != 0 { - //p.WriteCharacteristic(c, []byte{0x01, 0x90}, true) // sdk + if (c.Properties() & (gatt.CharWrite | gatt.CharWriteNR)) != 0 { + //p.WriteCharacteristic(c, []byte{0x03, 0x90, 0x01, 0x00}, true) // sdk ? + //p.WriteCharacteristic(c, []byte{0x01, 0x8c}, true) // reset stats ? + go func(c *gatt.Characteristic) { t := time.NewTicker(time.Second * 10) defer t.Stop() for range t.C { - log.Println("Ping") + //log.Println("Ping") p.WriteCharacteristic(c, []byte{0x01, 0x16}, true) // ping time.Sleep(10 * time.Second) } }(c) + //p.WriteCharacteristic(c, []byte{0x01, 0x18}, true) // version //p.WriteCharacteristic(c, []byte{0x01, 0x1a}, true) // battery @@ -85,23 +89,29 @@ func onConnect(p gatt.Peripheral, err error) { lm.Set(*lc) lc.Set(anki.LightFrontR, anki.EffectSteady, 10, 0, 0) lm.Set(*lc) - lc.Set(anki.LightTail, anki.EffectSteady, 10, 0, 0) + lc.Set(anki.LightTail, anki.EffectSteady, 5, 0, 0) lm.Set(*lc) lb := anki.Encode(lm) p.WriteCharacteristic(c, lb, true) - time.Sleep(time.Second) - ss := &anki.VehicleMsgSetSpeed{} - ss.Set(500, 2500) - b := anki.Encode(ss) - p.WriteCharacteristic(c, b, true) + go func(c *gatt.Characteristic) { + for i := 1; i <= 5; i++ { + time.Sleep(5 * time.Second) + log.Println("Set speed") + ss := &anki.VehicleMsgSetSpeed{} + ss.Set(anki.Speed(100*i), 1000) + b := anki.Encode(ss) + p.WriteCharacteristic(c, b, true) + } + }(c) go func(c *gatt.Characteristic) { - time.Sleep(5 * time.Second) + return // XXX disable for now + time.Sleep(15 * time.Second) log.Println("Change lane") cl := &anki.VehicleMsgChangeLane{} cl.Set(100, 100, 0.0) - b = anki.Encode(cl) + b := anki.Encode(cl) p.WriteCharacteristic(c, b, true) }(c) } @@ -115,22 +125,44 @@ func onNotify(c *gatt.Characteristic, b []byte, err error) { } id, payload := anki.SplitMsg(b) switch id { + case anki.VehicleMsgV2CPingResponse: + // ignore + case anki.VehicleMsgV2CCollisionDetected: + v := anki.VehicleMsgCollisionDetected{} + anki.Decode(b, &v) + log.Printf("%+v", v) + case anki.VehicleMsgV2CCycleOvertime: + v := anki.VehicleMsgCycleOvertime{} + anki.Decode(b, &v) + log.Printf("%+v", v) + case anki.VehicleMsgV2CBatteryLevelResponse: + v := anki.VehicleMsgBatteryLevelResponse{} + anki.Decode(b, &v) + log.Printf("%+v", v) + case anki.VehicleMsgV2CVersionResponse: + v := anki.VehicleMsgVersionResponse{} + anki.Decode(b, &v) + log.Printf("%+v", v) + case anki.VehicleMsgV2CSpeedUpdate: + v := anki.VehicleMsgSpeedUpdate{} + anki.Decode(b, &v) + log.Printf("%+v", v) case anki.VehicleMsgV2CLaneChangeUpdate: - lu := anki.VehicleMsgLaneChangeUpdate{} - anki.Decode(b, &lu) - log.Printf("%+v", lu) + v := anki.VehicleMsgLaneChangeUpdate{} + anki.Decode(b, &v) + log.Printf("%+v", v) case anki.VehicleMsgV2CStatusUpdate: - su := anki.VehicleMsgStatusUpdate{} - anki.Decode(b, &su) - log.Printf("%+v", su) + v := anki.VehicleMsgStatusUpdate{} + anki.Decode(b, &v) + log.Printf("%+v", v) case anki.VehicleMsgV2CLocalizationTransitionUpdate: - tu := anki.VehicleMsgLocalizationTransitionUpdate{} - anki.Decode(b, &tu) - log.Printf("%+v", tu) + v := anki.VehicleMsgLocalizationTransitionUpdate{} + anki.Decode(b, &v) + log.Printf("%+v", v) case anki.VehicleMsgV2CLocalizationPositionUpdate: - pu := anki.VehicleLocalizationPositionUpdate{} - anki.Decode(b, &pu) - log.Printf("%+v", pu) + v := anki.VehicleLocalizationPositionUpdate{} + anki.Decode(b, &v) + log.Printf("%+v", v) default: log.Printf("ID: %v | % X\n", id, payload) } diff --git a/protocol.go b/protocol.go index 113ee31..3317f55 100644 --- a/protocol.go +++ b/protocol.go @@ -98,10 +98,16 @@ type VehicleMsgVersionResponse struct { Version uint32 } -type VehicleBatteryLevelResponse struct { +type MilliVolt uint16 + +func (v MilliVolt) String() string { + return fmt.Sprintf("%6.4fV", float64(v)/1000.0) +} + +type VehicleMsgBatteryLevelResponse struct { Size uint8 MsgID ID - BatteryLevel uint16 // mV + BatteryLevel MilliVolt // mV } type VehicleMsgSDKMode struct { @@ -118,15 +124,27 @@ func (v *VehicleMsgSDKMode) Set(on uint8, flags uint8) { v.Flags = flags } +type Accel uint16 + +func (a Accel) String() string { + return fmt.Sprintf("%5.3fm/s²", float64(a)/1000.0) +} + +type Speed uint16 + +func (s Speed) String() string { + return fmt.Sprintf("%5.3fm/s", float64(s)/1000.0) +} + type VehicleMsgSetSpeed struct { Size uint8 MsgID ID - Speed uint16 // mm/sec - Accel uint16 // mm/sec² - SpeedLimit uint8 // respect road piece speed limit + Speed Speed // mm/sec + Accel Accel // mm/sec² + SpeedLimit uint8 // respect road piece speed limit } -func (v *VehicleMsgSetSpeed) Set(speed, accel uint16) { +func (v *VehicleMsgSetSpeed) Set(speed Speed, accel Accel) { v.Size = uint8(binary.Size(v) - 1) v.MsgID = VehicleMsgC2VSetSpeed v.Speed = speed @@ -174,10 +192,16 @@ see also: https://github.com/IBM-Bluemix/node-mqtt-for-anki-overdrive type VehicleMsgSetOffsetFromRoadCenter struct { Size uint8 MsgID ID - Offset float32 // mm + Offset Offset // mm +} + +type Offset float32 + +func (c Offset) String() string { + return fmt.Sprintf("%4.1fmm", c) } -func (v *VehicleMsgSetOffsetFromRoadCenter) Set(offset float32) { +func (v *VehicleMsgSetOffsetFromRoadCenter) Set(offset Offset) { v.Size = uint8(binary.Size(v) - 1) v.MsgID = VehicleMsgC2VSetOffsetFromRoadCenter v.Offset = offset @@ -186,14 +210,14 @@ func (v *VehicleMsgSetOffsetFromRoadCenter) Set(offset float32) { type VehicleMsgChangeLane struct { Size uint8 MsgID ID - HorizontalSpeed uint16 // mm/sec - HorizontalAccel uint16 // mm/sec² - Offset float32 // from road center mm + HorizontalSpeed Speed // mm/sec + HorizontalAccel Accel // mm/sec² + Offset Offset // from road center mm HopIntent uint8 Tag uint8 } -func (v *VehicleMsgChangeLane) Set(hspeed, haccel uint16, offset float32) { +func (v *VehicleMsgChangeLane) Set(hspeed Speed, haccel Accel, offset Offset) { v.Size = uint8(binary.Size(v) - 1) v.MsgID = VehicleMsgC2VChangeLane v.HorizontalSpeed = hspeed @@ -224,15 +248,15 @@ type VehicleLocalizationPositionUpdate struct { MsgID ID LocalizationID uint8 RoadPieceID uint8 - Offset float32 // from road center mm - Speed uint16 // mm/sec + Offset Offset // from road center mm + Speed Speed // mm/sec ParsingFlags Flags // ACK commands received LastRecvLaneChangeCmdID uint8 LastExecLaneChangeCmdID uint8 - LastDesiredHorizontalSpeed uint16 // mm/sec - LastDesiredSpeed uint16 // mm/sec + LastDesiredHorizontalSpeed Speed // mm/sec + LastDesiredSpeed Speed // mm/sec } //go:generate stringer -type=Direction @@ -256,8 +280,8 @@ type VehicleMsgLocalizationTransitionUpdate struct { // ACK commands received LastRecvLaneChangeCmdID uint8 LastExecLaneChangeCmdID uint8 - LastDesiredHorizontalSpeed uint16 // mm/sec - LastDesiredSpeed uint16 // mm/sec + LastDesiredHorizontalSpeed Speed // mm/sec + LastDesiredSpeed Speed // mm/sec // track grade detection UphillCounter uint8 diff --git a/protocol2.go b/protocol2.go index 333b871..e578c9c 100644 --- a/protocol2.go +++ b/protocol2.go @@ -1,5 +1,10 @@ package anki +import ( + "fmt" + "time" +) + const ( VehicleMsgV2CSpeedUpdate ID = 0x36 VehicleMsgV2CStatusUpdate ID = 0x3f @@ -15,9 +20,9 @@ const ( type VehicleMsgSpeedUpdate struct { Size uint8 MsgID ID - DesiredSpeed uint16 // mm/sec - Accel uint16 // mm/sec² - CurrentSpeed uint16 // mm/sec + DesiredSpeed Speed // mm/sec + Accel Accel // mm/sec² -- missing on older firmware + CurrentSpeed Speed // mm/sec -- missing on older firmware } // 05 3F 01 00 01 00 @@ -36,8 +41,8 @@ type VehicleMsgLaneChangeUpdate struct { MsgID ID CurrentOffsetFromRoadCenter float32 TargetOffsetFromRoadCenter float32 - HorizontalSpeed uint16 - VerticalSpeed uint16 + HorizontalSpeed Speed + VerticalSpeed Speed LaneChangeID uint8 } @@ -49,13 +54,19 @@ type VehicleMsgCollisionDetected struct { WasFrontBackCollision uint8 } +type UT uint32 + +func (u UT) String() string { + return fmt.Sprintf("%v", time.Duration(u)*time.Microsecond) +} + // 0B 86 11 00 26 08 00 00 13 10 00 00 cchii type VehicleMsgCycleOvertime struct { Size uint8 MsgID ID NumOvertimeCycles uint16 - AverageCycleTime uint32 // µsec - MaxCycleTime uint32 // µsec + AverageCycleTime UT // µsec + MaxCycleTime UT // µsec } // ID(201) | 04 15 03 00 00 -- cgit v1.2.3