99 lines
2.3 KiB
Go
99 lines
2.3 KiB
Go
package main
|
|
|
|
import (
|
|
"context"
|
|
"io"
|
|
"time"
|
|
|
|
"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) {
|
|
|
|
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)
|
|
ctx, cancel := context.WithCancel(context.Background())
|
|
defer cancel()
|
|
go func() {
|
|
device.DumpBroadcastMessages(ctx, drv, msgs)
|
|
logMsg("ant: dump broadcast messages ended")
|
|
}()
|
|
|
|
logMsg("ant: listening...")
|
|
for {
|
|
select {
|
|
case msg := <-msgs:
|
|
if deviceNumber != 0 && msg.DeviceNumber() != deviceNumber {
|
|
logPrintf("ant: received msg from ignored 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(),
|
|
Now: time.Now(),
|
|
}:
|
|
default:
|
|
logPrintf("ant: dropped event, buffer full\n")
|
|
}
|
|
}
|
|
case <-time.After(10 * time.Second):
|
|
logMsg("ant: no more messages arrived, restarting")
|
|
return
|
|
}
|
|
}
|
|
}
|