def InitialiseTrainer(dev): # will not read cadence until initialisation byte is sent data = struct.pack(sc.unsigned_int, 0x00000002) if debug.on(debug.Data2): logfile.Write("InitialiseTrainer data=%s (len=%s)" % (logfile.HexSpace(data), len(data))) dev.write(0x02, data)
data = [] d = ant.ComposeMessage( ant.msgID_BroadcastData, info) while (NrTimes): data.append(d) NrTimes -= 1 AntDongle.Write(data, False) #------------------------------------------------------- # Other data pages #------------------------------------------------------- else: error = "Unknown FE data page" if Unknown: logfile.Console ("IGNORED!! msg=%s ch=%s p=%s info=%s" % \ (hex(id), Channel, DataPageNumber, logfile.HexSpace(info))) #------------------------------------------------------- # WAIT So we do not cycle faster than 4 x per second #------------------------------------------------------- SleepTime = 0.25 - (time.time() - StartTime) if SleepTime > 0: time.sleep(SleepTime) #------------------------------------------------------- # Inform once per second #------------------------------------------------------- listenCount += 1 if listenCount == 4: listenCount = 0 if clv.SimulateTrainer: logfile.Console ( ("Simulate Cadence=%3s Power=%3s Speed=%4.1f hr=%3s " + \
#------------------------------------------------------------------------- CadenceEventTime = int(CadenceEventTime) & 0xffff # roll-over at 65535 = 64 seconds CadenceEventCount = int(CadenceEventCount) & 0xffff # roll-over at 65535 SpeedEventTime = int(SpeedEventTime) & 0xffff # roll-over at 65535 = 64 seconds SpeedEventCount = int(SpeedEventCount) & 0xffff # roll-over at 65535 #------------------------------------------------------------------------- # Prepare for next event #------------------------------------------------------------------------- PedalEchoPreviousCount = PedalEchoCount #------------------------------------------------------------------------- # Compose message #------------------------------------------------------------------------- info = ant.msgPage_SCS (ant.channel_SCS, CadenceEventTime, CadenceEventCount, SpeedEventTime, SpeedEventCount) scsdata = ant.ComposeMessage (ant.msgID_BroadcastData, info) #------------------------------------------------------------------------- # Return message to be sent #------------------------------------------------------------------------- return scsdata #------------------------------------------------------------------------------- # Main program for module test #------------------------------------------------------------------------------- if __name__ == "__main__": Initialize() time.sleep(1) scsdata = BroadcastMessage (0, 1, 45.6, 123) print (logfile.HexSpace(scsdata))
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
def ReceiveFromTrainer(devTrainer): global trainer_type Axis = 0 Buttons = 0 Cadence = 0 CurrentPower = 0 CurrentResistance = 0 Error = "" HeartRate = 0 PedalEcho = 0 Speed = 0 TargetResistance = 0 #----------------------------------------------------------------------------- # Read from trainer #----------------------------------------------------------------------------- data = "" try: data = devTrainer.read(0x82, 64, 30) except TimeoutError: pass except Exception as e: if "timeout error" in str(e) or "timed out" in str( e): # trainer did not return any data pass else: logfile.Console("ReceiveFromTrainer: USB READ ERROR: " + str(e)) if debug.on(debug.Data2): logfile.Write("Trainer recv data=%s (len=%s)" % (logfile.HexSpace(data), len(data))) #----------------------------------------------------------------------------- # Handle data when > 40 bytes (boundary as derived from antifier) #----------------------------------------------------------------------------- if len(data) > 40 and LegacyProtocol == False: #--------------------------------------------------------------------------- # Define buffer format #--------------------------------------------------------------------------- nDeviceSerial = 0 # 0...1 fDeviceSerial = sc.unsigned_short fFiller2_7 = sc.pad * (7 - 1) # 2...7 nYearProduction = 1 # 8 fYearProduction = sc.unsigned_char fFiller9_11 = sc.pad * (11 - 8) # 9...11 nHeartRate = 2 # 12 fHeartRate = sc.unsigned_char nButtons = 3 # 13 fButtons = sc.unsigned_char nHeartDetect = 4 # 14 fHeartDetect = sc.unsigned_char nErrorCount = 5 # 15 fErrorCount = sc.unsigned_char nAxis0 = 6 # 16-17 fAxis0 = sc.unsigned_short nAxis1 = 7 # 18-19 fAxis1 = sc.unsigned_short nAxis2 = 8 # 20-21 fAxis2 = sc.unsigned_short nAxis3 = 9 # 22-23 fAxis3 = sc.unsigned_short nHeader = 10 # 24-27 fHeader = sc.unsigned_int nDistance = 11 # 28-31 fDistance = sc.unsigned_int nSpeed = 12 # 32, 33 Wheel speed (Speed = WheelSpeed / SpeedScale in km/h) fSpeed = sc.unsigned_short fFiller34_35 = sc.pad * 2 # 34...35 Increases if you accellerate? fFiller36_37 = sc.pad * 2 # 36...37 Average power? nCurrentResistance = 13 # 38, 39 fCurrentResistance = sc.short nTargetResistance = 14 # 40, 41 fTargetResistance = sc.short nEvents = 15 # 42 fEvents = sc.unsigned_char fFiller43 = sc.pad # 43 nCadence = 16 # 44 fCadence = sc.unsigned_char fFiller45 = sc.pad # 45 nModeEcho = 17 # 46 fModeEcho = sc.unsigned_char nChecksumLSB = 18 # 47 fChecksumLSB = sc.unsigned_char nChecksumMSB = 19 # 48 fChecksumMSB = sc.unsigned_char fFiller49_63 = sc.pad * (63 - 48) # 49...63 format = sc.no_alignment + fDeviceSerial + fFiller2_7 + fYearProduction + \ fFiller9_11 + fHeartRate + fButtons + fHeartDetect + fErrorCount + \ fAxis0 + fAxis1 + fAxis2 + fAxis3 + fHeader + fDistance + fSpeed + \ fFiller34_35 + fFiller36_37 + fCurrentResistance + fTargetResistance + \ fEvents + fFiller43 + fCadence + fFiller45 + fModeEcho + \ fChecksumLSB + fChecksumMSB + fFiller49_63 #--------------------------------------------------------------------------- # Buffer must be 64 characters (struct.calcsize(format)), # Note that tt_FortiusSB returns 48 bytes only; append with dummy #--------------------------------------------------------------------------- for v in range(64 - len(data)): data.append(0) #--------------------------------------------------------------------------- # Parse buffer #--------------------------------------------------------------------------- tuple = struct.unpack(format, data) Cadence = tuple[nCadence] HeartRate = tuple[nHeartRate] PedalEcho = tuple[nEvents] TargetResistance = tuple[nTargetResistance] CurrentResistance = tuple[nCurrentResistance] Speed = Wheel2Speed(tuple[nSpeed]) CurrentPower = Resistance2Power(tuple[nCurrentResistance], Speed) Buttons = tuple[nButtons] Axis = tuple[nAxis1] elif LegacyProtocol == True: #--------------------------------------------------------------------------- # Define buffer format #--------------------------------------------------------------------------- nStatusAndCursors = 0 # 0 fStatusAndCursors = sc.unsigned_char nSpeed = 1 # 1, 2 Wheel speed (Speed = WheelSpeed / SpeedScale in km/h) fSpeed = sc.unsigned_short nCadence = 2 # 3 fCadence = sc.unsigned_char nHeartRate = 3 # 4 fHeartRate = sc.unsigned_char nStopWatch = 4 fStopWatch = sc.unsigned_int # 5,6,7,8 nCurrentResistance = 5 # 9 fCurrentResistance = sc.unsigned_char nPedalSensor = 6 # 10 fPedalSensor = sc.unsigned_char nAxis0 = 7 # 11 fAxis0 = sc.unsigned_char nAxis1 = 8 # 12 fAxis1 = sc.unsigned_char nAxis2 = 9 # 13 fAxis2 = sc.unsigned_char nAxis3 = 10 # 14 fAxis3 = sc.unsigned_char nCounter = 11 # 15 fCounter = sc.unsigned_char nWheelCount = 12 # 16 fWheelCount = sc.unsigned_char nYearProduction = 13 # 17 fYearProduction = sc.unsigned_char nDeviceSerial = 14 # 18, 19 fDeviceSerial = sc.unsigned_short nFirmwareVersion = 15 # 20 fFirmwareVersion = sc.unsigned_char #--------------------------------------------------------------------------- # Parse buffer # Note that the button-bits have an inversed logic: # 1=not pushed, 0=pushed. Hence the xor. #--------------------------------------------------------------------------- format = sc.no_alignment + fStatusAndCursors + fSpeed + fCadence + fHeartRate + fStopWatch + fCurrentResistance + \ fPedalSensor + fAxis0 + fAxis1 + fAxis2 + fAxis3 + fCounter + fWheelCount + \ fYearProduction + fDeviceSerial + fFirmwareVersion tuple = struct.unpack(format, data) Cadence = tuple[nCadence] HeartRate = tuple[nHeartRate] PedalEcho = tuple[nPedalSensor] CurrentResistance = tuple[nCurrentResistance] TargetResistance = CurrentResistance # Is not separately returned # is displayed by FortiusANT! Speed = Wheel2Speed(tuple[nSpeed]) Buttons = ((tuple[nStatusAndCursors] & 0xf0) >> 4) ^ 0x0f Axis = tuple[nAxis1] CurrentPower = Resistance2Power(CurrentResistance, Speed) else: Error = "Not Found" if debug.on(debug.Function): logfile.Write ("ReceiveFromTrainer() = hr=%s Cadence=%s Speed=%s TargetRes=%s CurrentRes=%s CurrentPower=%s, pe=%s %s" % \ ( HeartRate, Cadence, Speed, TargetResistance, CurrentResistance, CurrentPower, PedalEcho, Error) \ ) return Error, Speed, PedalEcho, HeartRate, CurrentPower, Cadence, TargetResistance, CurrentResistance, Buttons, Axis
def SendToTrainer(devTrainer, Mode, TargetMode, TargetPower, TargetGrade, PowercurveFactor, UserAndBikeWeight, \ PedalEcho, SpeedKmh, Cadence, Calibrate, SimulateTrainer): global LegacyProtocol # As filled in GetTrainer() if debug.on(debug.Function): logfile.Write("SendToTrainer(%s, %s, %s, %s, %s, %s, %s, %s)" % (Mode, TargetMode, TargetPower, TargetGrade, UserAndBikeWeight, PedalEcho, SpeedKmh, Cadence)) #--------------------------------------------------------------------------- # Data buffer depends on trainer_type # Refer to TotalReverse; "Legacy protocol" or "Newer protocol" #--------------------------------------------------------------------------- if LegacyProtocol == True: fDesiredForceValue = sc.unsigned_char # 0 0x00-0xff # 0x80 = field switched off # < 0x80 reduce brake force # > 0x80 increase brake force fStartStop = sc.unsigned_char # 1 0x01 = pause/start manual control # 0x02 = autopause if wheel < 20 # 0x04 = autostart if wheel >= 20 fStopWatch = sc.unsigned_int # 2...5 else: fControlCommand = sc.unsigned_int # 0...3 fTarget = sc.short # 4, 5 Resistance for Power=50...1000Watt fPedalecho = sc.unsigned_char # 6 fFiller7 = sc.pad # 7 fMode = sc.unsigned_char # 8 Idle=0, Ergo/Slope=2, Calibrate/speed=3 fWeight = sc.unsigned_char # 9 Ergo: 0x0a; Weight = ride + bike in kg fCalibrate = sc.unsigned_short # 10, 11 Depends on mode #--------------------------------------------------------------------------- # Prepare parameters to be sent to trainer #--------------------------------------------------------------------------- error = False if Mode == modeStop: Calibrate = 0 PedalEcho = 0 Target = 0 Weight = 0 pass elif Mode == modeResistance: if Calibrate == 0: # Use old formula: Calibrate = 0 # may be -8...+8 Calibrate = (Calibrate + 8) * 130 # 0x0410 if TargetMode == mode_Power: Target = Power2Resistance(TargetPower, SpeedKmh, Cadence) # Target *= PowercurveFactor # manual adjustment of requested resistance # IS NOT permitted: When trainer software # asks for 100W, you will get 100Watt! if LegacyProtocol == False and Target < Calibrate: Target = Calibrate # Avoid motor-function for low TargetPower Weight = 0x0a # weight=0x0a is a good fly-wheel value # UserAndBikeWeight is not used! elif TargetMode == mode_Grade: Target = Grade2Resistance(TargetGrade, UserAndBikeWeight, SpeedKmh, Cadence) Target *= PowercurveFactor # manual adjustment of requested resistance Weight = 0x0a # weight=0x0a is a good fly-wheel value # UserAndBikeWeight is not used! # an 100kg flywheels gives undesired behaviour :-) else: error = "SendToTrainer; Unsupported TargetMode %s" % TargetMode elif Mode == modeCalibrate: Calibrate = 0 PedalEcho = 0 Target = Speed2Wheel(20) # 20 km/h is our decision for calibration Weight = 0 else: error = "SendToTrainer; incorrect Mode %s!" % Mode if error: logfile.Console(error) else: #----------------------------------------------------------------------- # Build data buffer to be sent to trainer (legacy or new) #----------------------------------------------------------------------- if LegacyProtocol == True: if Mode == modeResistance: DesiredForceValue = Target StartStop, StopWatch = 0, 0 # GoldenCheetah sends 2-byte data # in sendRunCommand() with # StartStop always zero # so we should be good like this format = sc.no_alignment + fDesiredForceValue + fStartStop + fStopWatch data = struct.pack(format, DesiredForceValue, StartStop, StopWatch) else: data = False else: format = sc.no_alignment + fControlCommand + fTarget + fPedalecho + fFiller7 + fMode + fWeight + fCalibrate data = struct.pack(format, 0x00010801, int(Target), PedalEcho, Mode, Weight, Calibrate) #----------------------------------------------------------------------- # Send buffer to trainer #----------------------------------------------------------------------- if not SimulateTrainer and data != False: if debug.on(debug.Data2): logfile.Write("Trainer send data=%s (len=%s)" % (logfile.HexSpace(data), len(data))) logfile.Write (" target=%s pe=%s mode=%s weight=%s cal=%s" % \ (int(Target), PedalEcho, Mode, Weight, Calibrate)) try: devTrainer.write(0x02, data, 30) # send data to device except Exception as e: logfile.Console("SendToTrainer: USB WRITE ERROR " + str(e))
def GetTrainer(): global trainer_type global LegacyProtocol #--------------------------------------------------------------------------- # Initialize #--------------------------------------------------------------------------- if debug.on(debug.Function): logfile.Write("GetTrainer()") trainer_type = 0 LegacyProtocol = False #--------------------------------------------------------------------------- # Find supported trainer #--------------------------------------------------------------------------- msg = "No Tacx trainer found" for idp in [ tt_iMagic, tt_iMagicWG, tt_Fortius, tt_FortiusSB, tt_FortiusSB_nfw ]: try: if debug.on(debug.Function): logfile.Write("GetTrainer - Check for trainer %s" % (hex(idp))) dev = usb.core.find(idVendor=idVendor_Tacx, idProduct=idp) # find trainer USB device if dev != None: msg = "Connected to Tacx Trainer T" + hex(idp)[ 2:] # remove 0x from result, was "Trainer found:" if debug.on(debug.Data2 | debug.Function): logfile.Print(dev) trainer_type = idp break except Exception as e: if debug.on(debug.Function): logfile.Write("GetTrainer - " + str(e)) if "AttributeError" in str(e): msg = "GetTrainer - Could not find USB trainer: " + str(e) elif "No backend" in str(e): msg = "GetTrainer - No backend, check libusb: " + str(e) else: msg = "GetTrainer: " + str(e) #--------------------------------------------------------------------------- # Initialise trainer (if found) #--------------------------------------------------------------------------- if trainer_type == 0: dev = False else: # found trainer #----------------------------------------------------------------------- # iMagic As defined together with fritz-hh and jegorvin) #----------------------------------------------------------------------- if trainer_type == tt_iMagic: LegacyProtocol = True if debug.on(debug.Function): logfile.Write("GetTrainer - Found iMagic head unit") try: fxload.loadHexFirmware(dev, imagic_fw) if debug.on(debug.Function): logfile.Write( "GetTrainer - Initialising head unit, please wait 5 seconds" ) time.sleep(5) msg = "GetTrainer - iMagic head unit initialised" except: # not found msg = "GetTrainer - Unable to initialise trainer" dev = False #----------------------------------------------------------------------- # unintialised Fortius (as provided by antifier original code) #----------------------------------------------------------------------- if trainer_type == tt_FortiusSB_nfw: if debug.on(debug.Function): logfile.Write( "GetTrainer - Found uninitialised Fortius head unit") try: fxload.loadHexFirmware(dev, fortius_fw) if debug.on(debug.Function): logfile.Write( "GetTrainer - Initialising head unit, please wait 5 seconds" ) time.sleep(5) dev = usb.core.find(idVendor=idVendor_Tacx, idProduct=tt_FortiusB) if dev != None: msg = "GetTrainer - Fortius head unit initialised" trainer_type = tt_FortiusSB else: msg = "GetTrainer - Unable to load firmware" dev = False except: # not found msg = "GetTrainer - Unable to initialise trainer" dev = False #----------------------------------------------------------------------- # Set configuration #----------------------------------------------------------------------- if dev != False: dev.set_configuration() if trainer_type == tt_iMagic: dev.set_interface_altsetting(0, 1) #----------------------------------------------------------------------- # InitialiseTrainer (will not read cadence until initialisation byte is sent) #----------------------------------------------------------------------- if dev != False and LegacyProtocol == False: data = struct.pack(sc.unsigned_int, 0x00000002) if debug.on(debug.Data2): logfile.Write("InitialiseTrainer data=%s (len=%s)" % (logfile.HexSpace(data), len(data))) dev.write(0x02, data) #--------------------------------------------------------------------------- # Done #--------------------------------------------------------------------------- logfile.Console(msg) if debug.on(debug.Function): logfile.Write("GetTrainer() returns, trainertype=" + hex(trainer_type)) return dev, msg
def ReceiveFromTrainer(devTrainer): global trainer_type if debug.on(debug.Function): logfile.Write("ReceiveFromTrainer()") Axis = 0 Buttons = 0 Cadence = 0 CurrentPower = 0 HeartRate = 0 PedalEcho = 0 TargetResistance = 0 CurrentResistance = 0 Speed = 0 WheelSpeed = 0 #----------------------------------------------------------------------------- # Read from trainer #----------------------------------------------------------------------------- try: data = devTrainer.read(0x82, 64, 30) except Exception as e: if "timeout error" in str(e): #trainer did not return any data pass else: logfile.Write("ReceiveFromTrainer: USB READ ERROR: " + str(e)) data = "" if debug.on(debug.Data2): logfile.Write("Trainer recv data=%s (len=%s)" % (logfile.HexSpace(data), len(data))) #----------------------------------------------------------------------------- # Handle data when > 40 bytes Will fail when less than struct.calcsize(format) #----------------------------------------------------------------------------- if len(data) > 40: #--------------------------------------------------------------------------- # Define buffer format #--------------------------------------------------------------------------- nDeviceSerial = 0 # 0...1 fDeviceSerial = sc.unsigned_short fFiller2_7 = sc.pad * (7 - 1) # 2...7 nYearProduction = 1 # 8 fYearProduction = sc.unsigned_char fFiller9_11 = sc.pad * (11 - 8) # 9...11 nHeartRate = 2 # 12 fHeartRate = sc.unsigned_char nButtons = 3 # 13 fButtons = sc.unsigned_char nHeartDetect = 4 # 14 fHeartDetect = sc.unsigned_char nErrorCount = 5 # 15 fErrorCount = sc.unsigned_char nAxis0 = 6 # 16-17 fAxis0 = sc.unsigned_short nAxis1 = 7 # 18-19 fAxis1 = sc.unsigned_short nAxis2 = 8 # 20-21 fAxis2 = sc.unsigned_short nAxis3 = 9 # 22-23 fAxis3 = sc.unsigned_short nHeader = 10 # 24-27 fHeader = sc.unsigned_int nDistance = 11 # 28-31 fDistance = sc.unsigned_int nSpeed = 12 # 32, 33 Wheel speed (Speed = WheelSpeed / SpeedScale in km/h) fSpeed = sc.unsigned_short fFiller34_35 = sc.pad * 2 # 34...35 Increases if you accellerate? fFiller36_37 = sc.pad * 2 # 34...35 Average power? nCurrentResistance = 13 # 38, 39 fCurrentResistance = sc.short nTargetResistance = 14 # 40, 41 fTargetResistance = sc.short nEvents = 15 # 42 fEvents = sc.unsigned_char fFiller43 = sc.pad # 43 nCadence = 16 # 44 fCadence = sc.unsigned_char fFiller45 = sc.pad # 45 nModeEcho = 17 # 46 fModeEcho = sc.unsigned_char nChecksumLSB = 18 # 47 fChecksumLSB = sc.unsigned_char nChecksumMSB = 19 # 48 fChecksumMSB = sc.unsigned_char fFiller49_63 = sc.pad * (63 - 48) # 49...63 format = sc.no_alignment + fDeviceSerial + fFiller2_7 + fYearProduction + \ fFiller9_11 + fHeartRate + fButtons + fHeartDetect + fErrorCount + \ fAxis0 + fAxis1 + fAxis2 + fAxis3 + fHeader + fDistance + fSpeed + \ fFiller34_35 + fFiller36_37 + fCurrentResistance + fTargetResistance + \ fEvents + fFiller43 + fCadence + fFiller45 + fModeEcho + \ fChecksumLSB + fChecksumMSB + fFiller49_63 #--------------------------------------------------------------------------- # Parse buffer #--------------------------------------------------------------------------- tuple = struct.unpack(format, data) if debug.on(debug.Data2): logfile.Write ("ReceiveFromTrainer: TargetResistance=%s hr=%s sp=%s CurrentResistance=%s pe=%s cad=%s axis=%s %s %s %s" % \ (tuple[nTargetResistance], tuple[nHeartRate], tuple[nSpeed], \ tuple[nCurrentResistance], hex(tuple[nEvents]), tuple[nCadence], \ tuple[nAxis0], tuple[nAxis1], tuple[nAxis2], tuple[nAxis3] \ )) WheelSpeed = tuple[nSpeed] Cadence = tuple[nCadence] CurrentPower = Resistance2Power(tuple[nCurrentResistance], WheelSpeed) HeartRate = tuple[nHeartRate] PedalEcho = tuple[nEvents] TargetResistance = tuple[nTargetResistance] CurrentResistance = tuple[nCurrentResistance] Speed = Wheel2Speed(tuple[nSpeed]) Buttons = tuple[nButtons] Axis = tuple[nAxis1] else: Speed = "Not Found" return Speed, WheelSpeed, PedalEcho, HeartRate, CurrentPower, Cadence, TargetResistance, CurrentResistance, Buttons, Axis
def SendToTrainer(devTrainer, Mode, TargetMode, TargetPower, TargetGrade, UserAndBikeWeight, PedalEcho, WheelSpeed, Cadence, Calibrate, SimulateTrainer): if debug.on(debug.Function): logfile.Write("SendToTrainer(%s, %s, %s, %s, %s, %s, %s)" % (TargetMode, TargetPower, TargetGrade, UserAndBikeWeight, PedalEcho, WheelSpeed, Cadence)) fControlCommand = sc.unsigned_int # 0...3 fTarget = sc.short # 4, 5 Resistance for Power=50...1000Watt fPedalecho = sc.unsigned_char # 6 fFiller7 = sc.pad # 7 fMode = sc.unsigned_char # 8 Idle=0, Ergo/Slope=2, Calibrate/speed=3 fWeight = sc.unsigned_char # 9 Ergo: 0x0a; Weight = ride + bike in kg fCalibrate = sc.unsigned_short # 10, 11 Depends on mode error = False if Mode == modeStop: Calibrate = 0 PedalEcho = 0 Target = 0 Weight = 0 pass elif Mode == modeResistance: if Calibrate == 0: # Use old formula: Calibrate = 0 # may be -8...+8 Calibrate = (Calibrate + 8) * 130 # 0x0410 if TargetMode == gui.mode_Power: Target = Power2Resistance(TargetPower, WheelSpeed, Cadence) if Target < Calibrate: Target = Calibrate # Avoid motor-function for low TargetPower Weight = 0x0a # weight=0x0a is a good fly-wheel value # UserAndBikeWeight is not used! elif TargetMode == gui.mode_Grade: Target = Grade2Resistance(TargetGrade, UserAndBikeWeight, WheelSpeed, Cadence) ######## if Target < Calibrate: Target = Calibrate # Avoid motor-function for descends Weight = 0x0a # weight=0x0a is a good fly-wheel value # UserAndBikeWeight is not used! # an 100kg flywheels gives undesired behaviour :-) else: error = "SendToTrainer; Unsupported TargetMode %s" % TargetMode elif Mode == modeCalibrate: Calibrate = 0 PedalEcho = 0 Target = Speed2Wheel(20) Weight = 0 else: error = "SendToTrainer; incorrect Mode %s!" % Mode if error: logfile.Write(error) else: format = sc.no_alignment + fControlCommand + fTarget + fPedalecho + fFiller7 + fMode + fWeight + fCalibrate data = struct.pack(format, 0x00010801, int(Target), PedalEcho, Mode, Weight, Calibrate) if debug.on(debug.Data2): logfile.Write("Trainer send data=%s (len=%s)" % (logfile.HexSpace(data), len(data))) logfile.Write (" target=%s pe=%s mode=%s weight=%s cal=%s" % \ (int(Target), PedalEcho, Mode, Weight, Calibrate)) if not SimulateTrainer: try: devTrainer.write(0x02, data, 30) # send data to device except Exception as e: logfile.Write("SendToTrainer: USB WRITE ERROR " + str(e))