def BroadcastHeartrateMessage(devAntDongle, HeartRate): global EventCounter, HeartBeatCounter, HeartBeatEventTime, HeartBeatTime, PageChangeToggle #--------------------------------------------------------------------------- # Check if heart beat has occurred as tacx only reports # instantaneous heart rate data # Last heart beat is at HeartBeatEventTime # If now - HeartBeatEventTime > time taken for hr to occur, trigger beat. # # We pass here every 250ms. # If one heart_beat occurred, increment counter and time. # Ignore that multiple heart-beats could have occurred; increment # with one beat per cycle only. # # Page 0 is the main page and transmitted most often # In every set of 64 data-pages, page 2 and 3 must be transmitted 4 # times. # To make this fit in the EventCounter cycle (0...255) I have # chosen blocks of 64 messages as below: #------------------------------------------------------------------------- if (time.time() - HeartBeatTime) >= (60 / float(HeartRate)): HeartBeatCounter += 1 # Increment heart beat count HeartBeatEventTime += (60 / float(HeartRate) ) # Reset last time of heart beat HeartBeatTime = time.time() # Current time for next processing if HeartBeatEventTime >= 64 or HeartBeatCounter >= 256: # Rollover at 64seconds HeartBeatCounter = 0 HeartBeatEventTime = 0 HeartBeatTime = 0 if EventCounter % 4 == 0: PageChangeToggle ^= 0x80 # toggle bit every 4 counts if EventCounter % 64 <= 55: # Transmit 56 times Page 0 = Main data page DataPageNumber = 0 Spec1 = 0xff # Reserved Spec2 = 0xff # Reserved Spec3 = 0xff # Reserved comment = "(HR data p0)" elif EventCounter % 64 <= 59: # Transmit 4 times (56, 57, 58, 59) Page 2 = Manufacturer info DataPageNumber = 2 Spec1 = ant.Manufacturer_garmin Spec2 = (ant.SerialNumber_HRM & 0x00ff) # Serial Number LSB Spec3 = (ant.SerialNumber_HRM & 0xff00) >> 8 # Serial Number MSB # 1959-07-05 comment = "(HR data p2)" elif EventCounter % 64 <= 63: # Transmit 4 times (60, 61, 62, 63) Page 3 = Product information DataPageNumber = 3 Spec1 = ant.HWrevision_HRM Spec2 = ant.SWversion_HRM Spec3 = ant.ModelNumber_HRM comment = "(HR data p3)" info = ant.msgPage_Hrm(ant.channel_HRM, PageChangeToggle | DataPageNumber, Spec1, Spec2, Spec3, HeartBeatEventTime, HeartBeatCounter, HeartRate) hrdata = ant.ComposeMessage(ant.msgID_BroadcastData, info) #------------------------------------------------------------------------- # Prepare for next event #------------------------------------------------------------------------- EventCounter += 1 # Increment and ... EventCounter &= 0xff # maximize to 255 #------------------------------------------------------------------------- # Return message to be sent #------------------------------------------------------------------------- return hrdata
def Tacx2Dongle(self): global devAntDongle, devTrainer #--------------------------------------------------------------------------- # Initialize antDongle # Open two channels: # one to transmit the trainer info (Fitness Equipment) # one to transmit heartrate info (HRM monitor) #--------------------------------------------------------------------------- ant.ResetDongle(devAntDongle) # reset dongle ant.Calibrate(devAntDongle) # calibrate ANT+ dongle ant.Trainer_ChannelConfig( devAntDongle) # Create ANT+ master channel for FE-C ant.HRM_ChannelConfig(devAntDongle) # Create ANT+ master channel for HRM if not clv.gui: logfile.Write("Ctrl-C to exit") #--------------------------------------------------------------------------- # Loop control #--------------------------------------------------------------------------- self.RunningSwitch = True EventCounter = 0 #--------------------------------------------------------------------------- # Calibrate trainer #--------------------------------------------------------------------------- Buttons = 0 CountDown = 120 * 4 # 8 minutes; 120 is the max on the cadence meter ResistanceArray = numpy.array( [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # Array for running everage Calibrate = 0 SetTacxMsg(self, "* * * * * C A L I B R A T I N G * * * * *") while self.RunningSwitch == True and not clv.SimulateTrainer and clv.calibrate \ and not Buttons == usbTrainer.CancelButton and Calibrate == 0: StartTime = time.time() #------------------------------------------------------------------------- # Receive / Send trainer #------------------------------------------------------------------------- usbTrainer.SendToTrainer(devTrainer, usbTrainer.modeCalibrate, \ False, False, False, False, False, False, False, False, False) SpeedKmh, WheelSpeed, PedalEcho, HeartRate, CurrentPower, Cadence, TargetResistance, Resistance, Buttons, Axis = \ usbTrainer.ReceiveFromTrainer(devTrainer) #------------------------------------------------------------------------- # Show progress #------------------------------------------------------------------------- if clv.gui: SetTacxMsg(self, "* * * * * C A L I B R A T I N G * * * * *") SetValues(self, SpeedKmh, int(CountDown / 4), round(CurrentPower * -1, 0), gui.mode_Power, 0, 0, Resistance * -1, 0) # ---------------------------------------------------------------------- # Average power over the last 20 readings # Stop if difference between min/max is below threshold (30) # At least 30 seconds but not longer than the countdown time (8 minutes) # Note that the limits are empiracally established. # ---------------------------------------------------------------------- if Resistance < 0 and WheelSpeed > 0: # Calibration is started (with pedal kick) ResistanceArray = numpy.append(ResistanceArray, Resistance * -1) # Add new value to array ResistanceArray = numpy.delete(ResistanceArray, 0) # Remove oldest from array if CountDown < (120 * 4 - 30) and numpy.min(ResistanceArray) > 0: if (numpy.max(ResistanceArray) - numpy.min(ResistanceArray)) < 30 or CountDown <= 0: Calibrate = Resistance * -1 CountDown -= 0.25 # If not started, no count down! #------------------------------------------------------------------------- # WAIT So we do not cycle faster than 4 x per second #------------------------------------------------------------------------- SleepTime = 0.25 - (time.time() - StartTime) if SleepTime > 0: time.sleep(SleepTime) #--------------------------------------------------------------------------- # Stop trainer #--------------------------------------------------------------------------- usbTrainer.SendToTrainer(devTrainer, usbTrainer.modeStop, 0, False, False, 0, 0, 0, 0, 0, clv.SimulateTrainer) #--------------------------------------------------------------------------- # Initialize variables #--------------------------------------------------------------------------- TargetMode = gui.mode_Power TargetGradeFromDongle = 0 TargetPowerFromDongle = 100 # set initial Target Power TargetGrade = 0 # different sets used to implement TargetPower = 100 # manual mode UserAndBikeWeight = 75 + 10 # defined according the standard (data page 51) # testWeight = 10 # used to test SendToTrainer() CurrentPower = 0 SpeedKmh = 0 WheelSpeed = 0 PedalEcho = 0 HeartRate = 0 CurrentPower = 0 Cadence = 0 Resistance = 0 #--------------------------------------------------------------------------- # Trainer variables and counters #--------------------------------------------------------------------------- AccumulatedPower = 0 AccumulatedTimeCounter = 0 AccumulatedTime = 0 AccumulatedLastTime = time.time() DistanceTravelled = 0 #--------------------------------------------------------------------------- # Heart Rate #--------------------------------------------------------------------------- HeartBeatCounter = 0 HeartBeatEventTime = 0 HeartBeatTime = 0 PageChangeToggle = 0 try: while self.RunningSwitch == True: StartTime = time.time() #------------------------------------------------------------------- # Get data from trainer # TRAINER- SHOULD WRITE THEN READ 70MS LATER REALLY #------------------------------------------------------------------- if clv.SimulateTrainer: SpeedKmh, WheelSpeed, PedalEcho, HeartRate, CurrentPower, Cadence, Resistance, CurrentResistance, Buttons, Axis = \ SimulateReceiveFromTrainer (TargetPower, CurrentPower) else: SpeedKmh, WheelSpeed, PedalEcho, HeartRate, CurrentPower, Cadence, Resistance, CurrentResistance, Buttons, Axis = \ usbTrainer.ReceiveFromTrainer(devTrainer) if CurrentPower < 0: CurrentPower = 0 # No negative value defined for ANT message Page25 (#) CurrentPower /= clv.PowerFactor #------------------------------------------------------------------- # Show results #------------------------------------------------------------------- if SpeedKmh == "Not Found": SpeedKmh, WheelSpeed, PedalEcho, HeartRate, CurrentPower, Cadence, Resistance, Buttons, Axis = 0, 0, 0, 0, 0, 0, 0, 0, 0 SetTacxMsg(self, 'Cannot read from trainer') else: if clv.gui: SetTacxMsg(self, "Trainer detected") #------------------------------------------------------------------- # In manual-mode, power can be incremented or decremented # In all modes, operation can be stopped. #------------------------------------------------------------------- if clv.manual: if Buttons == usbTrainer.EnterButton: pass elif Buttons == usbTrainer.DownButton: TargetPower -= 50 # testWeight -= 10 to test effect of Weight elif Buttons == usbTrainer.UpButton: TargetPower += 50 # testWeight += 10 elif Buttons == usbTrainer.CancelButton: self.RunningSwitch = False else: pass else: if Buttons == usbTrainer.EnterButton: pass elif Buttons == usbTrainer.DownButton: pass elif Buttons == usbTrainer.UpButton: pass elif Buttons == usbTrainer.CancelButton: self.RunningSwitch = False else: pass if TargetMode == gui.mode_Power: TargetPower = TargetPowerFromDongle * clv.PowerFactor TargetGrade = 0 elif TargetMode == gui.mode_Grade: TargetPower = 0 TargetGrade = TargetGradeFromDongle else: logfile.Write("Unsupported TargetMode %s" % TargetMode) #------------------------------------------------------------------- # Send data to trainer (either power OR grade) #------------------------------------------------------------------- usbTrainer.SendToTrainer(devTrainer, usbTrainer.modeResistance, \ TargetMode, TargetPower, TargetGrade, UserAndBikeWeight, \ PedalEcho, WheelSpeed, Cadence, Calibrate, clv.SimulateTrainer) # testWeight #------------------------------------------------------------------- # Prepare data to be sent to ANT+ #------------------------------------------------------------------- CurrentPower = max(0, CurrentPower) # Not negative CurrentPower = min(4093, CurrentPower) # Limit to 4093 Cadence = min(253, Cadence) # Limit to 253 AccumulatedPower += CurrentPower if AccumulatedPower >= 65536: AccumulatedPower = 0 if EventCounter % 64 in ( 30, 31 ): # After 10 blocks of three messages, then 2 = 32 messages #--------------------------------------------------------------- # Send first and second manufacturer's info packet # FitSDKRelease_20.50.00.zip # profile.xlsx # D00001198_-_ANT+_Common_Data_Pages_Rev_3.1%20.pdf # page 28 byte 4,5,6,7- 15=dynastream, 89=tacx #--------------------------------------------------------------- comment = "(Manufacturer's info packet)" info = ant.msgPage80_ManufacturerInfo(ant.channel_FE) newdata = ant.ComposeMessage(ant.msgID_BroadcastData, info) if EventCounter % 64 in ( 62, 63 ): # After 10 blocks of three messages, then 2 = 32 messages #--------------------------------------------------------------- # Send first and second product info packet #--------------------------------------------------------------- comment = "(Product info packet)" info = ant.msgPage81_ProductInformation(ant.channel_FE) newdata = ant.ComposeMessage(ant.msgID_BroadcastData, info) elif EventCounter % 3 == 0: #--------------------------------------------------------------- # Send general fe data every 3 packets #--------------------------------------------------------------- AccumulatedTimeCounter += 1 AccumulatedTime = int(time.time() - AccumulatedLastTime) # time since start Distance = AccumulatedTime * SpeedKmh * 1000 / 3600 # SpeedKmh reported in kmh- convert to m/s DistanceTravelled += Distance if AccumulatedTimeCounter >= 256 or DistanceTravelled >= 256: # rollover at 64 seconds (256 quarter secs) AccumulatedTimeCounter = 0 AccumulatedLastTime = time.time() # Reset last loop time DistanceTravelled = 0 comment = "(General fe data)" # Note: AccumulatedTimeCounter as first parameter, # To be checked whether it should be AccumulatedTime (in 0.25 s) info = ant.msgPage16_GeneralFEdata( ant.channel_FE, AccumulatedTimeCounter, DistanceTravelled, SpeedKmh * 1000 * 1000 / 3600, HeartRate) newdata = ant.ComposeMessage(ant.msgID_BroadcastData, info) else: #--------------------------------------------------------------- # Send specific trainer data #--------------------------------------------------------------- comment = "(Specific trainer data)" info = ant.msgPage25_TrainerData(ant.channel_FE, EventCounter, Cadence, AccumulatedPower, CurrentPower) newdata = ant.ComposeMessage(ant.msgID_BroadcastData, info) #------------------------------------------------------------------- # Broadcast and receive ANT+ data #------------------------------------------------------------------- data = ant.SendToDongle([newdata], devAntDongle, comment, True, False) #------------------------------------------------------------------- # Here all response from the ANT dongle are processed (receive=True) # # Commands from dongle that are expected are: # - TargetGradeFromDongle or TargetPowerFromDongle #------------------------------------------------------------------- for d in data: synch, length, id, info, checksum, rest, Channel, DataPageNumber = ant.DecomposeMessage( d) error = False #--------------------------------------------------------------- # Fitness Equipment Channel inputs #--------------------------------------------------------------- if Channel == ant.channel_FE: if id == ant.msgID_AcknowledgedData: #------------------------------------------------------- # Data page 48 (0x30) Basic resistance #------------------------------------------------------- if DataPageNumber == 48: TargetMode = gui.mode_Basic TargetGradeFromDongle = 0 TargetPowerFromDongle = ant.msgUnpage48_BasicResistance( info) * 1000 # n % of maximum of 1000Watt #------------------------------------------------------- # Data page 49 (0x31) Target Power #------------------------------------------------------- elif DataPageNumber == 49: TargetMode = gui.mode_Power TargetGradeFromDongle = 0 TargetPowerFromDongle = ant.msgUnpage49_TargetPower( info) #------------------------------------------------------- # Data page 51 (0x33) Track resistance #------------------------------------------------------- elif DataPageNumber == 51: TargetMode = gui.mode_Grade TargetGradeFromDongle = ant.msgUnpage51_TrackResistance( info) TargetPowerFromDongle = 0 #------------------------------------------------------- # Data page 55 User configuration #------------------------------------------------------- elif DataPageNumber == 55: UserWeight, BicycleWeight, BicyleWheelDiameter, GearRatio = ant.msgUnpage55_UserConfiguration( info) UserAndBikeWeight = UserWeight + BicycleWeight #------------------------------------------------------- # Data page 70 Request data page #------------------------------------------------------- elif DataPageNumber == 70: SlaveSerialNumber, DescriptorByte1, DescriptorByte2, AckRequired, NrTimes, \ RequestedPageNumber, CommandType = ant.msgUnpage70_RequestDataPage(info) info = False if RequestedPageNumber == 80: info = ant.msgPage80_ManufacturerInfo( ant.channel_FE) comment = "(Manufactorer info)" elif RequestedPageNumber == 81: info = ant.msgPage81_ProductInformation( ant.channel_FE) comment = "(Product info)" elif RequestedPageNumber == 82: info = ant.msgPage82_BatteryStatus( ant.channel_FE) comment = "(Battery status)" else: error = "Requested page not suported" if info != False: data = [] d = ant.ComposeMessage(ant.msgID_BroadcastData, info) while (NrTimes): data.append(d) NrTimes -= 1 ant.SendToDongle(data, devAntDongle, comment, False) #------------------------------------------------------- # Other data pages #------------------------------------------------------- else: error = "Unknown data page" elif id == ant.msgID_ChannelResponse: Channel, InitiatingMessageID, ResponseCode = ant.unmsg64_ChannelResponse( info) pass else: error = "Unknown message ID" #--------------------------------------------------------------- # Heart Rate Monitor inputs #--------------------------------------------------------------- elif Channel == ant.channel_HRM: if id == ant.msgID_ChannelResponse: Channel, InitiatingMessageID, ResponseCode = ant.unmsg64_ChannelResponse( info) pass else: error = "Unknown message ID" #--------------------------------------------------------------- # Unknown channel #--------------------------------------------------------------- else: error = "Unknown channel" #--------------------------------------------------------------- # Unsupported channel, message or page can be silentedly ignored # Show WHAT we ignore, not to be blind for surprises! #--------------------------------------------------------------- if error and (True or debug.on(debug.Data1)): logfile.Write(\ "Dongle error:%s: synch=%s, len=%2s, id=%s, check=%s, channel=%s, page=%s(%s) info=%s" % \ (error, synch, length, id, checksum, Channel, DataPageNumber, hex(DataPageNumber), logfile.HexSpace(info))) #--------------------------------------------------------------------- # Broadcast Heartrate. # This appears as a separate ANT-device "on air" # Heartrate is filled if a HRM is detected by the trainer #--------------------------------------------------------------------- if True and HeartRate > 0: #----------------------------------------------------------------- # Check if heart beat has occurred as tacx only reports # instantaneous heart rate data # Last heart beat is at HeartBeatEventTime # If now - HeartBeatEventTime > time taken for hr to occur, trigger beat. # # We pass here every 250ms. # If one heart_beat occurred, increment counter and time. # Ignore that multiple heart-beats could have occurred; increment # with one beat per cycle only. # # Page 0 is the main page and transmitted most often # In every set of 64 data-pages, page 2 and 3 must be transmitted 4 # times. # To make this fit in the EventCounter cycle (0...255) I have # chosen blocks of 64 messages as below: #----------------------------------------------------------------- if (time.time() - HeartBeatTime) >= (60 / float(HeartRate)): HeartBeatCounter += 1 # Increment heart beat count HeartBeatEventTime += (60 / float(HeartRate) ) # Reset last time of heart beat HeartBeatTime = time.time( ) # Current time for next processing if HeartBeatEventTime >= 64 or HeartBeatCounter >= 256: # Rollover at 64seconds HeartBeatCounter = 0 HeartBeatEventTime = 0 HeartBeatTime = 0 if EventCounter % 4 == 0: PageChangeToggle ^= 0x80 # toggle bit every 4 counts if EventCounter % 64 <= 55: # Transmit 56 times Page 0 = Main data page DataPageNumber = 0 Spec1 = 0xff # Reserved Spec2 = 0xff # Reserved Spec3 = 0xff # Reserved comment = "(HR data p0)" elif EventCounter % 64 <= 59: # Transmit 4 times (56, 57, 58, 59) Page 2 = Manufacturer info DataPageNumber = 2 Spec1 = 0x01 # Manufacturer ID LSB 1=garmin, 15=Dynastream, see FitSDKRelease_21.20.00 profile.xlsx Spec2 = 0x75 # Serial Number LSB Spec3 = 0x59 # Serial Number MSB # 1959-07-05 comment = "(HR data p2)" elif EventCounter % 64 <= 63: # Transmit 4 times (60, 61, 62, 63) Page 3 = Product information DataPageNumber = 3 Spec1 = 0x01 # Hardware version Spec2 = 0x01 # Software version Spec3 = 0x33 # Model number comment = "(HR data p3)" info = ant.msgPage_Hrm(ant.channel_HRM, PageChangeToggle | DataPageNumber, Spec1, Spec2, Spec3, HeartBeatEventTime, HeartBeatCounter, HeartRate) hrdata = ant.ComposeMessage(ant.msgID_BroadcastData, info) # Removed, because I do not see the purpose # We have to send every 250ms on either channel # It does not meand, we have to send every 125ms on all channels. # SleepTime = 0.125 - (time.time() - StartTime) # if SleepTime > 0: time.sleep(SleepTime) # Sleep for 125ms # # So we transmit once every 125ms, alternating Trainer and HRM ant.SendToDongle([hrdata], devAntDongle, comment, False) #--------------------------------------------------------------------- # Show progress #--------------------------------------------------------------------- TargetPower = round(TargetPower, 0) SetValues(self, SpeedKmh, Cadence, round(CurrentPower, 0), TargetMode, TargetPower, TargetGrade, Resistance, HeartRate) #--------------------------------------------------------------------- # WAIT So we do not cycle faster than 4 x per second #--------------------------------------------------------------------- SleepTime = 0.25 - (time.time() - StartTime) if SleepTime > 0: time.sleep(SleepTime) if debug.on(debug.Data2): logfile.Write("Sleep(%4.2f) to fill 0.25 seconds done." % (SleepTime)) else: logfile.Write("Processing longer than 0.25 seconds: %4.2f" % (SleepTime * -1)) pass EventCounter += 1 # Increment and ... EventCounter &= 0xff # maximize to 255 except KeyboardInterrupt: logfile.Write("Stopped") #--------------------------------------------------------------------------- # Stop devices #--------------------------------------------------------------------------- ant.ResetDongle(devAntDongle) #reset dongle usbTrainer.SendToTrainer(devTrainer, usbTrainer.modeStop, 0, False, False, \ 0, 0, 0, 0, 0, clv.SimulateTrainer) return True