forked from WouterJD/FortiusANT
-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathFortiusAntBody.py
1246 lines (1120 loc) · 66.4 KB
/
FortiusAntBody.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#-------------------------------------------------------------------------------
# Version info
#-------------------------------------------------------------------------------
__version__ = "2020-09-30"
# 2020-09-30 During Runoff, modeCalibrate was used instead of modeResistance
# an error introduced with release 3.0 and now resolved.
# 2020-09-29 If NO ANTdongle present; users want to be able to test
# FortiusANT to evaluate the software before bying dongles.
# Therefore in manual mode, no ANTdongle required.
# Of course for USB-trainers only (==> not clv.Tacx_iVortex)
# Typo's corrected (thanks RogerPleijers)
# 2020-06-12 Added: BikePowerProfile and SpeedAndCadenceSensor final
# 2020-06-11 Added: BikePowerProfile (master)
# 2020-06-09 Added: SpeedAndCadenceSensor (master)
# 2020-05-27 Added: msgPage71_CommandStatus handled -- and tested
# 2020-05-24 i-Vortex adjustments
# - in manual mode, ANTdongle must be present as well, so that
# manual mode works for i-Vortex as well.
# For USB-trainers perhaps not needed, but FortiusANT needs a
# dongle - I would say by name and hence definition!
# 2020-05-15 Window title removed here
# 2020-05-14 pylint error free
# 2020-05-13 Used: msgUnpage50_WindResistance, msgUnpage51_TrackResistance
# 2020-05-12 Minor code improvements
# 2020-05-08 Usage of clsTacxTrainer
# 2020-05-07 Target*** variables global to survive Restart
# 2020-05-07 clsAntDongle encapsulates all functions
# and implements dongle recovery
# 2020-04-28 Created from FortiusAnt.py
# Now only contains the Tacx2Dongle() basics.
# Startup, GUI and Multiprocessing are in FortiusAnt.py
# 2020-04-28 HeartRateT not defined when using i-Vortex
# 2020-04-24 Recheck & document of TargetMode/Power/Grade/Resistance
# Some calculations relocated
# 2020-04-22 Page16_TacxVortexSetPower sent every 0.25 second
# 2020-04-22 Crash because frame used instead of self
# 2020-04-22 Page16_TacxVortexSetPower only sent when data changed
# HeartRateT was not initialized
# 2020-04-20 txtAntHRM used, i-Vortex paired
# 2020-04-17 Added: Page54 FE_Capabilities
# 2020-04-16 Write() replaced by Console() where needed
# 2020-04-15 Calibration is aborted when iFlow 1932 returns positive values
# Fortius returns one positive reading and then goes negative...
# 2020-04-10 PowerMode added (PowerMode prevails over ResistanceMode)
# 2020-04-09 The PowercurveFactor is displayed as Digital Gearbox in number of
# teeth. factor = 1 = 15 teeth.
# Not a bitmap but (new experience in Python) drawing rectangles.
# 2020-04-07 Before Calibrating, "Start pedalling" is displayed
# PowercurveFactor is limitted to 0.3 ... 3
# Runoff gives error "Calibrate not defined", resolved
# Messaging improved, method is still as inherited from antifier
# 2020-03-29 ReceiveFromTrainer() returns Error as extra parameter
# 2020-03-29 PowercurveFactor implemented to increment/decrement resistance
# Also displayed on console
# 2020-03-04 Console message is displayed when debug.Application is set
# (was debug.Any)
# Conversion error in SpeedKmh when trainer USB returns an error
# ReceiveFromTrainer() returns "Not found" in that case
# Correction of earlier solution.
# 2020-03-03 Format error resolved
# 2020-03-02 iMagic supported, thanks to Julian Pfefferkorn
# 2020-02-27 FE data page 252 ignored
# PrintWarnings added for documentation
# 2020-02-26 msgID_BurstData ignored
# usbTrainer.CalibrateSupported() used to skip calibration on iMagic
# 2020-02-18 Calibrate can be stopped with Ctrl-C
# 2020-02-18 Module antHRM.py and antFE.py created
# 2020-02-12 HRM display data pages 89, 95 implemented...
# 2020-02-12 if SpeedKmh == "Not Found"; required not to crash...
# something to be changed in usbTrainer some day...
# 2020-02-12 Different channel used for HRM and HRM_d
# msgPage80_ManufacturerInfo() was never sent
# 2020-02-10 clv.scs added (example code, not tested)
# 2020-02-09 id / channel handling improved
# (channel was checked before checking id)
# 2020-02-09 clv.hrm implemented
# 2020-01-23 manualGrade added
# in manual mode, dongle entirely ignored
#
# 2020-01-21 Calibration improved
# - Will run 8 minutes to allow tyre to warmup
# - Will end when calibration-value is stable
#
# 2020-01-07 Calibration implemented
#
# 2020-01-06 Calibration tested: calibrate=the initial resistance of the trainer
# which is subtracted from the TargetResistance
# Weight tested: it appears to be "flywheel weight", not user weight
# Mode: trainer has Resistance mode, no Ergo/Slope mode!
#
# 2019-12-24 Target grade implemented
# All received ANT+ messages handled
# Zwift interface works
#
# 2019-12-20 Navigation buttons implemented
# - During Runoff: up/down=add/reduce power, back=stop
# - Normal operation: back=stop
# - Manual operation: up/down=add/reduce power, back=stop
# - Menu mode: up/down=previous/next button,
# enter =press button
# back =stop program
#
# 2019-12-19 Powertest done
# FortiusAnt.py -g -a -p1 -d0 -m
# So using the GUI, clv.autostart, no debugging, manual control
# Powerfactor = 1, which is the standard
#
# Method: bike on the Fortius, power meter on bicycle.
# 1. Select gear so you can run 10km/hr at a reasonable cadence.
# Manually select power 50Watt (up/down button on headunit)
# Ride untill reading from FortiusAnt and PowerMeter is stable.
# Write down power from PowerMeter (46 in table).
# -- Increase power on headunit 100, 150, 200, 250, 300 Watt.
# Repeat test.
# 2. Select gears for 20, 30, 40, 50 km/hr.
# Repeat test.
#
# Test Results (Target power in column header, result in table)
# 50W 100W 150W 200W 250W 300W
# 10 km/hr 46 97 145 194 245 285
# 20 km/hr 50 100 145 197 245 290
# 30 km/hr 63 102 154 196 245 295
# 40 km/hr 105 120 160 210 260 305
# 50 km/hr 123 130 165 210 250 310
#
# Conclusions:
# a. 50Watt at 50km/hr gives odd readings but that's not too strange.
# b. Overall measured power correponds with TargetPower.
# c. Multiple measurements give different results within 5%.
# Tests are done intermittently and hence have different
# brake and tyre temperatures.
# d. PowerFactor not needed as a calibration option.
#
# Assumption
# Attempts to improve the algorithm may be useless, since it
# should not be more exact than the Tacx Fortius (was designed for)
# After all, changing the algorythm remains empirical
#
# 2019-12-17 The program works and is tested:
# - Windows 64, python 3
# - Tacx Fortius
# - Trainer Road in ERG mode
#
# The trainer is transmitted as a tacx. If the heartrate is
# detected on the trainer, the HRM is transmitted as a garmin.
#
# Todo:
# - why is HeartRate not sent to Trainer Road directly but is a
# separate HRM channel needed?
# (see function ComposeGeneralFeInfo)
# done - 2020-02-09: Because TrainerRoad and/or Zwift expect an
# independant ANT+ HRM
# - read buttons from trainer and navigate through menu
# (see function IdleFunction)
# done - 2019-12-20
#
# Tests (and extensions) to be done:
# - test with Trainer Road, resistance mode; done 2019-12-19
# - test with Zwift; done 2019-12-24
# - calibration test; done 2020-01-07
#-------------------------------------------------------------------------------
import argparse
import binascii
import math
import numpy
import os
import pickle
import platform, glob
import random
import sys
import struct
import threading
import time
import usb.core
import wx
from datetime import datetime
import antDongle as ant
import antFE as fe
import antHRM as hrm
import antPWR as pwr
import antSCS as scs
import antCTRL as ctrl
import debug
from FortiusAntGui import mode_Power, mode_Grade
import logfile
import usbTrainer
PrintWarnings = False # Print warnings even when logging = off
CycleTimeFast = 0.02 # TRAINER- SHOULD WRITE THEN READ 70MS LATER REALLY
CycleTimeANT = 0.25
# ------------------------------------------------------------------------------
# Initialize globals
# ------------------------------------------------------------------------------
def Initialize(pclv):
global clv, AntDongle, TacxTrainer
clv = pclv
AntDongle = None
TacxTrainer = None
# ==============================================================================
# Here we go, this is the real work what's all about!
# ==============================================================================
# ------------------------------------------------------------------------------
# I d l e F u n c t i o n
# ------------------------------------------------------------------------------
# input: None
#
# Description: In idle-mode, read trainer and show what button pressed
# So, when the trainer is not yet detected, the trainer cannot
# read for the headunit.
#
# Output: None
#
# Returns: The actual status of the headunit buttons
# ------------------------------------------------------------------------------
def IdleFunction(self):
global TacxTrainer
rtn = 0
if TacxTrainer and TacxTrainer.OK:
TacxTrainer.Refresh(True, usbTrainer.modeCalibrate)
rtn = TacxTrainer.Buttons
return rtn
# ------------------------------------------------------------------------------
# L o c a t e H W
# ------------------------------------------------------------------------------
# input: TacxTrainer, AntDongle
#
# Description: If DONGLE not already opened, Get the dongle
# If TRAINER not already opened, Get the trainer
# unless trainer is simulated then ignore!
# Show appropriate messages
#
# Output: TacxTrainer, AntDongle
#
# Returns: True if TRAINER and DONGLE found
# ------------------------------------------------------------------------------
def LocateHW(self):
global clv, AntDongle, TacxTrainer
if debug.on(debug.Application): logfile.Write ("Scan for hardware")
#---------------------------------------------------------------------------
# Get ANT dongle
#---------------------------------------------------------------------------
if debug.on(debug.Application): logfile.Write ("Get Dongle")
if AntDongle and AntDongle.OK:
pass
else:
AntDongle = ant.clsAntDongle()
if AntDongle.OK or not clv.Tacx_iVortex: # 2020-09-29
if clv.manual: AntDongle.Message += ' (manual power)'
if clv.manualGrade: AntDongle.Message += ' (manual grade)'
self.SetMessages(Dongle=AntDongle.Message)
#---------------------------------------------------------------------------
# Get Trainer and find trainer model for Windows and Linux
#---------------------------------------------------------------------------
if debug.on(debug.Application): logfile.Write ("Get Tacx Trainer")
if TacxTrainer and TacxTrainer.OK:
pass
else:
TacxTrainer = usbTrainer.clsTacxTrainer.GetTrainer(clv, AntDongle)
self.SetMessages(Tacx=TacxTrainer.Message)
#---------------------------------------------------------------------------
# Show where the heartrate comes from
#---------------------------------------------------------------------------
if clv.hrm == None:
self.SetMessages(HRM="Heartrate expected from Tacx Trainer")
else:
self.SetMessages(HRM="Heartrate expected from ANT+ HRM")
#---------------------------------------------------------------------------
# Done
#---------------------------------------------------------------------------
if debug.on(debug.Application): logfile.Write ("Scan for hardware - end")
# 2020-09-29
return ((AntDongle.OK or (not clv.Tacx_iVortex and (clv.manual or clv.manualGrade))) \
and TacxTrainer.OK)
# ------------------------------------------------------------------------------
# R u n o f f
# ------------------------------------------------------------------------------
# input: devTrainer
#
# Description: run trainer untill 40km/h reached then untill stopped.
# Initially, target power is 100Watt, which may be influenced
# with the up/down buttons on the headunit of the trainer.
#
# Output: none
#
# Returns: True
# ------------------------------------------------------------------------------
def Runoff(self):
global clv, AntDongle, TacxTrainer
if clv.SimulateTrainer or clv.Tacx_iVortex:
logfile.Console('Runoff not implemented for Simulated trainer or Tacx i-Vortex')
return False
TacxTrainer.SetPower(100)
rolldown = False
rolldown_time = 0
#---------------------------------------------------------------------------
# Pedal stroke Analysis
#---------------------------------------------------------------------------
pdaInfo = [] # Collection of (time, power)
LastPedalEcho = 0 # Flag that cadence sensor was seen
LastPower = 0 # statistics
PowerCount = 0
PowerEqual = 0
#self.InstructionsVariable.set('''
#CALIBRATION TIPS:
#1. Tyre pressure 100psi (unloaded and cold) aim for 7.2s rolloff
#2. Warm up for 2 mins, then cycle 30kph-40kph for 30s
#3. SpeedKmh up to above 40kph then stop pedalling and freewheel
#4. Rolldown timer will start automatically when you hit 40kph, so stop pedalling quickly!
#''')
if clv.PedalStrokeAnalysis:
CycleTime = CycleTimeFast # Quick poll to get more info
if debug.on(debug.Any):
logfile.Console("Runoff; Pedal Stroke Analysis active")
else:
CycleTime = CycleTimeANT # 0.25 Seconds, inspired by 4Hz ANT+
while self.RunningSwitch == True:
StartTime = time.time()
#-----------------------------------------------------------------------
# Get data from trainer
#-----------------------------------------------------------------------
TacxTrainer.Refresh(True, usbTrainer.modeResistance) # This cannot be an ANT trainer
#-----------------------------------------------------------------------
# Show what happens
#-----------------------------------------------------------------------
if TacxTrainer.Message == "Not Found":
self.SetValues(0, 0, 0, 0, 0, 0, 0, 0, 0)
self.SetMessages(Tacx="Check if trainer is powered on")
else:
self.SetValues( TacxTrainer.SpeedKmh, TacxTrainer.Cadence, \
TacxTrainer.CurrentPower, TacxTrainer.TargetMode, \
TacxTrainer.TargetPower, TacxTrainer.TargetGrade, \
TacxTrainer.TargetResistance, TacxTrainer.HeartRate, \
0, 0)
if not rolldown or rolldown_time == 0:
self.SetMessages(Tacx=TacxTrainer.Message + " - Cycle to above 40kph (then stop)")
else:
self.SetMessages(Tacx=TacxTrainer.Message + \
" - Rolldown timer %s - STOP pedalling!" % \
( round((time.time() - rolldown_time),1) ) \
)
#---------------------------------------------------------------------
# SpeedKmh up to 40 km/h and then rolldown
#---------------------------------------------------------------------
if TacxTrainer.SpeedKmh > 40: # SpeedKmh above 40, start rolldown
rolldown = True
if rolldown and TacxTrainer.SpeedKmh <=40 and rolldown_time == 0:
# rolldown timer starts when dips below 40
rolldown_time = time.time()
if rolldown and TacxTrainer.SpeedKmh < 0.1 : # wheel stopped
self.RunningSwitch = False # break loop
self.SetMessages(Tacx=TacxTrainer.Message + \
" - Rolldown time = %s seconds (aim 7s)" % \
round((time.time() - rolldown_time),1) \
)
#-------------------------------------------------------------------------
# #48 Frequency of data capture - sufficient for pedal stroke analysis?
#
# If we want to do pedal stroke analysis, we should measure every
# pedal-cycle multiple times per rotation.
#
# Number of measurements/sec = (cadence * 360/angle) / 60 = 6 * cadence / angle
# ==> Cycle time (in seconds) = angle / (6 * cadence)
# ==> Angle = Cycle time (in seconds) * 6 * cadence
#
# case 1. Cadence = 120 and Angle = 1 degree
# Cycle time (in seconds) = 1 / (6 * 120) = 1.3889 ms
#
# case 2. Cadence = 120 and Cycle time (in seconds) = 0.25
# angle = .25 * 6 * 120 = 180 degrees
#
# case 2. Cadence = 120 and Cycle time (in seconds) = 0.01
# angle = .01 * 6 * 120 = 1.80 degrees
#-------------------------------------------------------------------------
if clv.PedalStrokeAnalysis:
if LastPedalEcho == 0 and TacxTrainer.PedalEcho == 1 \
and len(pdaInfo) \
and TacxTrainer.Cadence:
# Pedal triggers cadence sensor
self.PedalStrokeAnalysis(pdaInfo, TacxTrainer.Cadence)
pdaInfo = []
# Store data for analysis until next signal
pdaInfo.append((time.time(), TacxTrainer.CurrentPower))
LastPedalEcho = TacxTrainer.PedalEcho
if TacxTrainer.CurrentPower > 50 and TacxTrainer.Cadence > 30:
# Gather some statistics while really pedalling
PowerCount += 1
if TacxTrainer.CurrentPower == LastPower: PowerEqual += 1
LastPower = TacxTrainer.CurrentPower
#-----------------------------------------------------------------------
# Respond to button press
#-----------------------------------------------------------------------
if TacxTrainer.Buttons == usbTrainer.EnterButton: pass
elif TacxTrainer.Buttons == usbTrainer.DownButton: TacxTrainer.AddPower (-50) # Subtract 50 Watts for calibration test
elif TacxTrainer.Buttons == usbTrainer.UpButton: TacxTrainer.AddPower ( 50) # Add 50 Watts for calibration test
elif TacxTrainer.Buttons == usbTrainer.CancelButton: self.RunningSwitch = False # Stop calibration
else: pass
#-----------------------------------------------------------------------
# WAIT untill CycleTime is done
#-----------------------------------------------------------------------
ElapsedTime = time.time() - StartTime
SleepTime = CycleTime - ElapsedTime
if SleepTime > 0:
time.sleep(SleepTime)
if debug.on(debug.Data2):
logfile.Write ("Sleep(%4.2f) to fill %s seconds done." % (SleepTime, CycleTime) )
else:
if ElapsedTime > CycleTime * 2 and debug.on(debug.Any):
logfile.Write ("Runoff; Processing time %5.3f is %5.3f longer than planned %5.3f (seconds)" % (ElapsedTime, SleepTime * -1, CycleTime) )
pass
#---------------------------------------------------------------------------
# Finalize
#---------------------------------------------------------------------------
self.SetValues( 0, 0, 0,TacxTrainer.TargetMode, 0, 0, 0, 0, 0, 0)
if not rolldown:
self.SetMessages(Tacx=TacxTrainer.Message)
if debug.on(debug.Any) and PowerCount > 0:
logfile.Console("Pedal Stroke Analysis: #samples = %s, #equal = %s (%3.0f%%)" % \
(PowerCount, PowerEqual, PowerEqual * 100 /PowerCount))
return True
# ------------------------------------------------------------------------------
# T a c x 2 D o n g l e
# ------------------------------------------------------------------------------
# input: AntDongle, TacxTrainer
#
# Description: Exchange data between TRAINER and DONGLE.
# TRAINER tells DONGLE speed, power, cadence, heartrate
# DONGLE tells TRAINER target power (or grade not tested)
#
# In manual mode, the target power from DONGLE is ignored and
# power is set using the headunit, like in runoff()
#
# Target and Actual data are shown on the interface
#
# Output: none
#
# Returns: True
# ------------------------------------------------------------------------------
def Tacx2Dongle(self):
global clv, AntDongle, TacxTrainer
Restart = False
while True:
rtn = Tacx2DongleSub(self, Restart)
if AntDongle.DongleReconnected:
self.SetMessages(Dongle=AntDongle.Message)
AntDongle.ApplicationRestart()
Restart = True
else:
break
return rtn
def Tacx2DongleSub(self, Restart):
global clv, AntDongle, TacxTrainer
assert(AntDongle and AntDongle.OK)
assert(TacxTrainer and TacxTrainer.OK)
AntHRMpaired = False
#---------------------------------------------------------------------------
# Command status data
#---------------------------------------------------------------------------
p71_LastReceivedCommandID = 255
p71_SequenceNr = 0
p71_CommandStatus = 255
p71_Data1 = 0xff
p71_Data2 = 0xff
p71_Data3 = 0xff
p71_Data4 = 0xff
ctrl_p71_LastReceivedCommandID = 255
ctrl_p71_SequenceNr = 255
ctrl_p71_CommandStatus = 255
ctrl_p71_Data1 = 0xff
ctrl_p71_Data2 = 0xff
ctrl_p71_Data3 = 0xff
ctrl_p71_Data4 = 0xff
#---------------------------------------------------------------------------
# Info from ANT slave channels
#---------------------------------------------------------------------------
HeartRate = 0 # This field is displayed
# We have two sources: the trainer or
# our own HRM slave channel.
#Cadence = 0 # Analogously for Speed Cadence Sensor
# But is not yet implemented
#---------------------------------------------------------------------------
# Pedal stroke Analysis
#---------------------------------------------------------------------------
pdaInfo = [] # Collection of (time, power)
LastPedalEcho = 0 # Flag that cadence sensor was seen
#---------------------------------------------------------------------------
# Initialize Dongle
# Open channels:
# one to transmit the trainer info (Fitness Equipment)
# one to transmit heartrate info (HRM monitor)
# one to interface with Tacx i-Vortex (VTX)
# one to interface with Tacx i-Vortex headunit (VHU)
#
# And if you want a dedicated Speed Cadence Sensor, implement like this...
#---------------------------------------------------------------------------
AntDongle.ResetDongle() # reset dongle
AntDongle.Calibrate() # calibrate ANT+ dongle
AntDongle.Trainer_ChannelConfig() # Create ANT+ master channel for FE-C
if clv.hrm == None:
AntDongle.HRM_ChannelConfig() # Create ANT+ master channel for HRM
elif clv.hrm < 0:
pass # No Heartrate at all
else:
#-------------------------------------------------------------------
# Create ANT+ slave channel for HRM; 0: auto pair, nnn: defined HRM
#-------------------------------------------------------------------
AntDongle.SlaveHRM_ChannelConfig(clv.hrm)
#-------------------------------------------------------------------
# Request what DeviceID is paired to the HRM-channel
# No pairing-loop: HRM perhaps not yet active and avoid delay
#-------------------------------------------------------------------
# msg = ant.msg4D_RequestMessage(ant.channel_HRM_s, ant.msgID_ChannelID)
# AntDongle.Write([msg], False, False)
if clv.Tacx_iVortex:
#-------------------------------------------------------------------
# Create ANT+ slave channel for VTX
# No pairing-loop: VTX perhaps not yet active and avoid delay
#-------------------------------------------------------------------
AntDongle.SlaveVTX_ChannelConfig(0)
# msg = ant.msg4D_RequestMessage(ant.channel_VTX_s, ant.msgID_ChannelID)
# AntDongle.Write([msg], False, False)
#-------------------------------------------------------------------
# Create ANT+ slave channel for VHU
#
# We create this channel right away. At some stage the VTX-channel
# sends the Page03_TacxVortexDataCalibration which provides the
# VortexID. This VortexID is the DeviceID that could be provided
# to SlaveVHU_ChannelConfig() to restrict pairing to that headunit
# only. Not relevant in private environments, so left as is here.
#-------------------------------------------------------------------
AntDongle.SlaveVHU_ChannelConfig(0)
if True:
#-------------------------------------------------------------------
# Create ANT+ master channel for PWR
#-------------------------------------------------------------------
AntDongle.PWR_ChannelConfig(ant.channel_PWR)
if clv.scs == None:
#-------------------------------------------------------------------
# Create ANT+ master channel for SCS
#-------------------------------------------------------------------
AntDongle.SCS_ChannelConfig(ant.channel_SCS)
else:
#-------------------------------------------------------------------
# Create ANT+ slave channel for SCS
# 0: auto pair, nnn: defined SCS
#-------------------------------------------------------------------
AntDongle.SlaveSCS_ChannelConfig(clv.scs)
pass
AntDongle.CTRL_ChannelConfig(ant.DeviceNumber_CTRL)
ControlCommand = ant.CommandNr_CTRL['None']
if not clv.gui: logfile.Console ("Ctrl-C to exit")
#---------------------------------------------------------------------------
# Loop control
#---------------------------------------------------------------------------
EventCounter = 0
#---------------------------------------------------------------------------
# Calibrate trainer
#---------------------------------------------------------------------------
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
StartPedalling = True
Counter = 0
if TacxTrainer.CalibrateSupported():
self.SetMessages(Tacx="* * * * S T A R T P E D A L L I N G * * * *")
if debug.on(debug.Function):
logfile.Write('Tacx2Dongle; start pedalling for calibration')
try:
# if True:
while self.RunningSwitch \
and clv.calibrate \
and not TacxTrainer.Buttons == usbTrainer.CancelButton \
and Calibrate == 0 \
and TacxTrainer.CalibrateSupported():
StartTime = time.time()
#-------------------------------------------------------------------
# Receive / Send trainer
#-------------------------------------------------------------------
TacxTrainer.Refresh(True, usbTrainer.modeCalibrate)
#-------------------------------------------------------------------
# When calibration IS supported, the following condition will NOT occur.
# iFlow 1932 is expected to support calibration but does not.
# This check is to stop calibration-loop because it will never end.
#
# First reading on 'my' Fortius shows a positive number, then goes negative
# so ignore the first x readings before deciding it will not work.
#-------------------------------------------------------------------
# print(StartPedalling, SpeedKmh, CurrentResistance)
if TacxTrainer.CurrentResistance > 0:
Counter += 1
if Counter == 10:
logfile.Console('Calibration stopped because of unexpected resistance value')
break
if TacxTrainer.CurrentResistance < 0 and TacxTrainer.SpeedKmh > 0:
# Calibration is started (with pedal kick)
#---------------------------------------------------------------
# Show progress (when calibrating is started)
# This changes the message from "Start Pedalling" to "Calibrating"
# The message must be given once for the console-mode (no GUI)
#---------------------------------------------------------------
if StartPedalling:
self.SetMessages(Tacx="* * * * C A L I B R A T I N G * * * *")
if debug.on(debug.Function):
logfile.Write('Tacx2Dongle; start calibration')
StartPedalling = False
self.SetValues(TacxTrainer.SpeedKmh, int(CountDown/4), \
round(TacxTrainer.CurrentPower * -1,0), \
mode_Power, 0, 0, TacxTrainer.CurrentResistance * -1, 0, 0, 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.
# --------------------------------------------------------------
ResistanceArray = numpy.append(ResistanceArray, TacxTrainer.CurrentResistance * -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 = TacxTrainer.CurrentResistance * -1
if debug.on(debug.Function):
logfile.Write('Tacx2Dongle; calibration ended %s' % Calibrate)
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)
except KeyboardInterrupt:
logfile.Console ("Stopped")
except Exception as e:
logfile.Console ("Calibration stopped with exception: %s" % e)
#---------------------------------------------------------------------------
# Stop trainer
#---------------------------------------------------------------------------
if TacxTrainer.OK:
if debug.on(debug.Function): logfile.Write('Tacx2Dongle; stop trainer')
TacxTrainer.SendToTrainer(True, usbTrainer.modeStop)
self.SetMessages(Tacx=TacxTrainer.Message)
#---------------------------------------------------------------------------
# Initialize variables
#---------------------------------------------------------------------------
if not Restart:
if clv.manualGrade:
TacxTrainer.SetGrade(0)
else:
TacxTrainer.SetPower(100)
TacxTrainer.ResetPowercurveFactor()
TargetPowerTime = 0 # Time that last TargetPower received
PowerModeActive = '' # Text showing in userinterface
LastANTtime = 0 # ANT+ interface is sent/received only
# every 250ms
#---------------------------------------------------------------------------
# Initialize antHRM and antFE module
#---------------------------------------------------------------------------
if debug.on(debug.Function): logfile.Write('Tacx2Dongle; initialize ANT')
fe.Initialize()
hrm.Initialize()
pwr.Initialize()
scs.Initialize()
ctrl.Initialize()
#---------------------------------------------------------------------------
# Initialize CycleTime: fast for PedalStrokeAnalysis
#---------------------------------------------------------------------------
if clv.PedalStrokeAnalysis:
CycleTime = CycleTimeFast # Quick poll to get more info
if debug.on(debug.Any): logfile.Console("Tacx2Dongle; Pedal Stroke Analysis active")
else:
CycleTime = CycleTimeANT # Seconds, default = 0.25 (inspired by 4Hz ANT+)
#---------------------------------------------------------------------------
# Our main loop!
# The loop has the following phases
# -- Get data from trainer
# -- Local adjustments (heartrate monitor, cadence sensor)
# -- Display actual values
# -- Pedal stroke analysis
# -- Modify data, due to Buttons or ANT
#---------------------------------------------------------------------------
if debug.on(debug.Function): logfile.Write('Tacx2Dongle; start main loop')
try:
while self.RunningSwitch == True and not AntDongle.DongleReconnected:
StartTime = time.time()
#-------------------------------------------------------------------
# ANT process is done once every 250ms
#-------------------------------------------------------------------
if (time.time() - LastANTtime) > 0.25:
LastANTtime = time.time()
QuarterSecond = True
else:
QuarterSecond = False
#-------------------------------------------------------------------
# Get data from trainer (Receive + Calc + Send)
#-------------------------------------------------------------------
TacxTrainer.Refresh(QuarterSecond, usbTrainer.modeResistance)
if clv.gui: self.SetMessages(Tacx=TacxTrainer.Message + PowerModeActive)
#-------------------------------------------------------------------
# If NO Speed Cadence Sensor defined, use Trainer-info
# Hook for future development
#-------------------------------------------------------------------
# if clv.scs == None:
# SpeedKmh = SpeedKmhSCS
# Cadence = CadenceSCS
#-------------------------------------------------------------------
# If NO HRM defined, use the HeartRate from the trainer
#-------------------------------------------------------------------
if clv.hrm == None:
HeartRate = TacxTrainer.HeartRate
# print('Use heartrate from trainer', HeartRate)
#-------------------------------------------------------------------
# Show actual status
#-------------------------------------------------------------------
self.SetValues(TacxTrainer.VirtualSpeedKmh,
TacxTrainer.Cadence, \
TacxTrainer.CurrentPower, \
TacxTrainer.TargetMode, \
TacxTrainer.TargetPower, \
TacxTrainer.TargetGrade, \
TacxTrainer.TargetResistance, \
HeartRate, \
TacxTrainer.TeethFront, \
TacxTrainer.TeethRear)
#-------------------------------------------------------------------
# Pedal Stroke Analysis
#-------------------------------------------------------------------
if clv.PedalStrokeAnalysis:
if LastPedalEcho == 0 and TacxTrainer.PedalEcho == 1 and \
len(pdaInfo) and TacxTrainer.Cadence:
# Pedal triggers cadence sensor
self.PedalStrokeAnalysis(pdaInfo, TacxTrainer.Cadence)
pdaInfo = []
pdaInfo.append((time.time(), TacxTrainer.CurrentPower)) # Store data for analysis
LastPedalEcho = TacxTrainer.PedalEcho # until next signal
#-------------------------------------------------------------------
# In manual-mode, power can be incremented or decremented
# In all modes, operation can be stopped.
#
# TargetMode is set here (manual mode) or received from ANT+ (Zwift)
# TargetPower and TargetGrade are set in this section only!
#-------------------------------------------------------------------
if clv.manual:
if TacxTrainer.Buttons == usbTrainer.EnterButton: pass
elif TacxTrainer.Buttons == usbTrainer.DownButton: TacxTrainer.AddPower(-50)
elif TacxTrainer.Buttons == usbTrainer.OKButton: TacxTrainer.SetPower(100)
elif TacxTrainer.Buttons == usbTrainer.UpButton: TacxTrainer.AddPower( 50)
elif TacxTrainer.Buttons == usbTrainer.CancelButton: self.RunningSwitch = False
else: pass
elif clv.manualGrade:
if TacxTrainer.Buttons == usbTrainer.EnterButton: pass
elif TacxTrainer.Buttons == usbTrainer.DownButton: TacxTrainer.AddGrade(-1)
elif TacxTrainer.Buttons == usbTrainer.OKButton: TacxTrainer.SetGrade(0)
elif TacxTrainer.Buttons == usbTrainer.UpButton: TacxTrainer.AddGrade( 1)
elif TacxTrainer.Buttons == usbTrainer.CancelButton: self.RunningSwitch = False
else: pass
else:
if TacxTrainer.Buttons == usbTrainer.EnterButton: TacxTrainer.FrontShiftUp()
elif TacxTrainer.Buttons == usbTrainer.DownButton: TacxTrainer.RearShiftDown()
elif TacxTrainer.Buttons == usbTrainer.OKButton: pass
elif TacxTrainer.Buttons == usbTrainer.UpButton: TacxTrainer.RearShiftUp()
elif TacxTrainer.Buttons == usbTrainer.CancelButton: TacxTrainer.FrontShiftDown()
else: pass
if ControlCommand == ant.CommandNr_CTRL["MenuUp"]: TacxTrainer.RearShiftUp()
elif ControlCommand == ant.CommandNr_CTRL['MenuDown']: TacxTrainer.RearShiftDown()
elif ControlCommand == ant.CommandNr_CTRL['MenuSelect']: TacxTrainer.FrontShiftToggle()
ControlCommand = ant.CommandNr_CTRL['None']
#-------------------------------------------------------------------
# Do ANT work every 1/4 second
#-------------------------------------------------------------------
messages = [] # messages to be sent to ANT
data = [] # responses received from ANT
if QuarterSecond:
LastANTtime = time.time()
#---------------------------------------------------------------
# Sending i-Vortex messages is done by Refesh() not here
#---------------------------------------------------------------
#---------------------------------------------------------------
# Broadcast Heartrate message
#---------------------------------------------------------------
if clv.hrm == None and TacxTrainer.HeartRate > 0:
messages.append(hrm.BroadcastHeartrateMessage(HeartRate))
#---------------------------------------------------------------
# Broadcast Bike Power message
#---------------------------------------------------------------
if True:
messages.append(pwr.BroadcastMessage( \
TacxTrainer.CurrentPower, TacxTrainer.Cadence))
#---------------------------------------------------------------
# Broadcast Speed and Cadence Sensor message
#---------------------------------------------------------------
if clv.scs == None:
messages.append(scs.BroadcastMessage( \
TacxTrainer.PedalEchoTime, TacxTrainer.PedalEchoCount, \
TacxTrainer.VirtualSpeedKmh, TacxTrainer.Cadence))
#---------------------------------------------------------------
# Broadcast Controllable message
#---------------------------------------------------------------
messages.append(ctrl.BroadcastControlMessage())
#---------------------------------------------------------------
# Broadcast TrainerData message to the CTP (Trainer Road, ...)
#---------------------------------------------------------------
# print('fe.BroadcastTrainerDataMessage', Cadence, CurrentPower, SpeedKmh, HeartRate)
messages.append(fe.BroadcastTrainerDataMessage (TacxTrainer.Cadence, \
TacxTrainer.CurrentPower, TacxTrainer.SpeedKmh, TacxTrainer.HeartRate))
#-------------------------------------------------------------------
# Broadcast and receive ANT+ responses
#-------------------------------------------------------------------
if len(messages) > 0:
data = AntDongle.Write(messages, True, False)
#-------------------------------------------------------------------
# Here all response from the ANT dongle are processed (receive=True)
#
# Commands from dongle that are expected are:
# - TargetGradeFromDongle or TargetPowerFromDongle
# - Information from HRM (if paired)
# - Information from i-Vortex (if paired)
#
# Input is grouped by messageID, then channel. This has little
# practical impact; grouping by Channel would enable to handle all
# ANT in a channel (device) module. No advantage today.
#-------------------------------------------------------------------
for d in data:
synch, length, id, info, checksum, _rest, Channel, DataPageNumber = ant.DecomposeMessage(d)
error = False
if clv.Tacx_iVortex and TacxTrainer.HandleANTmessage(d):
pass # Message is handled or ignored
#---------------------------------------------------------------
# AcknowledgedData = Slave -> Master
# channel_FE = From CTP (Trainer Road, Zwift) --> Tacx
#---------------------------------------------------------------
elif id == ant.msgID_AcknowledgedData:
#-----------------------------------------------------------
# Fitness Equipment Channel inputs
#-----------------------------------------------------------
if Channel == ant.channel_FE:
#-------------------------------------------------------
# Data page 48 (0x30) Basic resistance
#-------------------------------------------------------
if DataPageNumber == 48:
TacxTrainer.SetGrade(ant.msgUnpage48_BasicResistance(info) * 20)
TacxTrainer.SetRollingResistance(0.004)
TacxTrainer.SetWind(0.51, 0.0, 1.0)
p71_LastReceivedCommandID = DataPageNumber
p71_SequenceNr = int(p71_SequenceNr + 1) & 0xff
p71_CommandStatus = 0xff
p71_Data2 = 0xff
p71_Data3 = 0xff
p71_Data4 = 0xff
#-------------------------------------------------------
# Data page 49 (0x31) Target Power
#-------------------------------------------------------
elif DataPageNumber == 49:
TacxTrainer.SetPower(ant.msgUnpage49_TargetPower(info))
TargetPowerTime = time.time()
if False and clv.PowerMode and debug.on(debug.Application):
logfile.Write('PowerMode: TargetPower info received - timestamp set')
p71_LastReceivedCommandID = DataPageNumber
p71_SequenceNr = int(p71_SequenceNr + 1) & 0xff
p71_CommandStatus = 0
p71_Data2 = 0xff
p71_Data3 = int(TacxTrainer.TargetPower) & 0x00ff
p71_Data4 = (int(TacxTrainer.TargetPower) & 0xff00) >> 8
#-------------------------------------------------------
# Data page 50 (0x32) Wind Resistance
#-------------------------------------------------------
elif DataPageNumber == 50:
WindResistance, WindSpeed, DraftingFactor = \
ant.msgUnpage50_WindResistance(info)
TacxTrainer.SetWind(WindResistance, WindSpeed, DraftingFactor)
p71_LastReceivedCommandID = DataPageNumber
p71_SequenceNr = int(p71_SequenceNr + 1) & 0xff
p71_CommandStatus = 0
p71_Data2 = WindResistance
p71_Data3 = WindSpeed
p71_Data4 = DraftingFactor
#-------------------------------------------------------
# Data page 51 (0x33) Track resistance
#-------------------------------------------------------
elif DataPageNumber == 51:
if clv.PowerMode and (time.time() - TargetPowerTime) < 30:
#-----------------------------------------------
# In PowerMode, TrackResistance is ignored
# (for xx seconds after the last power-command)
# So if TrainerRoad is used simultaneously with
# Zwift/Rouvythe power commands from TR
# take precedence over Zwift/Rouvy and a
# power-training can be done while riding
# a Zwift/Rouvy simulation/video!
# When TrainerRoad is finished, the Track
# resistance is active again
#-----------------------------------------------
PowerModeActive = ' [P]'
if False and clv.PowerMode and debug.on(debug.Application):
logfile.Write('PowerMode: Grade info ignored')
pass
else:
Grade, RollingResistance = ant.msgUnpage51_TrackResistance(info)
TacxTrainer.SetGrade(Grade)
TacxTrainer.SetRollingResistance(RollingResistance)
PowerModeActive = ''
p71_LastReceivedCommandID = DataPageNumber
p71_SequenceNr = int(p71_SequenceNr + 1) & 0xff
p71_CommandStatus = 0
p71_Data2 = int(TacxTrainer.TargetPower) & 0x00ff
p71_Data3 = (int(TacxTrainer.TargetPower) & 0xff00) >> 8
p71_Data4 = RollingResistance
#-------------------------------------------------------
# Data page 55 User configuration
#-------------------------------------------------------
elif DataPageNumber == 55:
UserWeight, BicycleWeight, BicycleWheelDiameter, GearRatio = \
ant.msgUnpage55_UserConfiguration(info)
TacxTrainer.SetUserConfiguration(UserWeight, \
BicycleWeight, BicycleWheelDiameter, GearRatio)
#-------------------------------------------------------
# Data page 70 Request data page
#-------------------------------------------------------
elif DataPageNumber == 70:
_SlaveSerialNumber, _DescriptorByte1, _DescriptorByte2, \