retransmit as ANT+

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())

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

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,12 +151,43 @@ 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)
 
-#send_level(10)
+power_meter = None
+antnode = None
 
-i = 0
-with GracefulInterruptHandler() as sig:
+#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)
+
+    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
+
+    print "OK"
+    i = 0
+    with GracefulInterruptHandler() as sig:
         send_ack(sig, PING)
         print "ping done"
 
@@ -175,11 +213,32 @@ with GracefulInterruptHandler() as sig:
         send_ack(sig, START)
         print "START done"
 
+        level = 1
+
         while True:
+
             if sig.interrupted:
                 exit_all(sig)
+
             sleep(0.3)
-        i+=1
+            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)
 
@@ -195,12 +254,24 @@ with GracefulInterruptHandler() as sig:
                 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"
 
-send_ack(STOP)
-print "STOP done"
-
-for i in range(0, 5):
-    send_ack(PING)
+    for i in range(0, 5):
+        send_ack(sig, PING)
         print "ping done"
 
-port.close()
+    port.close()
+
+    if power_meter:
+        print "Closing power meter"
+        power_meter.close()
+        power_meter.unassign()
+    if antnode:
+        print "Stopping ANT node"
+        antnode.stop()
+
+import curses
+curses.wrapper(main)