harald/iconsole
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

retransmit as ANT+

Browse source
This commit is contained in:
Harald Hoyer 2017-04-19 21:12:16 +02:00
parent 29f2acebf7
commit 410e199723
3 changed files with 212 additions and 53 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

68
PowerMeterTx.py Normal file
View file

@ -0,0 +1,68 @@
import sys
from ant.core import message
from ant.core.constants import *
from ant.core.exceptions import ChannelError
from constants import *
VPOWER_DEBUG = True
CHANNEL_PERIOD = 8182
# Transmitter for Bicycle Power ANT+ sensor
class PowerMeterTx(object):
class PowerData:
def __init__(self):
self.eventCount = 0
self.eventTime = 0
self.cumulativePower = 0
self.instantaneousPower = 0
def __init__(self, antnode, sensor_id):
self.antnode = antnode
# 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(POWER_DEVICE_TYPE, sensor_id, 0)
self.channel.setPeriod(8182)
self.channel.setFrequency(57)
except ChannelError as e:
print "Channel config error: "+e.message
self.powerData = PowerMeterTx.PowerData()
def open(self):
self.channel.open()
def close(self):
self.channel.close()
def unassign(self):
self.channel.unassign()
# Power was updated, so send out an ANT+ message
def update(self, power, cadence):
if VPOWER_DEBUG: print 'PowerMeterTx: update called with power ', power
self.powerData.eventCount = (self.powerData.eventCount + 1) & 0xff
if VPOWER_DEBUG: print 'eventCount ', self.powerData.eventCount
self.powerData.cumulativePower = (self.powerData.cumulativePower + int(power)) & 0xffff
if VPOWER_DEBUG: print 'cumulativePower ', self.powerData.cumulativePower
self.powerData.instantaneousPower = int(power)
if VPOWER_DEBUG: print 'instantaneousPower ', self.powerData.instantaneousPower
payload = chr(0x10) # standard power-only message
payload += chr(self.powerData.eventCount)
payload += chr(0xFF) # Pedal power not used
payload += chr(cadence)
payload += chr(self.powerData.cumulativePower & 0xff)
payload += chr(self.powerData.cumulativePower >> 8)
payload += chr(self.powerData.instantaneousPower & 0xff)
payload += chr(self.powerData.instantaneousPower >> 8)
ant_msg = message.ChannelBroadcastDataMessage(self.channel.number, data=payload)
sys.stdout.write('+')
sys.stdout.flush()
if VPOWER_DEBUG: print 'Write message to ANT stick on channel ' + repr(self.channel.number)
self.antnode.driver.write(ant_msg.encode())

20
constants.py Normal file
View file

@ -0,0 +1,20 @@
SPEED_DEVICE_TYPE = 0x7B
CADENCE_DEVICE_TYPE = 0x7A
SPEED_CADENCE_DEVICE_TYPE = 0x79
POWER_DEVICE_TYPE = 0x0B
# Get the serial number of Raspberry Pi
def getserial():
# Extract serial from cpuinfo file
cpuserial = "0000000000000000"
try:
f = open('/proc/cpuinfo', 'r')
for line in f:
if line[0:6] == 'Serial':
cpuserial = line[10:26]
f.close()
except:
cpuserial = "ERROR000000000"
return cpuserial

177
iconsole.py
View file

@ -40,9 +40,13 @@
# 357.7 83
# 364.6 84 6.9 pro rpm
import serial, struct, sys
import serial, struct, sys, hashlib
from time import sleep
from binascii import hexlify
from ant.core import driver
from ant.core import node
from PowerMeterTx import PowerMeterTx
from constants import *
INIT_A0 = struct.pack('BBBBB', 0xf0, 0xa0, 0x02, 0x02, 0x94)
PING = struct.pack('BBBBB', 0xf0, 0xa0, 0x01, 0x01, 0x92)
@ -53,10 +57,13 @@ INIT_A4 = struct.pack('BBBBBBBBBBBBBBB', 0xf0, 0xa4, 0x01, 0x01, 0x01, 0x01, 0x0
START = struct.pack('BBBBBB', 0xf0, 0xa5, 0x01, 0x01, 0x02, 0x99)
STOP = struct.pack('BBBBBB', 0xf0, 0xa5, 0x01, 0x01, 0x04, 0x9b)
READ = struct.pack('BBBBB', 0xf0, 0xa2, 0x01, 0x01, 0x94)
POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 1
DEBUG = False
LOG = None
NETKEY = '\xB9\xA5\x21\xFB\xBD\x72\xC3\x45'
port = serial.Serial('/dev/rfcomm3')
print "OK"
import signal
port = serial.Serial('/dev/rfcomm0')
class GracefulInterruptHandler(object):
@ -130,9 +137,9 @@ def send_ack(sig, packet, expect=None, plen=0):
print "---> Retransmit"
return got
def send_level(lvl):
def send_level(sig, lvl):
packet = struct.pack('BBBBBB', 0xf0, 0xa6, 0x01, 0x01, lvl+1, (0xf0+0xa6+3+lvl) & 0xFF)
got = send_ack(packet)
got = send_ack(sig, packet)
return got
def exit_all(sig):
@ -144,63 +151,127 @@ def exit_all(sig):
send_ack(sig, PING)
print "ping done"
port.close()
if power_meter:
print "Closing power meter"
power_meter.close()
power_meter.unassign()
if antnode:
print "Stopping ANT node"
antnode.stop()
sys.exit(0)
power_meter = None
antnode = None
#send_level(10)
def main(win):
win.nodelay(True)
win.refresh()
stick = driver.USB1Driver(device="/dev/ttyUSB0", log=LOG, debug=DEBUG)
antnode = node.Node(stick)
print "Starting ANT node"
antnode.start()
key = node.NetworkKey('N:ANT+', NETKEY)
antnode.setNetworkKey(0, key)
i = 0
with GracefulInterruptHandler() as sig:
send_ack(sig, PING)
print "ping done"
print "Starting power meter with ANT+ ID " + repr(POWER_SENSOR_ID)
try:
# Create the power meter object and open it
power_meter = PowerMeterTx(antnode, POWER_SENSOR_ID)
power_meter.open()
except Exception as e:
print "power_meter error: " + e.message
power_meter = None
send_ack(sig, INIT_A0, expect=0xb7, plen=6)
print "A0 done"
print "OK"
i = 0
with GracefulInterruptHandler() as sig:
send_ack(sig, PING)
print "ping done"
send_ack(sig, INIT_A0, expect=0xb7, plen=6)
print "A0 done"
for i in range(0, 5):
send_ack(sig, PING)
print "ping done"
send_ack(sig, STATUS, plen=6)
print "status done"
send_ack(sig, PING)
print "ping done"
send_ack(sig, INIT_A3)
print "A3 done"
send_ack(sig, INIT_A4)
print "A4 done"
send_ack(sig, START)
print "START done"
level = 1
while True:
if sig.interrupted:
exit_all(sig)
sleep(0.3)
try:
key = win.getch()
if key == 'q':
break
if key == 'a':
level += 1
print "Level: %d" % level
send_level(sig, level)
if key == 'y':
level -= 1
print "Level: %d" % level
send_level(sig, level)
except Exception as e:
pass
#i+=1
# if i % 20 == 2:
# send_level((i/20) +1)
got = send_ack(sig, READ, plen=21)
if len(got) == 21:
gota = struct.unpack('BBBBBBBBBBBBBBBBBBBBB', got)
time = "%02d:%02d:%02d:%02d" % (gota[2]-1, gota[3]-1, gota[4]-1, gota[5]-1)
speed = "V: % 3.1f km/h" % ((100*(gota[6]-1) + gota[7] -1) / 10.0)
rpm = "% 3d RPM" % ((100*(gota[8]-1) + gota[9] -1))
distance = "D: % 3.1f km" % ((100*(gota[10]-1) + gota[11] -1) / 10.0)
calories = "% 3d kcal" % ((100*(gota[12]-1) + gota[13] -1))
hf = "HF % 3d" % ((100*(gota[14]-1) + gota[15] -1))
watt = "% 3.1f W" % ((100*(gota[16]-1) + gota[17] -1) / 10.0)
lvl = "L: %d" % (gota[18] -1)
print "%s - %s - %s - %s - %s - %s - %s - %s" % (time, speed, rpm, distance, calories, hf, watt, lvl)
power_meter.update(power = ((100*(gota[16]-1) + gota[17] -1) / 10.0),
cadence = ((100*(gota[8]-1) + gota[9] -1)))
send_ack(sig, STOP)
print "STOP done"
for i in range(0, 5):
send_ack(sig, PING)
print "ping done"
send_ack(sig, STATUS, plen=6)
print "status done"
port.close()
send_ack(sig, PING)
print "ping done"
if power_meter:
print "Closing power meter"
power_meter.close()
power_meter.unassign()
if antnode:
print "Stopping ANT node"
antnode.stop()
send_ack(sig, INIT_A3)
print "A3 done"
send_ack(sig, INIT_A4)
print "A4 done"
send_ack(sig, START)
print "START done"
while True:
if sig.interrupted:
exit_all(sig)
sleep(0.3)
i+=1
# if i % 20 == 2:
# send_level((i/20) +1)
got = send_ack(sig, READ, plen=21)
if len(got) == 21:
gota = struct.unpack('BBBBBBBBBBBBBBBBBBBBB', got)
time = "%02d:%02d:%02d:%02d" % (gota[2]-1, gota[3]-1, gota[4]-1, gota[5]-1)
speed = "V: % 3.1f km/h" % ((100*(gota[6]-1) + gota[7] -1) / 10.0)
rpm = "% 3d RPM" % ((100*(gota[8]-1) + gota[9] -1))
distance = "D: % 3.1f km" % ((100*(gota[10]-1) + gota[11] -1) / 10.0)
calories = "% 3d kcal" % ((100*(gota[12]-1) + gota[13] -1))
hf = "HF % 3d" % ((100*(gota[14]-1) + gota[15] -1))
watt = "% 3.1f W" % ((100*(gota[16]-1) + gota[17] -1) / 10.0)
lvl = "L: %d" % (gota[18] -1)
print "%s - %s - %s - %s - %s - %s - %s - %s" % (time, speed, rpm, distance, calories, hf, watt, lvl)
send_ack(STOP)
print "STOP done"
for i in range(0, 5):
send_ack(PING)
print "ping done"
port.close()
import curses
curses.wrapper(main)