added speed sensor

PowerMeterTx.py

@@ -0,0 +1,120 @@
+from ant.core import message
+from ant.core.constants import *
+from ant.core.exceptions import ChannelError
+from const import *
+import thread
+from binascii import hexlify
+import struct
+
+VPOWER_DEBUG = False
+CHANNEL_PERIOD = 8182
+
+# Transmitter for Bicycle Power ANT+ sensor
+class PowerMeterTx(object):
+    data_lock = thread.allocate_lock()
+
+    class PowerData:
+        def __init__(self):
+            self.eventCount = 0
+            self.eventTime = 0
+            self.cumulativePower = 0
+            self.instantaneousPower = 0
+            self.i = 0
+
+    def __init__(self, antnode, sensor_id):
+        self.antnode = antnode
+        self.power = 0
+        self.cadence = 0
+
+        # 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()
+        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, cadence):
+        self.data_lock.acquire()
+        self.power = power
+        self.cadence = cadence
+        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
+        if self.powerData.i % 121 == 30:
+            payload = chr(0x50)  # Manufacturer's Info
+            payload += chr(0xFF)
+            payload += chr(0xFF)
+            payload += chr(0x01) # HW Rev
+            payload += chr(0xFF)
+            payload += chr(0x00)
+            payload += chr(0x01)
+            payload += chr(0x00)
+
+        elif self.powerData.i % 121 == 60:
+            payload = chr(0x51)  # Product Info
+            payload += chr(0xFF)
+            payload += chr(0xFF) # SW Rev Supp
+            payload += chr(0x01) # SW Rev Main
+            payload += chr(0xFF)
+            payload += chr(0xFF)
+            payload += chr(0xFF)
+            payload += chr(0xFF)
+        else:
+            self.data_lock.acquire()
+            power = self.power
+            cadence = self.cadence
+            self.data_lock.release()
+            self.powerData.eventCount = (self.powerData.eventCount + 1) & 0xff
+            self.powerData.cumulativePower = (self.powerData.cumulativePower + int(power)) & 0xffff
+            self.powerData.instantaneousPower = int(power)
+            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)
+        self.antnode.driver.write(ant_msg.encode())

SpeedTx.py

@@ -0,0 +1,118 @@
+from ant.core import message
+from ant.core.constants import *
+from ant.core.exceptions import ChannelError
+from const import *
+import thread
+from binascii import hexlify
+import struct
+import time
+
+SPEED_DEBUG = False
+CHANNEL_PERIOD = 8182
+
+# Transmitter for Bicycle Speed ANT+ sensor
+class SpeedTx(object):
+    data_lock = thread.allocate_lock()
+
+    class SpeedData:
+        def __init__(self):
+            self.revCounts = 0
+            self.ucMessageCount = 0
+            self.ulRunTime = 0
+            self.ucPageChange = 0
+            self.ucExtMesgType = 0
+
+    def __init__(self, antnode, sensor_id, wheel = 0.100):
+        self.antnode = antnode
+        self.speed = 0
+        self.lastTime = 0
+        self.wheel = wheel
+        self.remWay = 0
+        # Get the channel
+        self.channel = antnode.getFreeChannel()
+        try:
+            self.channel.name = 'C:SPEED'
+            self.channel.assign('N:ANT+', CHANNEL_TYPE_TWOWAY_TRANSMIT)
+            self.channel.setID(SPEED_DEVICE_TYPE, sensor_id, 0)
+            self.channel.setPeriod(8118)
+            self.channel.setFrequency(57)
+        except ChannelError as e:
+            print "Channel config error: "+e.message
+        self.data = SpeedTx.SpeedData()
+        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, speed):
+        self.data_lock.acquire()
+        self.speed = speed
+        if self.lastTime == 0:
+            self.lastTime = time.time()
+        self.data_lock.release()
+
+    def process(self, msg):
+        if isinstance(msg, message.ChannelEventMessage) and \
+           msg.getMessageID() == 1 and \
+           msg.getMessageCode() == EVENT_TX:
+            self.broadcast()
+
+    def broadcast(self):
+        now = time.time()
+        self.data_lock.acquire()
+        if self.lastTime != 0:
+            way = self.speed * (now - self.lastTime) / 3.6 + self.remWay
+            rev = int( way / self.wheel )
+            self.remWay = way - rev * self.wheel
+            self.data.revCounts += rev
+        self.lastTime = now
+        self.data_lock.release()
+        #print "Rev: %d Way: %f" % (rev, way)
+
+        self.data.ucPageChange += 0x20;
+        self.data.ucPageChange &= 0xF0;
+
+        self.data.ucMessageCount += 1
+        if self.data.ucMessageCount >= 65:
+            self.data.ucMessageCount = 0
+            self.data.ucExtMesgType += 1
+            if self.data.ucExtMesgType >= 4:
+                self.data.ucExtMesgType = 1
+
+            if self.data.ucExtMesgType == 1:
+                ulElapsedTime2 = int(now/2)
+                payload = chr(0x01)
+                payload += chr((ulElapsedTime2 >> 8) & 0xFF)
+                payload += chr((ulElapsedTime2 >> 16) & 0xFF)
+                payload += chr((ulElapsedTime2 >> 24) & 0xFF)
+            elif self.data.ucExtMesgType == 2:
+                payload = chr(0x02)
+                payload += chr(0x02)
+                payload += chr(0xFE)
+                payload += chr(0x21)
+            elif self.data.ucExtMesgType == 3:
+                payload = chr(0x03)
+                payload += chr(0x01)
+                payload += chr(0x01)
+                payload += chr(0x01)
+        else:
+            payload = chr(self.data.ucPageChange & 0x80)
+            payload += chr(0xFF)
+            payload += chr(0xFF)
+            payload += chr(0xFF)
+
+        usTime1024 = int(now * 1024)
+        payload += chr(usTime1024 & 0xff)
+        payload += chr((usTime1024 >> 8) & 0xff)
+        payload += chr(self.data.revCounts & 0xff)
+        payload += chr((self.data.revCounts >> 8) & 0xff)
+
+        #print "Broadcast: %s" % hexlify(payload)
+        ant_msg = message.ChannelBroadcastDataMessage(self.channel.number, data=payload)
+        self.antnode.driver.write(ant_msg.encode())

const.py

@@ -0,0 +1,21 @@
+NETKEY = '\xB9\xA5\x21\xFB\xBD\x72\xC3\x45'
+CADENCE_DEVICE_TYPE = 0x7A
+SPEED_DEVICE_TYPE = 0x7B
+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

@@ -46,9 +46,9 @@ from binascii import hexlify
 from ant.core import driver
 from ant.core import node
 from bluetooth import *
-from ant.core import message
+from PowerMeterTx import PowerMeterTx
-from ant.core.constants import *
+from SpeedTx import SpeedTx
-from ant.core.exceptions import ChannelError
+from const import *
 
 INIT_A0 = struct.pack('BBBBB', 0xf0, 0xa0, 0x02, 0x02, 0x94)
 PING = struct.pack('BBBBB', 0xf0, 0xa0, 0x01, 0x01, 0x92)
@@ -61,91 +61,11 @@ STOP = struct.pack('BBBBBB', 0xf0, 0xa5, 0x01, 0x01, 0x04, 0x9b)
 READ = struct.pack('BBBBB', 0xf0, 0xa2, 0x01, 0x01, 0x94)
 DEBUG = False
 LOG = None
-NETKEY = '\xB9\xA5\x21\xFB\xBD\x72\xC3\x45'
 power_meter = None
-
+speed = None
-SPEED_DEVICE_TYPE = 0x7B
-CADENCE_DEVICE_TYPE = 0x7A
-SPEED_CADENCE_DEVICE_TYPE = 0x79
-POWER_DEVICE_TYPE = 0x0B
-
-VPOWER_DEBUG = False
-CHANNEL_PERIOD = 8182
-
-# 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
 
 POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 1
-
+SPEED_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 2
-# 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())
 
 class IConsole(object):
     def __init__(self, got):
@@ -299,6 +219,7 @@ def main(win):
         if len(got) == 21:
             ic = IConsole(got)
             power_meter.update(power = ic.power, cadence = ic.rpm)
+            speed.update(ic.speed)
             win.addstr(0,0, "%s - %s - %s - %s - %s - %s - %s - %s" % (ic.time_str,
                                                              ic.speed_str,
                                                              ic.rpm_str,
@@ -311,7 +232,6 @@ def main(win):
             win.refresh()
 
 if  __name__ =='__main__':
-    sock = btcon()
     stick = driver.USB1Driver(device="/dev/ttyANT", log=LOG, debug=DEBUG)
     antnode = node.Node(stick)
     print("Starting ANT node")
@@ -328,6 +248,16 @@ if  __name__ =='__main__':
         print("power_meter error: " + e.message)
         power_meter = None
 
+    print("Starting speed sensor with ANT+ ID " + repr(SPEED_SENSOR_ID))
+    try:
+        speed = SpeedTx(antnode, SPEED_SENSOR_ID, wheel = 0.1)
+        speed.open()
+    except Exception as e:
+        print("speed error: " + e.message)
+        speed = None
+
+    sock = btcon()
+
     curses.wrapper(main)
 
     if sock:
@@ -335,6 +265,10 @@ if  __name__ =='__main__':
         send_ack(PING)
         sock.close()
 
+    if speed:
+        print "Closing speed sensor"
+        speed.close()
+        speed.unassign()
     if power_meter:
         print "Closing power meter"
         power_meter.close()

testpower.py

@@ -0,0 +1,46 @@
+import serial, struct, sys, hashlib, curses
+from time import sleep
+from binascii import hexlify
+from ant.core import driver
+from ant.core import node
+from bluetooth import *
+from PowerMeterTx import PowerMeterTx
+from const import *
+
+power_meter = None
+
+POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 1
+
+if  __name__ =='__main__':
+    stick = driver.USB1Driver(device="/dev/ttyANT", log=None, debug=True)
+    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
+
+    i = 0
+    while True:
+        sleep(1)
+        power_meter.update(power = i, cadence = i)
+        i += 1
+        if (i > 200):
+            break
+
+    if power_meter:
+        print "Closing power meter"
+        power_meter.close()
+        power_meter.unassign()
+    if antnode:
+        print "Stopping ANT node"
+        antnode.stop()
+

testspeed.py

@@ -0,0 +1,46 @@
+import serial, struct, sys, hashlib, curses
+from time import sleep
+from binascii import hexlify
+from ant.core import driver
+from ant.core import node
+from bluetooth import *
+from SpeedTx import SpeedTx
+from const import *
+
+speed = None
+
+SPEED_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 2
+
+if  __name__ =='__main__':
+    stick = driver.USB1Driver(device="/dev/ttyANT", log=None, debug=True)
+    antnode = node.Node(stick)
+    print("Starting ANT node")
+    antnode.start()
+    key = node.NetworkKey('N:ANT+', NETKEY)
+    antnode.setNetworkKey(0, key)
+
+    print("Starting speed sensor with ANT+ ID " + repr(SPEED_SENSOR_ID))
+    try:
+        speed = SpeedTx(antnode, SPEED_SENSOR_ID, wheel = 0.1)
+        speed.open()
+    except Exception as e:
+        print("speed error: " + e.message)
+        speed = None
+
+    i = 0
+    while True:
+        sleep(1)
+        speed.update(speed = 25)
+        #print("Speed: %s" % i)
+        i += 1
+        if (i > 200):
+            break
+
+    if speed:
+        print "Closing speed sensor"
+        speed.close()
+        speed.unassign()
+    if antnode:
+        print "Stopping ANT node"
+        antnode.stop()
+