139 lines
5.2 KiB
Python
139 lines
5.2 KiB
Python
from ant.core import message
|
|
from ant.core.constants import *
|
|
from ant.core.exceptions import ChannelError
|
|
import thread
|
|
# from binascii import hexlify
|
|
import struct
|
|
|
|
CHANNEL_PERIOD = 8182
|
|
|
|
# Transmitter for Rower Power ANT+ sensor
|
|
class RowerTx(object):
|
|
data_lock = thread.allocate_lock()
|
|
|
|
class RowerData:
|
|
def __init__(self):
|
|
self.instantaneousPower = 0
|
|
self.distance = 0
|
|
self.i = 0
|
|
|
|
def __init__(self, antnode, sensor_id):
|
|
self.antnode = antnode
|
|
self.power = 0
|
|
self.sensor_id = sensor_id
|
|
|
|
# Get the channel
|
|
self.channel = antnode.getFreeChannel()
|
|
try:
|
|
self.channel.name = 'C:POWER'
|
|
self.channel.assign('N:ANT+', CHANNEL_TYPE_TWOWAY_TRANSMIT)
|
|
self.channel.setID(0x11, sensor_id & 0xFFFF, 5)
|
|
self.channel.setPeriod(8182)
|
|
self.channel.setFrequency(57)
|
|
except ChannelError as e:
|
|
print "Channel config error: " + e.message
|
|
self.powerData = RowerTx.RowerData()
|
|
self.channel.registerCallback(self)
|
|
|
|
def open(self):
|
|
self.channel.open()
|
|
|
|
def close(self):
|
|
self.channel.close()
|
|
|
|
def unassign(self):
|
|
self.channel.unassign()
|
|
|
|
def update(self, power):
|
|
self.data_lock.acquire()
|
|
self.power = power
|
|
self.data_lock.release()
|
|
|
|
def process(self, msg):
|
|
if isinstance(msg, message.ChannelEventMessage) and \
|
|
msg.getMessageID() == 1 and \
|
|
msg.getMessageCode() == EVENT_TX:
|
|
self.broadcast()
|
|
elif isinstance(msg, message.ChannelAcknowledgedDataMessage):
|
|
payload = msg.getPayload()
|
|
a, page, id_ = struct.unpack('BBB', payload[:3])
|
|
if a == 0 and page == 1 and id_ == 0xAA:
|
|
# print ("ChannelAcknowledgedDataMessage: " + hexlify(payload))
|
|
payload = chr(0x01)
|
|
payload += chr(0xAC)
|
|
payload += chr(0xFF)
|
|
payload += chr(0xFF)
|
|
payload += chr(0xFF)
|
|
payload += chr(0xFF)
|
|
payload += chr(0x00)
|
|
payload += chr(0x00)
|
|
ant_msg = message.ChannelBroadcastDataMessage(self.channel.number, data=payload)
|
|
self.antnode.driver.write(ant_msg.encode())
|
|
else:
|
|
print("Message ID %d Code %d" % (msg.getMessageID(), msg.getMessageCode()))
|
|
|
|
# Power was updated, so send out an ANT+ message
|
|
def broadcast(self):
|
|
self.powerData.i += 1
|
|
|
|
self.data_lock.acquire()
|
|
power = self.power
|
|
self.data_lock.release()
|
|
|
|
if power == 0:
|
|
speed_bytes = 0
|
|
distance_delta = 0
|
|
else:
|
|
pace = pow(2.80/float(power), 1.0/3.0)
|
|
speed = 1.0 / pace
|
|
speed_bytes = int(1000.0 / pace)
|
|
distance_delta = speed / 4.0
|
|
|
|
self.powerData.distance += distance_delta
|
|
|
|
if self.powerData.i % 132 == 64 or self.powerData.i % 132 == 65:
|
|
# page 80
|
|
payload = chr(0x50) # Manufacturer's Info
|
|
payload += chr(0xFF)
|
|
payload += chr(0xFF)
|
|
payload += chr(0x01) # HW Rev
|
|
payload += chr(0xFF) # MID LSB
|
|
payload += chr(0x00) # MID MSB
|
|
payload += chr(0x01) # Model LSB
|
|
payload += chr(0x00) # Model MSB
|
|
elif self.powerData.i % 132 == 130 or self.powerData.i % 132 == 131:
|
|
# page 81
|
|
payload = chr(0x51) # Product Info
|
|
payload += chr(0xFF)
|
|
payload += chr(0xFF) # SW Rev Supp
|
|
payload += chr(0x01) # SW Rev Main
|
|
payload += chr((self.sensor_id >> 0) & 0xFF) # Serial 0-7
|
|
payload += chr((self.sensor_id >> 8) & 0xFF) # Serial 8-15
|
|
payload += chr((self.sensor_id >> 16) & 0xFF) # Serial 16-23
|
|
payload += chr((self.sensor_id >> 24) & 0xFF) # Serial 24-31
|
|
elif self.powerData.i % 4 == 0 or self.powerData.i % 4 == 1:
|
|
# page 16
|
|
payload = chr(0x10) # standard fitness equipment page
|
|
payload += chr(22) # equipment type field / rower
|
|
payload += chr(self.powerData.i % 0xFF) # Elapsed Time
|
|
payload += chr(int(self.powerData.distance) % 0xFF) # Distance travelled accumulated
|
|
payload += chr(speed_bytes % 0xFF) # Speed LSB
|
|
payload += chr(speed_bytes >> 8) # Speed MSB
|
|
payload += chr(0xFF) # Heart rate
|
|
payload += chr((1<<2) | (1<<3)) # capabilities / FE state (transmit distance | virtual speed)
|
|
elif self.powerData.i % 4 == 2 or self.powerData.i % 4 == 3:
|
|
# page 22
|
|
self.powerData.instantaneousPower = int(power)
|
|
|
|
payload = chr(0x16) # specific rower data
|
|
payload += chr(0xFF)
|
|
payload += chr(0xFF)
|
|
payload += chr(0) # stroke count
|
|
payload += chr(0xFF) # stroke cadence
|
|
payload += chr(self.powerData.instantaneousPower & 0xff)
|
|
payload += chr(self.powerData.instantaneousPower >> 8)
|
|
payload += chr(0) # capabilities / FE state
|
|
|
|
ant_msg = message.ChannelBroadcastDataMessage(self.channel.number, data=payload)
|
|
self.antnode.driver.write(ant_msg.encode())
|