diff --git a/PowerMeterTx.py b/PowerMeterTx.py index 5678997..a400ac9 100644 --- a/PowerMeterTx.py +++ b/PowerMeterTx.py @@ -25,13 +25,14 @@ class PowerMeterTx(object): self.antnode = antnode self.power = 0 self.cadence = 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(POWER_DEVICE_TYPE, sensor_id, 0) + self.channel.setID(POWER_DEVICE_TYPE, sensor_id & 0xFFFF, 5) self.channel.setPeriod(8182) self.channel.setFrequency(57) except ChannelError as e: @@ -80,7 +81,7 @@ class PowerMeterTx(object): # Power was updated, so send out an ANT+ message def broadcast(self): self.powerData.i += 1 - if self.powerData.i % 121 == 30: + if self.powerData.i % 61 == 15: payload = chr(0x50) # Manufacturer's Info payload += chr(0xFF) payload += chr(0xFF) @@ -90,23 +91,25 @@ class PowerMeterTx(object): payload += chr(0x01) # Model LSB payload += chr(0x00) # Model MSB - elif self.powerData.i % 121 == 60: + elif self.powerData.i % 61 == 30: payload = chr(0x51) # Product Info payload += chr(0xFF) payload += chr(0xFF) # SW Rev Supp payload += chr(0x01) # SW Rev Main - payload += chr(0xFF) # Serial 0-7 - payload += chr(0xFF) - payload += chr(0xFF) - payload += chr(0xFF) # Serial 24-31 + 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 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 diff --git a/SpeedTx.py b/SpeedTx.py index 2354567..182e890 100644 --- a/SpeedTx.py +++ b/SpeedTx.py @@ -20,7 +20,7 @@ class SpeedTx(object): self.ucMessageCount = 0 self.ulRunTime = 0 self.ucPageChange = 0 - self.ucExtMesgType = 0 + self.ucExtMesgType = 1 def __init__(self, antnode, sensor_id, wheel = 0.100): self.antnode = antnode @@ -28,12 +28,13 @@ class SpeedTx(object): self.lastTime = 0 self.wheel = wheel self.remWay = 0 + self.sensor_id = sensor_id # 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.setID(SPEED_DEVICE_TYPE, sensor_id & 0xFFFF, 1) self.channel.setPeriod(8118) self.channel.setFrequency(57) except ChannelError as e: @@ -64,8 +65,8 @@ class SpeedTx(object): self.broadcast() def broadcast(self): - now = time.time() self.data_lock.acquire() + now = time.time() if self.lastTime != 0: way = self.speed * (now - self.lastTime) / 3.6 + self.remWay rev = int( way / self.wheel + 0.5 ) @@ -73,34 +74,36 @@ class SpeedTx(object): 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) + ulElapsedTime2 = int(now/2.0) + payload = chr(0x01 | (self.data.ucPageChange & 0x80)) + payload += chr((ulElapsedTime2) & 0xFF) 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) + payload = chr(0x02 | (self.data.ucPageChange & 0x80)) + payload += chr(0xFF) # MID + payload += chr((self.sensor_id >> 16)& 0xFF) # Serial 17-24 + payload += chr((self.sensor_id >> 24) & 0xFF) # Serial 25-32 elif self.data.ucExtMesgType == 3: - payload = chr(0x03) - payload += chr(0x01) - payload += chr(0x01) - payload += chr(0x01) + payload = chr(0x03 | (self.data.ucPageChange & 0x80)) + payload += chr(0x01) # HW + payload += chr(0x01) # SW + payload += chr(0x01) # Model + + if self.data.ucMessageCount >= 68: + self.data.ucMessageCount = 0 + self.data.ucExtMesgType += 1 else: payload = chr(self.data.ucPageChange & 0x80) payload += chr(0xFF) diff --git a/iConst.py b/iConst.py index fa3cfbb..386bfd7 100644 --- a/iConst.py +++ b/iConst.py @@ -1,20 +1,17 @@ CADENCE_DEVICE_TYPE = 0x7A +SPEED2_DEVICE_TYPE = 0x0F 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" + machineid = "0000000000000000" try: - f = open('/proc/cpuinfo', 'r') - for line in f: - if line[0:6] == 'Serial': - cpuserial = line[10:26] + f = open('/etc/machine-id', 'r') + machineid = f.readline() f.close() except: - cpuserial = "ERROR000000000" + machineid = "ERROR000000000" - return cpuserial + return machineid diff --git a/testpower.py b/testpower.py index 81ede05..7345802 100644 --- a/testpower.py +++ b/testpower.py @@ -9,7 +9,7 @@ from iConst import * power_meter = None -POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 1 +POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xFFFFfffe) + 1 if __name__ =='__main__': NETKEY = unhexlify(sys.argv[1]) diff --git a/testpowerspeed.py b/testpowerspeed.py new file mode 100644 index 0000000..668ca87 --- /dev/null +++ b/testpowerspeed.py @@ -0,0 +1,61 @@ +import serial, struct, sys, hashlib, curses +from time import sleep +from binascii import hexlify,unhexlify +from ant.core import driver +from ant.core import node +from PowerMeterTx import PowerMeterTx +from SpeedTx import SpeedTx +from iConst import * + +power_meter = None +POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xFFFFfffe) + 1 +speed = None +SPEED_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xFFFFfffe) + 2 + +if __name__ =='__main__': + NETKEY = unhexlify(sys.argv[1]) + stick = driver.USB1Driver(device="/dev/ttyANT", log=None, debug=True) + antnode = node.Node(stick) + print("Starting ANT node on network %s" % sys.argv[1]) + 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("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) + power_meter.update(power = i, cadence = i) + speed.update(speed = 25) + i += 1 + if (i > 200): + break + + if power_meter: + print "Closing power meter" + power_meter.close() + power_meter.unassign() + if speed: + print "Closing speed sensor" + speed.close() + speed.unassign() + if antnode: + print "Stopping ANT node" + antnode.stop() + diff --git a/testspeed.py b/testspeed.py index 7693cda..3479def 100644 --- a/testspeed.py +++ b/testspeed.py @@ -8,8 +8,7 @@ from SpeedTx import SpeedTx from iConst import * speed = None - -SPEED_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 2 +SPEED_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xFFFFfffe) + 2 if __name__ =='__main__': NETKEY = unhexlify(sys.argv[1])