Erik Brakkee
19e4e78c75
Prefix log output with 'ant:', 'bt:', or 'usb:' to distinguish between ANT+, Bluetooth LE, and USB device messages.
87 lines
2.0 KiB
Go
87 lines
2.0 KiB
Go
package main
|
|
|
|
import (
|
|
"context"
|
|
"io"
|
|
|
|
"github.com/google/gousb"
|
|
"github.com/half2me/antgo/ant"
|
|
"github.com/half2me/antgo/device"
|
|
"github.com/half2me/antgo/driver/usb"
|
|
)
|
|
|
|
const (
|
|
antDataPageMask = 0x7F // Mask for data page number (7 bits, clears toggle bit)
|
|
antDataPageCyclingPower = 0x10 // Data page 16 = Cycling Power Feature page
|
|
)
|
|
|
|
func resetAndWait(drv io.ReadWriter) error {
|
|
if _, err := drv.Write(ant.SystemResetMessage()); err != nil {
|
|
return err
|
|
}
|
|
// drain until we see the startup message (0x6F)
|
|
for {
|
|
msg, err := ant.ReadMsg(drv)
|
|
if err != nil {
|
|
return err
|
|
}
|
|
if msg.Class() == ant.MESSAGE_STARTUP {
|
|
return nil
|
|
}
|
|
}
|
|
}
|
|
|
|
func ListenForTrainer(vendorId uint16, productId uint16, deviceNumber uint32,
|
|
events chan<- Event) {
|
|
defer close(events)
|
|
|
|
drv, err := usb.GetDevice(gousb.ID(vendorId), gousb.ID(productId))
|
|
if err != nil {
|
|
panic(err)
|
|
}
|
|
defer drv.Close()
|
|
|
|
logMsg("ant: device opened")
|
|
|
|
if err := resetAndWait(drv); err != nil {
|
|
panic(err)
|
|
}
|
|
if err := device.StartRxScanModeWait(drv); err != nil {
|
|
panic(err)
|
|
}
|
|
logMsg("ant: scan mode started")
|
|
|
|
msgs := make(chan ant.BroadcastMessage, 100)
|
|
go device.DumpBroadcastMessages(context.Background(), drv, msgs)
|
|
|
|
logMsg("ant: listening...")
|
|
for msg := range msgs {
|
|
if msg.DeviceNumber() != deviceNumber {
|
|
logPrintf("Received msg from ignored ant+ device %v\n", msg.DeviceNumber())
|
|
continue
|
|
}
|
|
//fmt.Printf("raw msg: device type=0x%02x device number=%d content=%x\n",
|
|
// msg.DeviceType(), msg.DeviceNumber(), msg.Content())
|
|
|
|
switch msg.DeviceType() {
|
|
case ant.DEVICE_TYPE_POWER:
|
|
// Check data page first
|
|
page := msg[4] & antDataPageMask
|
|
if page != antDataPageCyclingPower {
|
|
continue
|
|
}
|
|
cur := ant.PowerMessage(msg)
|
|
logPrintf("ant: %v: power: %dW cadence: %d rpm\n",
|
|
msg.DeviceNumber(), cur.InstantaneousPower(), cur.InstantaneousCadence())
|
|
select {
|
|
case events <- Event{
|
|
Power: cur.InstantaneousPower(),
|
|
Cadence: cur.InstantaneousCadence(),
|
|
}:
|
|
default:
|
|
logPrintf("ant: dropped event, buffer full\n")
|
|
}
|
|
}
|
|
}
|
|
}
|