added speed sensor
This commit is contained in:
Harald Hoyer
parent
cb7612b32d
commit
75c8241f1b
120
PowerMeterTx.py
Normal file
120
PowerMeterTx.py
Normal file
|
|
@ -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())
|
||||
118
SpeedTx.py
Normal file
118
SpeedTx.py
Normal file
|
|
@ -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())
|
||||
21
const.py
Normal file
21
const.py
Normal file
|
|
@ -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
|
||||
106
iconsole.py
106
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 ant.core.constants import *
|
||||
from ant.core.exceptions import ChannelError
|
||||
from PowerMeterTx import PowerMeterTx
|
||||
from SpeedTx import SpeedTx
|
||||
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_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
|
||||
speed = None
|
||||
|
||||
POWER_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 1
|
||||
|
||||
# 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())
|
||||
SPEED_SENSOR_ID = int(int(hashlib.md5(getserial()).hexdigest(), 16) & 0xfffe) + 2
|
||||
|
||||
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()
|
||||
|
|
|
|||
46
testpower.py
Normal file
46
testpower.py
Normal file
|
|
@ -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()
|
||||
|
||||
46
testspeed.py
Normal file
46
testspeed.py
Normal file
|
|
@ -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()
|
||||
|
||||
Loading…
Reference in a new issue