diff --git a/PowerMeterTx.py b/PowerMeterTx.py new file mode 100644 index 0000000..61c37d3 --- /dev/null +++ b/PowerMeterTx.py @@ -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()) diff --git a/constants.py b/constants.py new file mode 100644 index 0000000..f4a344f --- /dev/null +++ b/constants.py @@ -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 diff --git a/iconsole.py b/iconsole.py index f908064..883dfa3 100644 --- a/iconsole.py +++ b/iconsole.py @@ -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)