├── .gitignore ├── LICENSE ├── README.md ├── src ├── AntStick.cpp ├── AntStick.h ├── FitnessEquipmentControl.cpp ├── FitnessEquipmentControl.h ├── HeartRateMonitor.cpp ├── HeartRateMonitor.h ├── NetTools.cpp ├── NetTools.h ├── TelemetryServer.cpp ├── TelemetryServer.h ├── Tools.cpp ├── Tools.h ├── main.cpp ├── stdafx.cpp ├── stdafx.h └── targetver.h └── vs2017 ├── TrainerControl.sln └── TrainerControl ├── TrainerControl.vcxproj └── TrainerControl.vcxproj.filters /.gitignore: -------------------------------------------------------------------------------- 1 | # Visual Studio files 2 | *.suo 3 | *.ncb 4 | *.db 5 | *.user 6 | *.pdb 7 | *.idb 8 | *.xss 9 | *.xsc 10 | *.xsx 11 | ipch/ 12 | *.sdf 13 | *.opensdf 14 | .vs/ 15 | 16 | # Editor specific files 17 | *.*~ 18 | *.~* 19 | *.swp 20 | 21 | # Git temporary merge files 22 | *.BACKUP.* 23 | *.BASE.* 24 | *.LOCAL.* 25 | *.REMOTE.* 26 | *.orig 27 | *.orig.meta 28 | *.pdb.meta 29 | Thumbs.db.meta 30 | 31 | # oother 32 | *.exe 33 | *.ilk 34 | Debug/ 35 | Release/ 36 | x86/ 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Read data and control a bike trainer 2 | 3 | This is a server application allowing to read data from ANT+ devices and 4 | sending them over to a TCP connection. It can currently read heart rate from 5 | an ANT+ HRM and read power, speed and cadence from an ANT+ FE-C trainer (most 6 | recent trainers support this). It can also control the resistance of the 7 | trainer by setting the slope. 8 | 9 | This application is written as back end server for a 10 | [Racket](https://www.racket-lang.org) based front end, as described in [this 11 | blog post](https://alex-hhh.github.io/2017/11/bike-trainer.html). This is a 12 | prototype application for a hobby project, and as such: 13 | 14 | * It runs on Windows only, using Visual Studio 2017 to compile it. While it 15 | could be ported to Linux easily, I have no short term plans of doing so. 16 | * It uses C++ 17 features and there are no plans to support older C++ 17 | compilers and standards. 18 | * The network protocol for the telemetry is only intended for the front end 19 | application and may change at any time, with no backwards compatibility 20 | considerations. 21 | 22 | ## Building the application 23 | 24 | The application is built on a Windows platform using Visual Studio 2017 (the 25 | Community Edition will work fine). It could be easily ported to Linux as 26 | well, but this hasn't been done yet. 27 | 28 | You will need to install **libusb** using 29 | [vcpkg](https://github.com/Microsoft/vcpkg), see that projects instructions on 30 | how to install it. 31 | 32 | Open the `vs2017/TrainerControl.sln` solution and build it (if you opened it 33 | before setting up the environment variables, you will need to re-open it). 34 | 35 | The resulting executable will be in the `Debug` or `Release` folder. 36 | 37 | ## Running the application 38 | 39 | To run the application, open a command window and type: 40 | 41 | ./TrainerControl.exe 42 | 43 | The application will try to find the ANT+ USB stick and connect to the heart 44 | rate monitor and bike trainer. It will also accept TCP connections on port 45 | 7500. 46 | -------------------------------------------------------------------------------- /src/AntStick.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * AntStick -- communicate with an ANT+ USB stick 3 | * Copyright (C) 2017, 2018 Alex Harsanyi 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include "AntStick.h" 20 | #include "Tools.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include "winsock2.h" // for struct timeval 30 | 31 | #ifdef WIN32 32 | #pragma comment (lib, "libusb-1.0.lib") 33 | #endif 34 | 35 | /** IMPLEMENTATION NOTE 36 | * 37 | * The ANT Message Protocol implemented here is documented in the "ANT Message 38 | * Protocol And Usage" document available from https://www.thisisant.com/ 39 | * 40 | * D00000652_ANT_Message_Protocol_and_Usage_Rev_5.1.pdf 41 | * 42 | * Also see the "ANT+ Common Pages" document from the same place. 43 | * 44 | * D00001198_-_ANT+_Common_Data_Pages_Rev_3.1 45 | */ 46 | 47 | // When this is defined, some debug info is printed to std::cerr 48 | // #define DEBUG_OUTPUT 1 49 | 50 | namespace { 51 | 52 | enum ChannelType { 53 | BIDIRECTIONAL_RECEIVE = 0x00, 54 | BIDIRECTIONAL_TRANSMIT = 0x10, 55 | 56 | SHARED_BIDIRECTIONAL_RECEIVE = 0x20, 57 | SHARED_BIDIRECTIONAL_TRANSMIT = 0x30, 58 | 59 | UNIDIRECTIONAL_RECEIVE_ONLY = 0x40, 60 | UNIDIRECTIONAL_TRANSMIT_ONLY = 0x50 61 | }; 62 | 63 | void CheckChannelResponse ( 64 | const Buffer &response, uint8_t channel, uint8_t cmd, uint8_t status) 65 | { 66 | if (response.size() < 5) 67 | { 68 | if (! std::uncaught_exception()) 69 | throw std::runtime_error ("CheckChannelResponse: short response"); 70 | } 71 | else if (response[2] != CHANNEL_RESPONSE 72 | || response[3] != channel 73 | || response[4] != cmd 74 | || response[5] != status) 75 | { 76 | #if defined DEBUG_OUTPUT 77 | DumpData(&response[0], response.size(), std::cerr); 78 | std::cerr << "expecting channel: " << (int)channel 79 | << ", cmd " << (int)cmd << ", status " << (int)status << "\n"; 80 | #endif 81 | // Funny thing: this function is also called from a destructor while 82 | // an exception is being unwound. Don't cause further trouble... 83 | if (! std::uncaught_exception()) 84 | throw std::runtime_error ("CheckChannelResponse: bad response"); 85 | } 86 | } 87 | 88 | void AddMessageChecksum (Buffer &b) 89 | { 90 | uint8_t c = 0; 91 | std::for_each (b.begin(), b.end(), [&](uint8_t e) { c ^= e; }); 92 | b.push_back (c); 93 | } 94 | 95 | bool IsGoodChecksum (const Buffer &message) 96 | { 97 | uint8_t c = 0; 98 | std::for_each (message.begin(), message.end(), [&](uint8_t e) { c ^= e; }); 99 | return c == 0; 100 | } 101 | 102 | Buffer MakeMessage (AntMessageId id, uint8_t data) 103 | { 104 | Buffer b; 105 | b.push_back (SYNC_BYTE); 106 | b.push_back (0x01); // data length 107 | b.push_back (static_cast(id)); 108 | b.push_back (data); 109 | AddMessageChecksum (b); 110 | return b; 111 | } 112 | 113 | Buffer MakeMessage (AntMessageId id, uint8_t data0, uint8_t data1) 114 | { 115 | Buffer b; 116 | b.push_back (SYNC_BYTE); 117 | b.push_back (0x02); // data length 118 | b.push_back (static_cast(id)); 119 | b.push_back (data0); 120 | b.push_back (data1); 121 | AddMessageChecksum (b); 122 | return b; 123 | } 124 | 125 | Buffer MakeMessage (AntMessageId id, 126 | uint8_t data0, uint8_t data1, uint8_t data2) 127 | { 128 | Buffer b; 129 | b.push_back (SYNC_BYTE); 130 | b.push_back (0x03); // data length 131 | b.push_back (static_cast(id)); 132 | b.push_back (data0); 133 | b.push_back (data1); 134 | b.push_back (data2); 135 | AddMessageChecksum (b); 136 | return b; 137 | } 138 | 139 | Buffer MakeMessage (AntMessageId id, 140 | uint8_t data0, uint8_t data1, uint8_t data2, 141 | uint8_t data3, uint8_t data4) 142 | { 143 | Buffer b; 144 | b.push_back (SYNC_BYTE); 145 | b.push_back (0x05); // data length 146 | b.push_back (static_cast(id)); 147 | b.push_back (data0); 148 | b.push_back (data1); 149 | b.push_back (data2); 150 | b.push_back (data3); 151 | b.push_back (data4); 152 | AddMessageChecksum (b); 153 | return b; 154 | } 155 | 156 | Buffer MakeMessage (AntMessageId id, Buffer data) 157 | { 158 | Buffer b; 159 | b.push_back (SYNC_BYTE); 160 | b.push_back (static_cast(data.size())); 161 | b.push_back (static_cast(id)); 162 | b.insert (b.end(), data.begin(), data.end()); 163 | AddMessageChecksum (b); 164 | return b; 165 | } 166 | 167 | Buffer MakeMessage (AntMessageId id, uint8_t data0, const Buffer &data) 168 | { 169 | Buffer b; 170 | b.push_back (SYNC_BYTE); 171 | b.push_back (static_cast(data.size() + 1)); 172 | b.push_back (static_cast(id)); 173 | b.push_back (data0); 174 | b.insert (b.end(), data.begin(), data.end()); 175 | AddMessageChecksum (b); 176 | return b; 177 | } 178 | 179 | Buffer MakeMessage (AntMessageId id, uint8_t data0, uint8_t data1, const Buffer &data) 180 | { 181 | Buffer b; 182 | b.push_back (SYNC_BYTE); 183 | b.push_back (static_cast(data.size() + 2)); 184 | b.push_back (static_cast(id)); 185 | b.push_back (data0); 186 | b.push_back(data1); 187 | b.insert (b.end(), data.begin(), data.end()); 188 | AddMessageChecksum (b); 189 | return b; 190 | } 191 | 192 | struct ChannelEventName { 193 | AntChannelEvent event; 194 | const char *text; 195 | } g_ChanelEventNames[] = { 196 | { RESPONSE_NO_ERROR, "no error" }, 197 | { EVENT_RX_SEARCH_TIMEOUT, "channel search timeout" }, 198 | { EVENT_RX_FAIL, "rx fail" }, 199 | { EVENT_TX, "broadcast tx complete" }, 200 | { EVENT_TRANSFER_RX_FAILED, "rx transfer fail" }, 201 | { EVENT_TRANSFER_TX_COMPLETED, "tx complete" }, 202 | { EVENT_TRANSFER_TX_FAILED, "tx fail" }, 203 | { EVENT_CHANNEL_CLOSED, "channel closed" }, 204 | { EVENT_RX_FAIL_GO_TO_SEARCH, "dropped to search mode" }, 205 | { EVENT_CHANNEL_COLLISION, "channel collision" }, 206 | { EVENT_TRANSFER_TX_START, "burst transfer start" }, 207 | { EVENT_TRANSFER_NEXT_DATA_BLOCK, "burst next data block" }, 208 | { CHANNEL_IN_WRONG_STATE, "channel in wrong state" }, 209 | { CHANNEL_NOT_OPENED, "channel not opened" }, 210 | { CHANNEL_ID_NOT_SET, "channel id not set" }, 211 | { CLOSE_ALL_CHANNELS, "all channels closed" }, 212 | { TRANSFER_IN_PROGRESS, "transfer in progress" }, 213 | { TRANSFER_SEQUENCE_NUMBER_ERROR, "transfer sequence error" }, 214 | { TRANSFER_IN_ERROR, "burst transfer error" }, 215 | { MESSAGE_SIZE_EXCEEDS_LIMIT, "message too big" }, 216 | { INVALID_MESSAGE, "invalid message" }, 217 | { INVALID_NETWORK_NUMBER, "invalid network number" }, 218 | { INVALID_LIST_ID, "invalid list id" }, 219 | { INVALID_SCAN_TX_CHANNEL, "attempt to transmit in ANT channel 0 in scan mode" }, 220 | { INVALID_PARAMETER_PROVIDED, "invalid parameter" }, 221 | { EVENT_SERIAL_QUE_OVERFLOW, "output serial overflow" }, 222 | { EVENT_QUE_OVERFLOW, "input serial overflow" }, 223 | { ENCRYPT_NEGOTIATION_SUCCESS, "encrypt negotiation success" }, 224 | { ENCRYPT_NEGOTIATION_FAIL, "encrypt negotiation fail" }, 225 | { NVM_FULL_ERROR, "nvm full" }, 226 | { NVM_WRITE_ERROR, "nvm write fail" }, 227 | { USB_STRING_WRITE_FAIL, "usb write fail" }, 228 | { MESG_SERIAL_ERROR_ID, "bad usb packet received" }, 229 | { LAST_EVENT_ID, nullptr}}; 230 | 231 | std::ostream& operator<< (std::ostream &out, const AntChannel::Id &id) 232 | { 233 | out << "#"; 234 | return out; 235 | } 236 | 237 | }; // end anonymous namespace 238 | 239 | const char *ChannelEventAsString(AntChannelEvent e) 240 | { 241 | for (int i = 0; g_ChanelEventNames[i].event != LAST_EVENT_ID; i++) { 242 | if (g_ChanelEventNames[i].event == e) 243 | return g_ChanelEventNames[i].text; 244 | } 245 | return "unknown channel event"; 246 | } 247 | 248 | 249 | // ................................................... AntMessageReader .... 250 | 251 | /** Read ANT messages from an USB device (the ANT stick) */ 252 | class AntMessageReader 253 | { 254 | public: 255 | AntMessageReader (libusb_device_handle *dh, uint8_t endpoint); 256 | ~AntMessageReader(); 257 | 258 | void MaybeGetNextMessage(Buffer &message); 259 | void GetNextMessage (Buffer &message); 260 | 261 | private: 262 | 263 | void GetNextMessage1(Buffer &message); 264 | 265 | static void LIBUSB_CALL Trampoline (libusb_transfer *); 266 | void SubmitUsbTransfer(); 267 | void CompleteUsbTransfer(const libusb_transfer *); 268 | 269 | libusb_device_handle *m_DeviceHandle; 270 | uint8_t m_Endpoint; 271 | libusb_transfer *m_Transfer; 272 | 273 | /** Hold partial data received from the USB stick. A single USB read 274 | * might not return an entire ANT message. */ 275 | Buffer m_Buffer; 276 | unsigned m_Mark; // buffer position up to where data is available 277 | bool m_Active; // is there a transfer active? 278 | }; 279 | 280 | 281 | AntMessageReader::AntMessageReader (libusb_device_handle *dh, uint8_t endpoint) 282 | : m_DeviceHandle (dh), 283 | m_Endpoint (endpoint), 284 | m_Transfer (nullptr), 285 | m_Mark(0), 286 | m_Active (false) 287 | { 288 | m_Buffer.reserve (1024); 289 | m_Transfer = libusb_alloc_transfer (0); 290 | } 291 | 292 | AntMessageReader::~AntMessageReader() 293 | { 294 | if (m_Active) { 295 | libusb_cancel_transfer (m_Transfer); 296 | } 297 | while (m_Active) { 298 | libusb_handle_events(nullptr); 299 | } 300 | libusb_free_transfer (m_Transfer); 301 | } 302 | 303 | /** Fill `message' with the next available message. If no message is received 304 | * within a small amount of time, an empty buffer is returned. If a message 305 | * is returned, it is a valid message (good header, length and checksum). 306 | */ 307 | void AntMessageReader::MaybeGetNextMessage (Buffer &message) 308 | { 309 | message.clear(); 310 | 311 | if (m_Active) { 312 | struct timeval tv; 313 | tv.tv_sec = 0; 314 | tv.tv_usec = 10 * 1000; 315 | int r = libusb_handle_events_timeout_completed (nullptr, &tv, nullptr); 316 | if (r < 0) 317 | throw LibusbError ("libusb_handle_events_timeout_completed", r); 318 | } 319 | 320 | if(! m_Active) { 321 | // a transfer was completed, see if we have a full message. 322 | GetNextMessage1(message); 323 | } 324 | } 325 | 326 | 327 | /** Fill `message' with the next available message. If a message is returned, 328 | * it is a valid message (good header, length and checksum). If no message is 329 | * received within a small amount of time, a timeout exception will be 330 | * thrown. 331 | */ 332 | void AntMessageReader::GetNextMessage(Buffer &message) 333 | { 334 | MaybeGetNextMessage(message); 335 | int tries = 100; 336 | while (message.empty() && tries > 0) 337 | { 338 | MaybeGetNextMessage(message); 339 | tries--; 340 | } 341 | if (message.empty()) 342 | throw std::runtime_error ("AntMessageReader::GetNextMessage: timed out"); 343 | } 344 | 345 | void AntMessageReader::GetNextMessage1(Buffer &message) 346 | { 347 | // Cannot operate on the buffer while a transfer is active 348 | assert (! m_Active); 349 | 350 | // In case the CompleteUsbTransfer was not called... 351 | m_Buffer.erase(m_Buffer.begin() + m_Mark, m_Buffer.end()); 352 | 353 | // Look for the sync byte which starts a message 354 | while ((! m_Buffer.empty()) && m_Buffer[0] != SYNC_BYTE) { 355 | m_Buffer.erase(m_Buffer.begin()); 356 | m_Mark--; 357 | } 358 | 359 | // An ANT message has the following sequence: SYNC, LEN, MSGID, DATA, 360 | // CHECKSUM. An empty message has at least 4 bytes in it. 361 | if (m_Mark < 4) { 362 | SubmitUsbTransfer(); 363 | return MaybeGetNextMessage(message); 364 | } 365 | 366 | // LEN is the length of the data, actual message length is LEN + 4. 367 | unsigned len = m_Buffer[1] + 4; 368 | 369 | if (m_Mark < len) { 370 | SubmitUsbTransfer(); 371 | return MaybeGetNextMessage(message); 372 | } 373 | 374 | std::copy(m_Buffer.begin(), m_Buffer.begin() + len, std::back_inserter(message)); 375 | // Remove the message from the buffer. 376 | m_Buffer.erase (m_Buffer.begin(), m_Buffer.begin() + len); 377 | m_Mark -= len; 378 | 379 | if (! IsGoodChecksum (message)) 380 | throw std::runtime_error ("AntMessageReader::GetNextMessage1: bad checksum"); 381 | } 382 | 383 | void LIBUSB_CALL AntMessageReader::Trampoline (libusb_transfer *t) 384 | { 385 | AntMessageReader *a = reinterpret_cast(t->user_data); 386 | a->CompleteUsbTransfer (t); 387 | } 388 | 389 | void AntMessageReader::SubmitUsbTransfer() 390 | { 391 | assert (! m_Active); 392 | 393 | const int read_size = 128; 394 | const int timeout = 10000; 395 | 396 | // Make sure we have enough space in the buffer. 397 | m_Buffer.resize (m_Mark + read_size); 398 | 399 | libusb_fill_bulk_transfer ( 400 | m_Transfer, m_DeviceHandle, m_Endpoint, 401 | &m_Buffer[m_Mark], read_size, Trampoline, this, timeout); 402 | 403 | int r = libusb_submit_transfer (m_Transfer); 404 | if (r < 0) 405 | throw LibusbError ("libusb_submit_transfer", r); 406 | 407 | m_Active = true; 408 | } 409 | 410 | void AntMessageReader::CompleteUsbTransfer(const libusb_transfer *t) 411 | { 412 | assert(t == m_Transfer); 413 | 414 | m_Active = false; 415 | 416 | if (m_Transfer->status == LIBUSB_TRANSFER_COMPLETED) { 417 | m_Mark += m_Transfer->actual_length; 418 | } 419 | 420 | m_Buffer.erase(m_Buffer.begin() + m_Mark, m_Buffer.end()); 421 | } 422 | 423 | 424 | 425 | // ................................................... AntMessageWriter .... 426 | 427 | /** Write ANT messages to a USB device (the ANT stick). */ 428 | class AntMessageWriter 429 | { 430 | public: 431 | AntMessageWriter (libusb_device_handle *dh, uint8_t endpoint); 432 | ~AntMessageWriter(); 433 | 434 | void WriteMessage (const Buffer &message); 435 | 436 | private: 437 | 438 | static void LIBUSB_CALL Trampoline (libusb_transfer *); 439 | void SubmitUsbTransfer(const Buffer &message, int timeout); 440 | void CompleteUsbTransfer(const libusb_transfer *); 441 | void WaitForCompletion(int timeout); 442 | 443 | libusb_device_handle *m_DeviceHandle; 444 | uint8_t m_Endpoint; 445 | libusb_transfer *m_Transfer; 446 | 447 | Buffer m_Buffer; 448 | bool m_Active; // is there a transfer active? 449 | }; 450 | 451 | 452 | AntMessageWriter::AntMessageWriter (libusb_device_handle *dh, uint8_t endpoint) 453 | : m_DeviceHandle (dh), m_Endpoint (endpoint), m_Transfer (nullptr), m_Active (false) 454 | { 455 | m_Transfer = libusb_alloc_transfer (0); 456 | } 457 | 458 | AntMessageWriter::~AntMessageWriter() 459 | { 460 | if (m_Active) { 461 | libusb_cancel_transfer (m_Transfer); 462 | } 463 | WaitForCompletion(2000); 464 | libusb_free_transfer (m_Transfer); 465 | } 466 | 467 | /** Write `message' to the USB device. This is presumably an ANT message, but 468 | * we don't check. When this function returns, the message has been written 469 | * (there is no buffering on the application side). An exception is thrown if 470 | * there is an error or a timeout. 471 | */ 472 | void AntMessageWriter::WriteMessage (const Buffer &message) 473 | { 474 | assert (! m_Active); 475 | SubmitUsbTransfer(message, 2000 /* milliseconds */); 476 | WaitForCompletion(2000 /* milliseconds */); 477 | 478 | if (m_Transfer->status == LIBUSB_TRANSFER_COMPLETED) { 479 | m_Active = false; // sometimes CompleteUsbTransfer() is not called, not sure why... 480 | } 481 | 482 | if (m_Active || m_Transfer->status != LIBUSB_TRANSFER_COMPLETED) { 483 | m_Active = false; 484 | int r = 0; 485 | 486 | if (m_Transfer->status == LIBUSB_TRANSFER_STALL) { 487 | r = libusb_clear_halt(m_DeviceHandle, m_Endpoint); 488 | if (r < 0) { 489 | throw LibusbError("libusb_clear_halt", r); 490 | } 491 | } 492 | 493 | throw LibusbError("AntMessageReader", m_Transfer->status); 494 | } 495 | } 496 | 497 | void LIBUSB_CALL AntMessageWriter::Trampoline (libusb_transfer *t) 498 | { 499 | AntMessageWriter *a = reinterpret_cast(t->user_data); 500 | a->CompleteUsbTransfer (t); 501 | } 502 | 503 | void AntMessageWriter::SubmitUsbTransfer(const Buffer &message, int timeout) 504 | { 505 | m_Buffer = message; 506 | 507 | libusb_fill_bulk_transfer ( 508 | m_Transfer, m_DeviceHandle, m_Endpoint, 509 | &m_Buffer[0], m_Buffer.size(), Trampoline, this, timeout); 510 | 511 | int r = libusb_submit_transfer (m_Transfer); 512 | if (r < 0) 513 | throw LibusbError ("libusb_submit_transfer", r); 514 | 515 | m_Active = true; 516 | } 517 | 518 | void AntMessageWriter::CompleteUsbTransfer(const libusb_transfer *t) 519 | { 520 | assert (t == m_Transfer); 521 | m_Active = false; 522 | } 523 | 524 | void AntMessageWriter::WaitForCompletion(int timeout) 525 | { 526 | using namespace std::chrono; 527 | 528 | struct timeval tv; 529 | tv.tv_sec = timeout / 1000; 530 | tv.tv_usec = (timeout - tv.tv_sec * 1000) * 1000; 531 | auto start = high_resolution_clock::now(); 532 | milliseconds accumulated; 533 | 534 | // NOTE: libusb_handle_events and friends will handle all USB events, but 535 | // not necessarily our event, as such they might return before our 536 | // transfer is complete. We wait repeatedly until our event is complete 537 | // or 'timeout' milliseconds have passed. 538 | 539 | while (m_Active && accumulated.count() < timeout) 540 | { 541 | int r = libusb_handle_events_timeout_completed (nullptr, &tv, nullptr); 542 | if (r < 0) 543 | throw LibusbError ("libusb_handle_events_timeout_completed", r); 544 | auto now = high_resolution_clock::now(); 545 | accumulated = duration_cast(now - start); 546 | } 547 | } 548 | 549 | 550 | // ......................................................... AntChannel .... 551 | 552 | AntChannel::AntChannel (AntStick *stick, 553 | AntChannel::Id channel_id, 554 | unsigned period, 555 | uint8_t timeout, 556 | uint8_t frequency) 557 | : m_Stick (stick), 558 | m_IdReqestOutstanding (false), 559 | m_AckDataRequestOutstanding(false), 560 | m_ChannelId(channel_id), 561 | m_MessagesReceived(0), 562 | m_MessagesFailed(0) 563 | { 564 | m_ChannelNumber = stick->NextChannelId(); 565 | 566 | if (m_ChannelNumber == -1) 567 | throw std::runtime_error("AntChannel: no more channel ids left"); 568 | 569 | // we hard code the type to BIDIRECTIONAL_RECEIVE, using other channel 570 | // types would require changes to the handling code anyway. 571 | m_Stick->WriteMessage ( 572 | MakeMessage ( 573 | ASSIGN_CHANNEL, m_ChannelNumber, 574 | static_cast(BIDIRECTIONAL_RECEIVE), 575 | static_cast(m_Stick->GetNetwork()))); 576 | Buffer response = m_Stick->ReadInternalMessage(); 577 | CheckChannelResponse (response, m_ChannelNumber, ASSIGN_CHANNEL, 0); 578 | 579 | m_Stick->WriteMessage( 580 | MakeMessage(SET_CHANNEL_ID, m_ChannelNumber, 581 | static_cast(m_ChannelId.DeviceNumber & 0xFF), 582 | static_cast((m_ChannelId.DeviceNumber >> 8) & 0xFF), 583 | m_ChannelId.DeviceType, 584 | // High nibble of the transmission_type is the top 4 bits 585 | // of the 20 bit device id. 586 | static_cast((m_ChannelId.DeviceNumber >> 12) & 0xF0))); 587 | response = m_Stick->ReadInternalMessage(); 588 | CheckChannelResponse (response, m_ChannelNumber, SET_CHANNEL_ID, 0); 589 | 590 | Configure(period, timeout, frequency); 591 | 592 | m_Stick->WriteMessage ( 593 | MakeMessage (OPEN_CHANNEL, m_ChannelNumber)); 594 | response = m_Stick->ReadInternalMessage(); 595 | CheckChannelResponse (response, m_ChannelNumber, OPEN_CHANNEL, 0); 596 | 597 | m_State = CH_SEARCHING; 598 | m_Stick->RegisterChannel (this); 599 | } 600 | 601 | AntChannel::~AntChannel() 602 | { 603 | try { 604 | // The user has not called RequestClose(), try to close the channel 605 | // now, but this might fail. 606 | if (m_State != CH_CLOSED) { 607 | m_Stick->WriteMessage (MakeMessage (CLOSE_CHANNEL, m_ChannelNumber)); 608 | Buffer response = m_Stick->ReadInternalMessage(); 609 | CheckChannelResponse (response, m_ChannelNumber, CLOSE_CHANNEL, 0); 610 | 611 | // The channel has to respond with an EVENT_CHANNEL_CLOSED channel 612 | // event, but we cannot process that (it would go through 613 | // Tick(). We wait at least for the event to be generated. 614 | Sleep(0); 615 | 616 | m_Stick->WriteMessage (MakeMessage (UNASSIGN_CHANNEL, m_ChannelNumber)); 617 | response = m_Stick->ReadInternalMessage(); 618 | CheckChannelResponse (response, m_ChannelNumber, UNASSIGN_CHANNEL, 0); 619 | } 620 | } 621 | catch (std::exception &) { 622 | // discard it 623 | } 624 | 625 | m_Stick->UnregisterChannel (this); 626 | } 627 | 628 | /** Request this channel to close. Closing the channel involves receiving a 629 | * status message back, so HandleMessage() still has to be called with chanel 630 | * messages until IsOpen() returns false. 631 | */ 632 | void AntChannel::RequestClose() 633 | { 634 | m_Stick->WriteMessage (MakeMessage (CLOSE_CHANNEL, m_ChannelNumber)); 635 | Buffer response = m_Stick->ReadInternalMessage(); 636 | CheckChannelResponse (response, m_ChannelNumber, CLOSE_CHANNEL, 0); 637 | } 638 | 639 | void AntChannel::SendAcknowledgedData(int tag, const Buffer &message) 640 | { 641 | m_AckDataQueue.push(AckDataItem(tag, message)); 642 | } 643 | 644 | void AntChannel::RequestDataPage(uint8_t page_id, int transmit_count) 645 | { 646 | const uint8_t DP_DP_REQUEST = 0x46; 647 | 648 | Buffer msg; 649 | msg.push_back(DP_DP_REQUEST); 650 | msg.push_back(0xFF); // slave serial LSB 651 | msg.push_back(0xFF); // slave serial MSB 652 | msg.push_back(0xFF); // descriptor 1 653 | msg.push_back(0xFF); // descriptor 2 654 | // number of times we ask the slave to transmit the data page (if it is 655 | // lost due to channel collisions the slave won't care) 656 | msg.push_back(transmit_count); 657 | msg.push_back(page_id); 658 | msg.push_back(0x01); // command type: 0x01 request data page 659 | SendAcknowledgedData(page_id, msg); 660 | } 661 | 662 | /** Configure communication parameters for the channel 663 | */ 664 | void AntChannel::Configure (unsigned period, uint8_t timeout, uint8_t frequency) 665 | { 666 | m_Stick->WriteMessage ( 667 | MakeMessage (SET_CHANNEL_PERIOD, m_ChannelNumber, period & 0xFF, (period >> 8) & 0xff)); 668 | Buffer response = m_Stick->ReadInternalMessage(); 669 | CheckChannelResponse (response, m_ChannelNumber, SET_CHANNEL_PERIOD, 0); 670 | 671 | m_Stick->WriteMessage ( 672 | MakeMessage (SET_CHANNEL_SEARCH_TIMEOUT, m_ChannelNumber, timeout)); 673 | response = m_Stick->ReadInternalMessage(); 674 | CheckChannelResponse (response, m_ChannelNumber, SET_CHANNEL_SEARCH_TIMEOUT, 0); 675 | 676 | m_Stick->WriteMessage ( 677 | MakeMessage (SET_CHANNEL_RF_FREQ, m_ChannelNumber, frequency)); 678 | response = m_Stick->ReadInternalMessage(); 679 | CheckChannelResponse(response, m_ChannelNumber, SET_CHANNEL_RF_FREQ, 0); 680 | } 681 | 682 | /** Called by the AntStick::Tick method to process a message received on this 683 | * channel. This will look for some channel events, and process them, but 684 | * delegate most of the messages to ProcessMessage() in the derived class. 685 | */ 686 | void AntChannel::HandleMessage(const uint8_t *data, int size) 687 | { 688 | if (m_State == CH_CLOSED) { 689 | // We should not receive messages if we are closed, maybe we should 690 | // throw? 691 | #if defined DEBUG_OUTPUT 692 | std::cerr << "AntChannel::HandleMessage -- received a message while closed\n"; 693 | DumpData(data, size, std::cerr); 694 | #endif 695 | return; 696 | } 697 | 698 | switch (data[2]) { 699 | case CHANNEL_RESPONSE: 700 | OnChannelResponseMessage (data, size); 701 | break; 702 | case BROADCAST_DATA: 703 | if (m_ChannelId.DeviceNumber == 0 && ! m_IdReqestOutstanding) 704 | { 705 | // We received a broadcast message on this channel and we don't 706 | // have a master serial number, find out who is sending us 707 | // broadcast data 708 | m_Stick->WriteMessage ( 709 | MakeMessage (REQUEST_MESSAGE, m_ChannelNumber, SET_CHANNEL_ID)); 710 | m_IdReqestOutstanding = true; 711 | } 712 | MaybeSendAckData(); 713 | OnMessageReceived(data, size); 714 | m_MessagesReceived++; 715 | break; 716 | case RESPONSE_CHANNEL_ID: 717 | OnChannelIdMessage (data, size); 718 | break; 719 | default: 720 | OnMessageReceived (data, size); 721 | break; 722 | } 723 | } 724 | 725 | /** Send an ACKNOWLEDGE_DATA message, if we have one to send and there are no 726 | * outstanding ones. 727 | */ 728 | void AntChannel::MaybeSendAckData() 729 | { 730 | if (! m_AckDataRequestOutstanding && ! m_AckDataQueue.empty()) { 731 | const AckDataItem &item = m_AckDataQueue.front(); 732 | m_Stick->WriteMessage(MakeMessage(ACKNOWLEDGE_DATA, m_ChannelNumber, item.data)); 733 | m_AckDataRequestOutstanding = true; 734 | } 735 | } 736 | 737 | /** Process a channel response message. 738 | */ 739 | void AntChannel::OnChannelResponseMessage (const uint8_t *data, int size) 740 | { 741 | assert(data[2] == CHANNEL_RESPONSE); 742 | 743 | auto msg_id = data[4]; 744 | auto event = static_cast(data[5]); 745 | // msg_id should be 1 if it is a general event and an message ID if it 746 | // is a response to an channel message we sent previously. We don't 747 | // expect chanel responses here 748 | if (msg_id == 1) 749 | { 750 | if (event == EVENT_RX_FAIL) 751 | m_MessagesFailed++; 752 | 753 | if (event == EVENT_RX_SEARCH_TIMEOUT) { 754 | // ignore it, we are closed, but we need to wait for the closed 755 | // message 756 | } 757 | else if (event == EVENT_CHANNEL_CLOSED) { 758 | // NOTE: a search timeout will close the channel. 759 | if (m_State != CH_CLOSED) { 760 | ChangeState(CH_CLOSED); 761 | m_Stick->WriteMessage(MakeMessage(UNASSIGN_CHANNEL, m_ChannelNumber)); 762 | Buffer response = m_Stick->ReadInternalMessage(); 763 | CheckChannelResponse(response, m_ChannelNumber, UNASSIGN_CHANNEL, 0); 764 | } 765 | return; 766 | } 767 | else if (event == EVENT_RX_FAIL_GO_TO_SEARCH) { 768 | m_ChannelId.DeviceNumber = 0; // lost our device 769 | ChangeState(CH_SEARCHING); 770 | } 771 | else if (event == RESPONSE_NO_ERROR) { 772 | // we seem to be getting these from time to time, ignore them 773 | } 774 | else if (m_AckDataRequestOutstanding) { 775 | // We received a status for a ACKNOWLEDGE_DATA transmission 776 | auto tag = m_AckDataQueue.front().tag; 777 | m_AckDataQueue.pop(); 778 | m_AckDataRequestOutstanding = false; 779 | OnAcknowledgedDataReply(tag, event); 780 | } 781 | else { 782 | #if defined DEBUG_OUTPUT 783 | std::cerr << "Got unexpected channel event " << (unsigned)event << ": " 784 | << ChannelEventAsString (event) << "\n"; 785 | #endif 786 | } 787 | } else { 788 | #if defined DEBUG_OUTPUT 789 | std::cerr << "Unexpected reply for command " << (unsigned)msg_id 790 | << ": " << (unsigned) event << " " << ChannelEventAsString (event) 791 | << std::endl; 792 | #endif 793 | } 794 | 795 | return; 796 | } 797 | 798 | /** Process a RESPONSE_CHANNEL_ID message. We ask for one when we receive a 799 | * broadcast data and we used it to identify the master device we are paired 800 | * with. 801 | */ 802 | void AntChannel::OnChannelIdMessage (const uint8_t *data, int size) 803 | { 804 | assert(data[2] == RESPONSE_CHANNEL_ID); 805 | 806 | // we asked for this when we received the first broadcast message on the 807 | // channel. Normally this would be a logic error in the program (and 808 | // therefore it should be an assert(), but this is a packet we received 809 | // from the AntStick...) 810 | if (data[3] != m_ChannelNumber) { 811 | throw std::runtime_error ("AntChannel::OnChannelIdMessage: unexpected channel number"); 812 | } 813 | 814 | auto transmission_type = static_cast(data[7] & 0x03); 815 | // note: high nibble of the transmission type byte represents the 816 | // extended 20bit device number 817 | uint16_t device_number = data[4] | (data[5] << 8) | ((data[7] >> 4) & 0x0F) << 16; 818 | uint8_t device_type = data[6]; 819 | 820 | if (m_ChannelId.DeviceType == 0) { 821 | m_ChannelId.DeviceType = device_type; 822 | } else if (m_ChannelId.DeviceType != device_type) { 823 | // we seem to have paired up with a different device type than we 824 | // asked for... 825 | throw std::runtime_error ("AntChannel::OnChannelIdMessage: unexpected device type"); 826 | } 827 | 828 | if (m_ChannelId.DeviceNumber == 0) { 829 | m_ChannelId.DeviceNumber = device_number; 830 | } else if (m_ChannelId.DeviceNumber != device_number) { 831 | // we seem to have paired up with a different device than we asked 832 | // for... 833 | throw std::runtime_error ("AntChannel::OnChannelIdMessage: unexpected device number"); 834 | } 835 | 836 | // NOTE: fist channel id responses might not contain a message ID. 837 | if (m_ChannelId.DeviceNumber != 0) { 838 | ChangeState(CH_OPEN); 839 | #if defined DEBUG_OUTPUT 840 | std::cerr << "Got a device number: " << m_ChannelId.DeviceNumber << "\n"; 841 | #endif 842 | } 843 | 844 | m_IdReqestOutstanding = false; 845 | } 846 | 847 | /** Change the channel state to 'new_state' and call OnStateChanged() if the 848 | * state has actually changed. 849 | */ 850 | void AntChannel::ChangeState(State new_state) 851 | { 852 | if (m_State != new_state) { 853 | OnStateChanged(m_State, new_state); 854 | m_State = new_state; 855 | } 856 | } 857 | 858 | void AntChannel::OnStateChanged (State old_state, State new_state) 859 | { 860 | // do nothing. An implementation is provided so any derived classes not 861 | // interested in state changes can just ignore this. 862 | } 863 | 864 | void AntChannel::OnAcknowledgedDataReply(int tag, AntChannelEvent event) 865 | { 866 | // do nothing. An implementation is provided so any derived classes not 867 | // interested in ack data replies can just ignore this. 868 | } 869 | 870 | 871 | // ........................................................... AntStick .... 872 | 873 | const char * AntStickNotFound::what() const /*noexcept*/ 874 | { 875 | return "USB ANT stick not found"; 876 | } 877 | 878 | uint8_t AntStick::g_AntPlusNetworkKey[8] = { 879 | 0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45 880 | }; 881 | 882 | // ANT+ memory sticks vendor and product ids. We will use the first USB 883 | // device found. 884 | struct ant_stick_devid_ { 885 | int vid; 886 | int pid; 887 | } ant_stick_devid[] = { 888 | {0x0fcf, 0x1008}, 889 | {0x0fcf, 0x1009} 890 | }; 891 | 892 | int num_ant_stick_devid = sizeof(ant_stick_devid) / sizeof(ant_stick_devid[0]); 893 | 894 | /** Find the USB device for the ANT stick. Return nullptr if not found, 895 | * throws an exception if there is a problem with the lookup. 896 | */ 897 | libusb_device_handle* FindAntStick() 898 | { 899 | libusb_device **devs; 900 | ssize_t devcnt = libusb_get_device_list(nullptr, &devs); 901 | if (devcnt < 0) 902 | throw LibusbError("libusb_get_device_list", devcnt); 903 | 904 | libusb_device_handle *ant_stick = nullptr; // the one we are looking for 905 | bool found_it = false; 906 | int i = 0; 907 | libusb_device *dev = nullptr; 908 | 909 | while ((dev = devs[i++]) != nullptr && ! found_it) 910 | { 911 | struct libusb_device_descriptor desc; 912 | int r = libusb_get_device_descriptor(dev, &desc); 913 | if (r < 0) { 914 | libusb_free_device_list(devs, 1); 915 | throw LibusbError("libusb_get_device_descriptor", r); 916 | } 917 | 918 | for (int i = 0; i < num_ant_stick_devid; ++i) 919 | { 920 | if (desc.idVendor == ant_stick_devid[i].vid 921 | && desc.idProduct == ant_stick_devid[i].pid) 922 | { 923 | int r = libusb_open(dev, &ant_stick); 924 | if (r < 0) 925 | { 926 | throw LibusbError("libusb_open", r); 927 | } 928 | found_it = true; 929 | break; 930 | } 931 | } 932 | } 933 | libusb_free_device_list(devs, 1); 934 | 935 | return ant_stick; 936 | } 937 | 938 | /** Perform USB setup stuff to get the USB device ready for communication. 939 | */ 940 | void ConfigureAntStick(libusb_device_handle *ant_stick) 941 | { 942 | int r = libusb_claim_interface(ant_stick, 0); // Interface 0 must always exist 943 | if (r < 0) 944 | throw LibusbError("libusb_claim_interface", r); 945 | 946 | int actual_config = 0; 947 | int desired_config = 1; // ant sticks support only one configuration 948 | r = libusb_get_configuration(ant_stick, &actual_config); 949 | if (r < 0) 950 | throw LibusbError("libusb_get_configuration", r); 951 | 952 | if (actual_config != desired_config) 953 | { 954 | // According to libusb documentation, we cannot change the 955 | // configuration if the application has claimed interfaces. 956 | r = libusb_release_interface(ant_stick, 0); 957 | if (r < 0) 958 | throw LibusbError("libusb_release_interface", r); 959 | 960 | std::cerr << "libusb_set_configuration actual = " << actual_config 961 | << ", desired = " << desired_config << "\n"; 962 | r = libusb_set_configuration(ant_stick, desired_config); 963 | if (r < 0) 964 | throw LibusbError("libusb_set_configuration", r); 965 | 966 | r = libusb_claim_interface(ant_stick, 0); // Interface 0 must always exist 967 | if (r < 0) 968 | throw LibusbError("libusb_claim_interface", r); 969 | } 970 | r = libusb_reset_device(ant_stick); 971 | if (r < 0) 972 | throw LibusbError("libusb_reset_device", r); 973 | } 974 | 975 | /** Return the read and write end USB endpoints for the ANT stick device. 976 | * These will be used to read/write data from/to the ANT stick. 977 | */ 978 | void GetAntStickReadWriteEndpoints( 979 | libusb_device *ant_stick, 980 | uint8_t *read_endpoint, uint8_t *write_endpoint) 981 | { 982 | libusb_config_descriptor *cdesc = nullptr; 983 | int r = libusb_get_config_descriptor(ant_stick, 0, &cdesc); 984 | if (r < 0) 985 | throw LibusbError("libusb_get_config_descriptor", 0); 986 | 987 | try { 988 | if (cdesc->bNumInterfaces != 1) 989 | { 990 | throw std::runtime_error("GetAntStickReadWriteEndpoints: unexpected number of interfaces"); 991 | } 992 | const libusb_interface *i = cdesc->interface; 993 | if (i->num_altsetting != 1) 994 | { 995 | throw std::runtime_error("GetAntStickReadWriteEndpoints: unexpected number of alternate settings"); 996 | } 997 | const libusb_interface_descriptor *idesc = i->altsetting; 998 | 999 | for (int e = 0; e < idesc->bNumEndpoints; e++) 1000 | { 1001 | const libusb_endpoint_descriptor *edesc = idesc->endpoint + e; 1002 | 1003 | uint8_t edir = edesc->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK; 1004 | 1005 | // NOTE: we technically look for the last read and write endpoints, 1006 | // but there should be only one of each anyway. 1007 | switch (edir) { 1008 | case LIBUSB_ENDPOINT_IN: 1009 | *read_endpoint = edesc->bEndpointAddress; 1010 | break; 1011 | case LIBUSB_ENDPOINT_OUT: 1012 | *write_endpoint = edesc->bEndpointAddress; 1013 | break; 1014 | } 1015 | } 1016 | 1017 | libusb_free_config_descriptor(cdesc); 1018 | } 1019 | catch (...) 1020 | { 1021 | libusb_free_config_descriptor(cdesc); 1022 | throw; 1023 | } 1024 | } 1025 | 1026 | AntStick::AntStick() 1027 | : m_DeviceHandle (nullptr), 1028 | m_SerialNumber (0), 1029 | m_Version (""), 1030 | m_MaxNetworks (-1), 1031 | m_MaxChannels (-1), 1032 | m_Network(-1) 1033 | { 1034 | try { 1035 | m_DeviceHandle = FindAntStick(); 1036 | if (! m_DeviceHandle) 1037 | { 1038 | throw AntStickNotFound(); 1039 | } 1040 | 1041 | // Not needed on Windows, but harmless and needed on Linux. Don't 1042 | // check return code, as we don't care about it. 1043 | libusb_set_auto_detach_kernel_driver(m_DeviceHandle, 1); 1044 | 1045 | ConfigureAntStick(m_DeviceHandle); 1046 | 1047 | uint8_t read_endpoint, write_endpoint; 1048 | 1049 | auto *device = libusb_get_device(m_DeviceHandle); 1050 | 1051 | GetAntStickReadWriteEndpoints( 1052 | device, &read_endpoint, &write_endpoint); 1053 | 1054 | int r = libusb_clear_halt(m_DeviceHandle, read_endpoint); 1055 | if (r < 0) { 1056 | throw LibusbError("libusb_clear_halt(read_endpoint)", r); 1057 | } 1058 | 1059 | r = libusb_clear_halt(m_DeviceHandle, write_endpoint); 1060 | if (r < 0) { 1061 | throw LibusbError("libusb_clear_halt(write_endpoint)", r); 1062 | } 1063 | 1064 | auto rt = std::unique_ptr( 1065 | new AntMessageReader (m_DeviceHandle, read_endpoint)); 1066 | m_Reader = std::move (rt); 1067 | 1068 | auto wt = std::unique_ptr( 1069 | new AntMessageWriter (m_DeviceHandle, write_endpoint)); 1070 | m_Writer = std::move (wt); 1071 | 1072 | Reset(); 1073 | QueryInfo(); 1074 | 1075 | m_LastReadMessage.reserve (1024); 1076 | } 1077 | catch (...) 1078 | { 1079 | // Need to clean up, as no one will do it for us.... 1080 | m_Reader = std::move (std::unique_ptr()); 1081 | m_Writer = std::move (std::unique_ptr()); 1082 | if (m_DeviceHandle) 1083 | libusb_close(m_DeviceHandle); 1084 | throw; 1085 | } 1086 | } 1087 | 1088 | AntStick::~AntStick() 1089 | { 1090 | m_Reader = std::move (std::unique_ptr()); 1091 | m_Writer = std::move (std::unique_ptr()); 1092 | if (m_DeviceHandle) { 1093 | libusb_close(m_DeviceHandle); 1094 | } 1095 | } 1096 | 1097 | void AntStick::WriteMessage(const Buffer &b) 1098 | { 1099 | m_Writer->WriteMessage (b); 1100 | } 1101 | 1102 | /** Read a message from the ANT stick and return it. This is used only for 1103 | * reading messages indented for the ANT stick and channel management and any 1104 | * broadcast and other messages are queued up so they will be correctly 1105 | * processed by AntStick::Tick() 1106 | */ 1107 | const Buffer& AntStick::ReadInternalMessage() 1108 | { 1109 | auto SetAsideMessage = [] (const Buffer &message) -> bool { 1110 | return (message[2] == BROADCAST_DATA 1111 | || message[2] == BURST_TRANSFER_DATA 1112 | || (message[2] == CHANNEL_RESPONSE 1113 | && (message[4] == 0x01 || 1114 | message[4] == ACKNOWLEDGE_DATA || 1115 | message[4] == BURST_TRANSFER_DATA))); 1116 | }; 1117 | 1118 | for(int i = 0; i < 50; ++i) { 1119 | m_Reader->GetNextMessage(m_LastReadMessage); 1120 | if (SetAsideMessage(m_LastReadMessage)) 1121 | m_DelayedMessages.push(m_LastReadMessage); 1122 | else 1123 | return m_LastReadMessage; 1124 | } 1125 | 1126 | m_LastReadMessage.clear(); 1127 | return m_LastReadMessage; 1128 | } 1129 | 1130 | /** Reset the ANT stick by sending it a reset command. Also discard any 1131 | * queued up messages, so we don't process anything from the previous ANT 1132 | * Stick user. 1133 | */ 1134 | void AntStick::Reset() 1135 | { 1136 | // At least my AntStick dongle occasionally "forgets" to send a 1137 | // STARTUP_MESSAGE after a RESET_SYSTEM, however, the system appears to 1138 | // work fine. It is unclear if the system is reset but the startup 1139 | // message is not sent or if the system is not reset at all. 1140 | 1141 | try { 1142 | WriteMessage(MakeMessage(RESET_SYSTEM, 0)); 1143 | int ntries = 50; 1144 | while (ntries-- > 0) { 1145 | Buffer message = ReadInternalMessage(); 1146 | if(message.size() >= 2 && message[2] == STARTUP_MESSAGE) { 1147 | std::swap(m_DelayedMessages, std::queue()); 1148 | return; 1149 | } 1150 | } 1151 | } 1152 | catch (const std::exception &) { 1153 | // Discard any exceptions for the reset message (there are timeouts 1154 | // from the message reader) 1155 | } 1156 | 1157 | #ifdef DEBUG_OUTPUT 1158 | std::cerr << "AntStick::Reset() -- did not receive STARTUP_MESSAGE\n"; 1159 | #endif 1160 | 1161 | // throw std::runtime_error("AntStick::Reset: failed to receive startup message"); 1162 | } 1163 | 1164 | 1165 | /** Query basic information from the ANT Stick, such as the serial number, 1166 | * version, maximum networks and channels that it supports. 1167 | */ 1168 | void AntStick::QueryInfo() 1169 | { 1170 | WriteMessage (MakeMessage (REQUEST_MESSAGE, 0, RESPONSE_SERIAL_NUMBER)); 1171 | Buffer msg_serial = ReadInternalMessage(); 1172 | if (msg_serial[2] != RESPONSE_SERIAL_NUMBER) 1173 | throw std::runtime_error ("AntStick::QueryInfo: unexpected message"); 1174 | m_SerialNumber = msg_serial[3] | (msg_serial[4] << 8) | (msg_serial[5] << 16) | (msg_serial[6] << 24); 1175 | 1176 | WriteMessage (MakeMessage (REQUEST_MESSAGE, 0, RESPONSE_VERSION)); 1177 | Buffer msg_version = ReadInternalMessage(); 1178 | if (msg_version[2] != RESPONSE_VERSION) 1179 | throw std::runtime_error ("AntStick::QueryInfo: unexpected message"); 1180 | const char *version = reinterpret_cast(&msg_version[3]); 1181 | m_Version = version; 1182 | 1183 | WriteMessage (MakeMessage (REQUEST_MESSAGE, 0, RESPONSE_CAPABILITIES)); 1184 | Buffer msg_caps = ReadInternalMessage(); 1185 | if (msg_caps[2] != RESPONSE_CAPABILITIES) 1186 | throw std::runtime_error ("AntStick::QueryInfo: unexpected message"); 1187 | 1188 | m_MaxChannels = msg_caps[3]; 1189 | m_MaxNetworks = msg_caps[4]; 1190 | } 1191 | 1192 | void AntStick::RegisterChannel (AntChannel *c) 1193 | { 1194 | m_Channels.push_back (c); 1195 | } 1196 | 1197 | void AntStick::UnregisterChannel (AntChannel *c) 1198 | { 1199 | auto i = std::find (m_Channels.begin(), m_Channels.end(), c); 1200 | m_Channels.erase(i); 1201 | } 1202 | 1203 | int AntStick::NextChannelId() const 1204 | { 1205 | int id = 0; 1206 | for (int i = 0; i < m_MaxChannels; ++i) { 1207 | for (auto j : m_Channels) { 1208 | if (j->m_ChannelNumber == i) 1209 | goto next; 1210 | } 1211 | return i; 1212 | next: 1213 | 1 == 2; 1214 | } 1215 | return -1; 1216 | } 1217 | 1218 | void AntStick::SetNetworkKey (uint8_t key[8]) 1219 | { 1220 | // Currently, only one network use is supported, so always use network id 1221 | // 0 for now 1222 | uint8_t network = 0; 1223 | 1224 | m_Network = -1; 1225 | Buffer nkey; 1226 | nkey.push_back (network); 1227 | nkey.insert (nkey.end(), &key[0], &key[8]); 1228 | WriteMessage (MakeMessage (SET_NETWORK_KEY, nkey)); 1229 | Buffer response = ReadInternalMessage(); 1230 | CheckChannelResponse (response, network, SET_NETWORK_KEY, 0); 1231 | m_Network = network; 1232 | } 1233 | 1234 | bool AntStick::MaybeProcessMessage(const Buffer &message) 1235 | { 1236 | auto channel = message[3]; 1237 | 1238 | if (message[2] == BURST_TRANSFER_DATA) 1239 | channel = message[3] & 0x1f; 1240 | 1241 | for(auto ch : m_Channels) { 1242 | if (ch->m_ChannelNumber == channel) { 1243 | ch->HandleMessage (&message[0], message.size()); 1244 | return true; 1245 | } 1246 | } 1247 | 1248 | return false; 1249 | } 1250 | 1251 | void AntStick::Tick() 1252 | { 1253 | if (m_DelayedMessages.empty()) 1254 | { 1255 | m_Reader->MaybeGetNextMessage(m_LastReadMessage); 1256 | } 1257 | else 1258 | { 1259 | m_LastReadMessage = m_DelayedMessages.front(); 1260 | m_DelayedMessages.pop(); 1261 | } 1262 | 1263 | if (m_LastReadMessage.empty()) return; 1264 | 1265 | //std::cout << "AntStick::Tick() got a message\n"; 1266 | //DumpData(&m_LastReadMessage[0], m_LastReadMessage.size(), std::cout); 1267 | 1268 | if (! MaybeProcessMessage (m_LastReadMessage)) 1269 | { 1270 | #if defined DEBUG_OUTPUT 1271 | std::cerr << "Unprocessed message:\n"; 1272 | DumpData (&m_LastReadMessage[0], m_LastReadMessage.size(), std::cerr); 1273 | #endif 1274 | } 1275 | } 1276 | 1277 | void TickAntStick(AntStick *s) 1278 | { 1279 | s->Tick(); 1280 | struct timeval tv; 1281 | tv.tv_sec = 0; 1282 | tv.tv_usec = 10 * 1000; 1283 | int r = libusb_handle_events_timeout_completed (nullptr, &tv, nullptr); 1284 | if (r < 0) 1285 | throw LibusbError("libusb_handle_events_timeout_completed", r); 1286 | } 1287 | -------------------------------------------------------------------------------- /src/AntStick.h: -------------------------------------------------------------------------------- 1 | /** 2 | * AntStick -- communicate with an ANT+ USB stick 3 | * Copyright (C) 2017, 2018 Alex Harsanyi 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | // TODO: move libusb in the C++ file 25 | #pragma warning (push) 26 | #pragma warning (disable: 4200) 27 | #include 28 | #pragma warning (pop) 29 | 30 | typedef std::vector Buffer; 31 | 32 | class AntMessageReader; 33 | class AntMessageWriter; 34 | class AntStick; 35 | 36 | enum AntMessageId { 37 | SYNC_BYTE = 0xA4, 38 | INVALID = 0x00, 39 | 40 | // Configuration messages 41 | UNASSIGN_CHANNEL = 0x41, 42 | ASSIGN_CHANNEL = 0x42, 43 | SET_CHANNEL_ID = 0x51, 44 | SET_CHANNEL_PERIOD = 0x43, 45 | SET_CHANNEL_SEARCH_TIMEOUT = 0x44, 46 | SET_CHANNEL_RF_FREQ = 0x45, 47 | SET_NETWORK_KEY = 0x46, 48 | SET_TRANSMIT_POWER = 0x47, 49 | SET_SEARCH_WAVEFORM = 0x49, // XXX: Not in official docs 50 | ADD_CHANNEL_ID = 0x59, 51 | CONFIG_LIST = 0x5A, 52 | SET_CHANNEL_TX_POWER = 0x60, 53 | LOW_PRIORITY_CHANNEL_SEARCH_TIMOUT = 0x63, 54 | SERIAL_NUMBER_SET_CHANNEL = 0x65, 55 | ENABLE_EXT_RX_MESGS = 0x66, 56 | ENABLE_LED = 0x68, 57 | ENABLE_CRYSTAL = 0x6D, 58 | LIB_CONFIG = 0x6E, 59 | FREQUENCY_AGILITY = 0x70, 60 | PROXIMITY_SEARCH = 0x71, 61 | CHANNEL_SEARCH_PRIORITY = 0x75, 62 | // SET_USB_INFO = 0xff 63 | 64 | // Notifications 65 | STARTUP_MESSAGE = 0x6F, 66 | SERIAL_ERROR_MESSAGE = 0xAE, 67 | 68 | // Control messages 69 | RESET_SYSTEM = 0x4A, 70 | OPEN_CHANNEL = 0x4B, 71 | CLOSE_CHANNEL = 0x4C, 72 | OPEN_RX_SCAN_MODE = 0x5B, 73 | REQUEST_MESSAGE = 0x4D, 74 | SLEEP_MESSAGE = 0xC5, 75 | 76 | // Data messages 77 | BROADCAST_DATA = 0x4E, 78 | ACKNOWLEDGE_DATA = 0x4F, 79 | BURST_TRANSFER_DATA = 0x50, 80 | 81 | // Responses (from channel) 82 | CHANNEL_RESPONSE = 0x40, 83 | 84 | // Responses (from REQUEST_MESSAGE, 0x4d) 85 | RESPONSE_CHANNEL_STATUS = 0x52, 86 | RESPONSE_CHANNEL_ID = 0x51, 87 | RESPONSE_VERSION = 0x3E, 88 | RESPONSE_CAPABILITIES = 0x54, 89 | RESPONSE_SERIAL_NUMBER = 0x61 90 | }; 91 | 92 | /** Channel events received as part of the CHANNEL_RESPONSE message, defined 93 | * in section 9.5.6 "Channel Response / Event Messages" in 94 | * D00000652_ANT_Message_Protocol_and_Usage_Rev_5.1 95 | */ 96 | enum AntChannelEvent { 97 | RESPONSE_NO_ERROR = 0, 98 | EVENT_RX_SEARCH_TIMEOUT = 1, 99 | EVENT_RX_FAIL = 2, 100 | EVENT_TX = 3, 101 | EVENT_TRANSFER_RX_FAILED = 4, 102 | EVENT_TRANSFER_TX_COMPLETED = 5, 103 | EVENT_TRANSFER_TX_FAILED = 6, 104 | EVENT_CHANNEL_CLOSED = 7, 105 | EVENT_RX_FAIL_GO_TO_SEARCH = 8, 106 | EVENT_CHANNEL_COLLISION = 9, 107 | EVENT_TRANSFER_TX_START = 10, 108 | EVENT_TRANSFER_NEXT_DATA_BLOCK = 17, 109 | CHANNEL_IN_WRONG_STATE = 21, 110 | CHANNEL_NOT_OPENED = 22, 111 | CHANNEL_ID_NOT_SET = 24, 112 | CLOSE_ALL_CHANNELS = 25, 113 | TRANSFER_IN_PROGRESS = 31, 114 | TRANSFER_SEQUENCE_NUMBER_ERROR = 32, 115 | TRANSFER_IN_ERROR = 33, 116 | MESSAGE_SIZE_EXCEEDS_LIMIT = 39, 117 | INVALID_MESSAGE = 40, 118 | INVALID_NETWORK_NUMBER = 41, 119 | INVALID_LIST_ID = 48, 120 | INVALID_SCAN_TX_CHANNEL = 49, 121 | INVALID_PARAMETER_PROVIDED = 51, 122 | EVENT_SERIAL_QUE_OVERFLOW = 52, 123 | EVENT_QUE_OVERFLOW = 53, 124 | ENCRYPT_NEGOTIATION_SUCCESS = 56, 125 | ENCRYPT_NEGOTIATION_FAIL = 57, 126 | NVM_FULL_ERROR = 64, 127 | NVM_WRITE_ERROR = 65, 128 | USB_STRING_WRITE_FAIL = 112, 129 | MESG_SERIAL_ERROR_ID = 174, 130 | LAST_EVENT_ID = 0xff 131 | }; 132 | 133 | const char *ChannelEventAsString(AntChannelEvent e); 134 | 135 | 136 | // ......................................................... AntChannel .... 137 | 138 | enum TransmissionType { 139 | ANT_INDEPENDENT_CHANNEL = 0x01 140 | }; 141 | 142 | /** 143 | * Represents an ANT communication channel managed by the AntStick class. 144 | * This class represents the "slave" endpoint, the master being the 145 | * device/sensor that sends the data. 146 | * 147 | * This class is not useful directly. It needs to be derived from and at 148 | * least the ProcessMessage() function implemented to handle messages received 149 | * on the channel. The idea is that Heart Rate, Power Meter, etc channels are 150 | * implemented by deriving from this class and providing access to the 151 | * relevant user data. 152 | */ 153 | class AntChannel 154 | { 155 | public: 156 | 157 | friend class AntStick; 158 | 159 | /** The Channel ID identifies the master we want to pair up with. In ANT+ 160 | * terminology, a master is the actual sensor sending data, like a Heart 161 | * Rate monitor, and we are always the "slave". 162 | */ 163 | struct Id { 164 | Id(uint8_t device_type, uint32_t device_number = 0) 165 | : TransmissionType(0), 166 | DeviceType(device_type), 167 | DeviceNumber(device_number) 168 | { 169 | // empty 170 | } 171 | 172 | /** Defines the transmission type. We always set it to 0, once paired 173 | * up, the master will tell us what the transmission type is. 174 | */ 175 | uint8_t TransmissionType; 176 | 177 | /** Type of device we want to pair up with (e.g. Heart Rate Monitor, 178 | * Power Meter, etc). These are IDs defines in the relevant device 179 | * profile in the ANT+ documentation. 180 | */ 181 | uint8_t DeviceType; 182 | 183 | /** The serial number of the device we want to pair up with. A value 184 | * of 0 indicates a "search" for any device of DeviceType type. 185 | */ 186 | uint32_t DeviceNumber; 187 | }; 188 | 189 | /** The state of the channel, you can get the current state with 190 | * ChannelState() 191 | */ 192 | enum State { 193 | CH_SEARCHING, // Searching for a master 194 | CH_OPEN, // Open, receiving broadcast messages from a master 195 | CH_CLOSED // Closed, will not receive any messages, object 196 | // needs to be destroyed 197 | }; 198 | 199 | AntChannel (AntStick *stick, 200 | Id channel_id, 201 | unsigned period, 202 | uint8_t timeout, 203 | uint8_t frequency); 204 | virtual ~AntChannel(); 205 | 206 | void RequestClose(); 207 | State ChannelState() const { return m_State; } 208 | Id ChannelId() const { return m_ChannelId; } 209 | int MessagesReceived() const { return m_MessagesReceived; } 210 | int MessagesFailed() const { return m_MessagesFailed; } 211 | 212 | protected: 213 | /* Derived classes can use these methods. */ 214 | 215 | /** Send 'message' as an acknowledged message. The actual message will 216 | * not be sent immediately (they can only be sent shortly after a 217 | * broadcast message is received). OnAcknowledgedDataReply() will be 218 | * called with 'tag' and the result of the transmission. If the 219 | * transmission fails, it will not be retried. 220 | */ 221 | void SendAcknowledgedData(int tag, const Buffer &message); 222 | 223 | /** Ask a master device to transmit data page identified by 'page_id'. 224 | * The master will only send some data pages are only sent when requested 225 | * explicitly. The request is sent as an acknowledged data message, but a 226 | * successful transmission does not mean that the master device will send 227 | * the data page. The master will send these data pages as normal 228 | * broadcast data messages and should be processed in OnMessageReceived(). 229 | * They will be send by the master 'transmit_count' number of times (in 230 | * case the data page is lost due to collisions) 231 | */ 232 | void RequestDataPage(uint8_t page_id, int transmit_count = 4); 233 | 234 | private: 235 | /* Derived classes will need to override these methods */ 236 | 237 | /** Called when a message received on this channel and it is not a status 238 | * message. This should be overridden to process and interpret broadcast 239 | * messages received by the channel. 240 | */ 241 | virtual void OnMessageReceived (const uint8_t *data, int size) = 0; 242 | 243 | /** Called when the state of the channel has changed. Default 244 | * implementation does nothing. 245 | */ 246 | virtual void OnStateChanged (State old_state, State new_state); 247 | 248 | /** Called when we receive the status reply for an acknowledged message we 249 | * that was sent. 'tag' is the same tag that was passed to 250 | * SendAcknowledgedData() and can be used to identify which message was 251 | * sent (or failed to send) 'event' is one of EVENT_TRANSFER_TX_COMPLETED, 252 | * or EVENT_TRANSFER_TX_FAILED. Note that failed messages are not 253 | * retried, the derived class can try again by calling 254 | * SendAcknowledgedData() 255 | */ 256 | virtual void OnAcknowledgedDataReply(int tag, AntChannelEvent event); 257 | 258 | private: 259 | 260 | State m_State; 261 | Id m_ChannelId; 262 | /** The channel number is a unique identifier assigned by the AntStick it 263 | * is used when assembling messages or decoding messages received by the 264 | * ANT Stick. */ 265 | int m_ChannelNumber; 266 | 267 | /** A queued ACKNOWLEDGE_DATA message. We can only send these messages 268 | * one-by-one when a broadcast message is received, so 269 | * SendAcknowledgedData() queues them up. 270 | */ 271 | struct AckDataItem { 272 | AckDataItem(int t, const Buffer &d) 273 | : tag(t), data(d) {} 274 | int tag; 275 | Buffer data; 276 | }; 277 | 278 | /** Queue of ACKNOWLEDGE_DATA messages waiting to be sent. 279 | */ 280 | std::queue m_AckDataQueue; 281 | 282 | /** When true, an ACKNOWLEDGE_DATA message was send out and we have not 283 | * received confirmation for it yet. 284 | */ 285 | bool m_AckDataRequestOutstanding; 286 | 287 | /** When true, a Channel ID request is outstanding. We always identify 288 | * channels when we receive the first broadcast message on them. 289 | */ 290 | bool m_IdReqestOutstanding; 291 | 292 | AntStick *m_Stick; 293 | 294 | /** Number of broadcast messages received (the broadcast messages contain 295 | * useful data from the sensors). 296 | */ 297 | int m_MessagesReceived; 298 | 299 | /** Number of times we failed to receive a message. 300 | */ 301 | int m_MessagesFailed; 302 | 303 | void Configure (unsigned period, uint8_t timeout, uint8_t frequency); 304 | void HandleMessage(const uint8_t *data, int size); 305 | void MaybeSendAckData(); 306 | void OnChannelResponseMessage (const uint8_t *data, int size); 307 | void OnChannelIdMessage (const uint8_t *data, int size); 308 | void ChangeState(State new_state); 309 | }; 310 | 311 | 312 | // ........................................................... AntStick .... 313 | 314 | /** Exception thrown when the ANT stick is not found (perhaps because it is 315 | not plugged into a USB port). */ 316 | class AntStickNotFound : public std::exception 317 | { 318 | public: 319 | const char * what() const /*noexcept*/ override; 320 | }; 321 | 322 | 323 | /** 324 | * Represents the physical USB ANT Stick used to communicate with ANT+ 325 | * devices. An ANT Stick manages one or more AntChannel instances. The 326 | * Tick() method needs to be called periodically to process the received 327 | * messages and distribute them to the AntChannel instances. In addition to 328 | * that, `libusb_handle_events_timeout_completed` or equivalent needs to be 329 | * called periodically to allow libusb to process messages. See also 330 | * `TickAntStick()` 331 | * 332 | * @hint Don't forget to call libusb_init() somewhere in your program before 333 | * using this class. 334 | */ 335 | class AntStick 336 | { 337 | friend AntChannel; 338 | 339 | public: 340 | AntStick(); 341 | ~AntStick(); 342 | 343 | void SetNetworkKey (uint8_t key[8]); 344 | 345 | unsigned GetSerialNumber() const { return m_SerialNumber; } 346 | std::string GetVersion() const { return m_Version; } 347 | int GetMaxNetworks() const { return m_MaxNetworks; } 348 | int GetMaxChannels() const { return m_MaxChannels; } 349 | int GetNetwork() const { return m_Network; } 350 | 351 | void Tick(); 352 | 353 | static uint8_t g_AntPlusNetworkKey[8]; 354 | 355 | private: 356 | 357 | void WriteMessage(const Buffer &b); 358 | const Buffer& ReadInternalMessage(); 359 | 360 | void Reset(); 361 | void QueryInfo(); 362 | void RegisterChannel (AntChannel *c); 363 | void UnregisterChannel (AntChannel *c); 364 | int NextChannelId() const; 365 | 366 | bool MaybeProcessMessage(const Buffer &message); 367 | 368 | libusb_device_handle *m_DeviceHandle; 369 | 370 | unsigned m_SerialNumber; 371 | std::string m_Version; 372 | int m_MaxNetworks; 373 | int m_MaxChannels; 374 | 375 | int m_Network; 376 | 377 | std::queue m_DelayedMessages; 378 | Buffer m_LastReadMessage; 379 | 380 | std::unique_ptr m_Reader; 381 | std::unique_ptr m_Writer; 382 | 383 | std::vector m_Channels; 384 | }; 385 | 386 | /** Call libusb_handle_events_timeout_completed() than the AntStick's Tick() 387 | * method. This is an all-in-one function to get the AntStick to work, but it 388 | * is only appropriate if the application communicates with a single USB 389 | * device. 390 | * 391 | * @hint Don't forget to call libusb_init() somewhere in your program before 392 | * using this function. 393 | */ 394 | void TickAntStick(AntStick *s); 395 | 396 | /* 397 | Local Variables: 398 | mode: c++ 399 | End: 400 | */ 401 | -------------------------------------------------------------------------------- /src/FitnessEquipmentControl.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FitnessEquipmentControl -- communicate with an ANT+ FE-C trainer 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include "FitnessEquipmentControl.h" 20 | #include "Tools.h" 21 | #include 22 | #include 23 | 24 | /** IMPLEMENTATION NOTE 25 | * 26 | * Implementation of the ANT+ Fitness Equipment Device profile is based on the 27 | * "D000001231_-_ANT+_Device_Profile_-_Fitness_Equipment_-_Rev_4.2.pdf" 28 | * document available from https://www.thisisant.com 29 | */ 30 | 31 | namespace { 32 | 33 | // Values taken from the HRM ANT+ Device Profile document 34 | enum { 35 | ANT_DEVICE_TYPE = 0x11, 36 | CHANNEL_PERIOD = 8192, 37 | CHANNEL_FREQUENCY = 57, 38 | SEARCH_TIMEOUT = 30 39 | }; 40 | 41 | enum { 42 | DP_GENERAL = 0x10, 43 | DP_TRAINER_SPECIFIC = 0x19, 44 | DP_USER_CONFIG = 0x37, 45 | DP_FE_CAPABILITIES = 0x36, 46 | DP_BASIC_RESISTANCE = 0x30, 47 | DP_TARGET_POWER = 0x31, 48 | DP_WIND_RESISTANCE = 0x32, 49 | DP_TRACK_RESISTANCE = 0x33 50 | }; 51 | 52 | // amount of time in milliseconds before values become stale. 53 | enum { 54 | STALE_TIMEOUT = 5000 55 | }; 56 | 57 | }; // end anonymous namespace 58 | 59 | FitnessEquipmentControl::FitnessEquipmentControl(AntStick *stick, uint32_t device_number) 60 | : AntChannel(stick, 61 | AntChannel::Id(ANT_DEVICE_TYPE, device_number), 62 | CHANNEL_PERIOD, 63 | SEARCH_TIMEOUT, 64 | CHANNEL_FREQUENCY) 65 | { 66 | // Set some reasonable defaults for all parameters 67 | m_UpdateUserConfig = true; 68 | m_UserWeight = 75.0; 69 | m_BikeWeight = 10.0; 70 | m_BikeWheelDiameter = 0.668; 71 | 72 | m_WindResistanceCoefficient = 0.51; // default value from device profile 73 | m_WindSpeed = 0; 74 | // A drafting factor of 1 indicates no drafting effect (i.e riding alone 75 | // or at the front of the pack), a drafting factor of 0 removes all air 76 | // resistance from the simulation. 77 | m_DraftingFactor = 1.0; 78 | m_Slope = 0; 79 | // Value recommended by the device profile for asphalt road. 80 | m_RollingResistance = 0.004; 81 | 82 | m_TargetResistance = 0; 83 | m_TargetPower = 0; 84 | 85 | m_CapabilitiesStatus = CAPABILITIES_UNKNOWN; 86 | m_MaxResistance = 0; 87 | m_BasicResistanceControl = false; 88 | m_TargetPowerControl = false; 89 | m_SimulationControl = false; 90 | 91 | m_ZeroOffsetCalibrationRequired = false; 92 | m_SpinDownCalibrationRequired = false; 93 | m_UserConfigurationRequired = false; 94 | 95 | // Trainer output parameters 96 | auto ts = CurrentMilliseconds(); 97 | m_InstantPowerTimestamp = ts; 98 | m_InstantPower = 0; 99 | m_InstantSpeedTimestamp = ts; 100 | m_InstantSpeed = 0; 101 | m_InstantSpeedIsVirtual = false; 102 | m_InstantCadenceTimestamp = ts; 103 | m_InstantCadence = 0; 104 | m_TrainerState = STATE_RESERVED; 105 | m_SimulationState = TS_AT_TARGET_POWER; 106 | } 107 | 108 | double FitnessEquipmentControl::InstantPower() const 109 | { 110 | if ((CurrentMilliseconds() - m_InstantPowerTimestamp) > STALE_TIMEOUT) { 111 | return 0; 112 | } else { 113 | return m_InstantPower; 114 | } 115 | } 116 | 117 | double FitnessEquipmentControl::InstantSpeed() const 118 | { 119 | if ((CurrentMilliseconds() - m_InstantPowerTimestamp) > STALE_TIMEOUT) { 120 | return 0; 121 | } else { 122 | return m_InstantSpeed; 123 | } 124 | } 125 | 126 | bool FitnessEquipmentControl::InstantSpeedIsVirtual() const 127 | { 128 | return m_InstantSpeedIsVirtual; 129 | } 130 | 131 | double FitnessEquipmentControl::InstantCadence() const 132 | { 133 | if ((CurrentMilliseconds() - m_InstantPowerTimestamp) > STALE_TIMEOUT) { 134 | return 0; 135 | } else { 136 | return m_InstantCadence; 137 | } 138 | } 139 | 140 | void FitnessEquipmentControl::SetUserParams( 141 | double user_weight, 142 | double bike_weight, 143 | double wheel_diameter) 144 | { 145 | m_UserWeight = user_weight; 146 | m_BikeWeight = bike_weight; 147 | m_BikeWheelDiameter = wheel_diameter; 148 | m_UpdateUserConfig = true; 149 | } 150 | 151 | void FitnessEquipmentControl::OnMessageReceived(const uint8_t *data, int size) 152 | { 153 | if (data[2] != BROADCAST_DATA) 154 | return; 155 | 156 | switch(data[4]) { 157 | case DP_GENERAL: 158 | ProcessGeneralPage(data + 4, size - 4); 159 | break; 160 | case DP_TRAINER_SPECIFIC: 161 | ProcessTrainerSpecificPage(data + 4, size - 4); 162 | break; 163 | case DP_FE_CAPABILITIES: 164 | ProcessCapabilitiesPage(data + 4, size - 4); 165 | break; 166 | default: 167 | #if 0 168 | std::cout << "FitnessEquipmentControl unknown data page: \n"; 169 | DumpData(data, size, std::cout); 170 | #endif 171 | break; 172 | } 173 | 174 | if (ChannelId().DeviceNumber == 0) { 175 | // Don't request anything until we have a device number 176 | } else if (m_CapabilitiesStatus == CAPABILITIES_UNKNOWN) { 177 | RequestDataPage(DP_FE_CAPABILITIES); 178 | m_CapabilitiesStatus = CAPABILITIES_REQUESTED; 179 | } 180 | else if (m_UpdateUserConfig) { 181 | SendUserConfigPage(); 182 | } 183 | } 184 | 185 | void FitnessEquipmentControl::SendUserConfigPage() 186 | { 187 | std::cout << "Sending user config:\n" 188 | << "\tRider Weight: " << std::setprecision(2) << m_UserWeight << " kg\n" 189 | << "\tBike Weight: " << std::setprecision(2) << m_BikeWeight << " kg\n" 190 | << "\tWheel Diameter: " << std::setprecision(4) << m_BikeWheelDiameter << " meters" 191 | << std::endl; 192 | uint16_t uw = static_cast(m_UserWeight / 0.01); 193 | uint16_t bw = static_cast(m_BikeWeight / 0.05); 194 | // Wheel size in centimeters 195 | uint16_t ws = static_cast(m_BikeWheelDiameter / 0.01); 196 | // The 10 mm part of wheel size 197 | uint16_t ws1 = static_cast(m_BikeWheelDiameter / 0.001) - ws * 10; 198 | 199 | Buffer msg; 200 | msg.push_back(DP_USER_CONFIG); 201 | msg.push_back(uw & 0xFF); 202 | msg.push_back((uw >> 8) & 0xFF); 203 | msg.push_back(0xFF); // reserved 204 | msg.push_back((ws1 & 0x3) | ((bw | 0x03) << 4)); 205 | msg.push_back((bw >> 4) & 0xFF); 206 | msg.push_back(ws & 0xFF); 207 | msg.push_back(0x00); // gear ratio -- we send invalid value 208 | 209 | SendAcknowledgedData(DP_USER_CONFIG, msg); 210 | m_UpdateUserConfig = false; 211 | } 212 | 213 | void FitnessEquipmentControl::ProcessGeneralPage( 214 | const uint8_t *data, int size) 215 | { 216 | uint8_t capabilities = data[7] & 0x0f; 217 | // NOTE: bit 3 is the lap toggle field, which we don't use 218 | m_TrainerState = static_cast((data[7] >> 4) & 0x07); 219 | uint8_t speed_lsb = data[4]; 220 | uint8_t speed_msb = data[5]; 221 | m_InstantSpeedTimestamp = CurrentMilliseconds(); 222 | m_InstantSpeed = ((speed_msb << 8) + speed_lsb) * 0.001; 223 | m_InstantSpeedIsVirtual = (capabilities & 0x3) != 0; 224 | m_EquipmentType = static_cast(data[1] & 0x1F); 225 | } 226 | 227 | void FitnessEquipmentControl::ProcessTrainerSpecificPage( 228 | const uint8_t *data, int size) 229 | { 230 | uint8_t trainer_status = (data[6] >> 4) & 0x0f; 231 | uint8_t flags = data[7] & 0x0f; 232 | // NOTE: bit 3 is the lap toggle field, which we don't use 233 | m_TrainerState = static_cast((data[7] >> 4) & 0x07); 234 | uint8_t power_lsb = data[5]; 235 | uint8_t power_msb = data[6] & 0x0F; 236 | auto ts = CurrentMilliseconds(); 237 | m_InstantPowerTimestamp = ts; 238 | m_InstantPower = (power_msb << 8) + power_lsb; 239 | m_SimulationState = static_cast(flags & 0x03); 240 | m_InstantPowerTimestamp = ts; 241 | m_InstantCadence = data[2]; 242 | m_ZeroOffsetCalibrationRequired = (trainer_status & 0x01) != 0; 243 | m_SpinDownCalibrationRequired = (trainer_status & 0x02) != 0; 244 | m_UserConfigurationRequired = (trainer_status & 0x04) != 0; 245 | m_UpdateUserConfig = m_UpdateUserConfig | m_UserConfigurationRequired; 246 | } 247 | 248 | void FitnessEquipmentControl::ProcessCapabilitiesPage( 249 | const uint8_t *data, int size) 250 | { 251 | m_MaxResistance = (data[6] << 8) + data[5]; 252 | uint8_t capabilities = data[7]; 253 | bool BasicResistanceControl = (capabilities & 0x01) != 0; 254 | bool TargetPowerControl = (capabilities & 0x02) != 0; 255 | bool SimulationControl = (capabilities & 0x04) != 0; 256 | 257 | // We can receive this data page multiple times 258 | if (m_CapabilitiesStatus != CAPABILITIES_RECEIVED 259 | || BasicResistanceControl != m_BasicResistanceControl 260 | || TargetPowerControl != m_TargetPowerControl 261 | || SimulationControl != m_SimulationControl) 262 | { 263 | m_CapabilitiesStatus = CAPABILITIES_RECEIVED; 264 | m_BasicResistanceControl = BasicResistanceControl; 265 | m_TargetPowerControl = TargetPowerControl; 266 | m_SimulationControl = SimulationControl; 267 | 268 | std::cout << "Got trainer capabilities:\n" 269 | << "\tMax Resistance: " << m_MaxResistance << " Newtons\n" 270 | << "\tControl Modes: " 271 | << (m_BasicResistanceControl ? "Basic Resistance" : "") 272 | << (m_TargetPowerControl ? "; Target Power" : "") 273 | << (m_SimulationControl ? "; Simulation" : "") 274 | << std::endl; 275 | } 276 | } 277 | 278 | void FitnessEquipmentControl::OnAcknowledgedDataReply( 279 | int tag, AntChannelEvent event) 280 | { 281 | if (event != EVENT_TRANSFER_TX_COMPLETED) { 282 | // Reset relevant state to send requests again 283 | if (tag == DP_FE_CAPABILITIES) { 284 | m_CapabilitiesStatus = CAPABILITIES_UNKNOWN; 285 | } else if (tag == DP_USER_CONFIG) { 286 | m_UpdateUserConfig = true; 287 | } else if (tag == DP_TRACK_RESISTANCE) { 288 | SendTrackResistanceDataPage(); 289 | } 290 | } 291 | } 292 | 293 | void FitnessEquipmentControl::OnStateChanged ( 294 | AntChannel::State old_state, AntChannel::State new_state) 295 | { 296 | if (new_state == AntChannel::CH_OPEN) { 297 | std::cout << "Connected to ANT+ FE-C with serial " << ChannelId().DeviceNumber << std::endl; 298 | } 299 | 300 | if (new_state != AntChannel::CH_OPEN) { 301 | m_CapabilitiesStatus = CAPABILITIES_UNKNOWN; 302 | m_MaxResistance = 0; 303 | m_BasicResistanceControl = false; 304 | m_TargetPowerControl = false; 305 | m_SimulationControl = false; 306 | 307 | m_ZeroOffsetCalibrationRequired = false; 308 | m_SpinDownCalibrationRequired = false; 309 | m_UserConfigurationRequired = false; 310 | 311 | // Trainer output parameters 312 | m_InstantPower = 0; 313 | m_InstantSpeed = 0; 314 | m_InstantSpeedIsVirtual = false; 315 | m_InstantCadence = 0; 316 | m_TrainerState = STATE_RESERVED; 317 | m_SimulationState = TS_AT_TARGET_POWER; 318 | } 319 | } 320 | 321 | void FitnessEquipmentControl::SetSlope(double slope) 322 | { 323 | std::cout << "Set Slope to " << slope << std::endl; 324 | m_Slope = slope; 325 | SendTrackResistanceDataPage(); 326 | } 327 | 328 | void FitnessEquipmentControl::SendTrackResistanceDataPage() 329 | { 330 | Buffer msg; 331 | msg.push_back(DP_TRACK_RESISTANCE); 332 | msg.push_back(0xFF); 333 | msg.push_back(0xFF); 334 | msg.push_back(0xFF); 335 | msg.push_back(0xFF); 336 | uint16_t raw_slope = static_cast((m_Slope + 200.0) / 0.01); 337 | msg.push_back(raw_slope & 0xFF); 338 | msg.push_back((raw_slope >> 8) & 0xFF); 339 | uint8_t raw_rr = static_cast(m_RollingResistance * 5e5); 340 | msg.push_back(raw_rr); 341 | SendAcknowledgedData(DP_TRACK_RESISTANCE, msg); 342 | } 343 | 344 | namespace { 345 | 346 | struct EquipmentTypeName { 347 | FitnessEquipmentControl::EquipmentType type; 348 | const char *name; 349 | } g_EquipmentTypeNames[] = { 350 | { FitnessEquipmentControl::ET_GENERAL, "general" }, 351 | { FitnessEquipmentControl::ET_TREADMILL, "treadmill" }, 352 | { FitnessEquipmentControl::ET_ELLIPTICAL, "elliptical" }, 353 | { FitnessEquipmentControl::ET_STATIONARY_BIKE, "stationary bike" }, 354 | { FitnessEquipmentControl::ET_ROWER, "rower" }, 355 | { FitnessEquipmentControl::ET_CLIMBER, "climber" }, 356 | { FitnessEquipmentControl::ET_NORDIC_SKIER, "nordic skier"}, 357 | { FitnessEquipmentControl::ET_TRAINER, "trainer" }, 358 | { FitnessEquipmentControl::ET_UNKNOWN, "unknown" } 359 | }; 360 | 361 | }; // end anonymous namespace 362 | 363 | const char *EquipmentTypeAsString (FitnessEquipmentControl::EquipmentType et) 364 | { 365 | for (int i = 0; g_EquipmentTypeNames[i].type != FitnessEquipmentControl::ET_UNKNOWN; i++) { 366 | if (g_EquipmentTypeNames[i].type == et) 367 | return g_EquipmentTypeNames[i].name; 368 | } 369 | return "unknown"; 370 | } 371 | 372 | -------------------------------------------------------------------------------- /src/FitnessEquipmentControl.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FitnessEquipmentControl -- communicate with an ANT+ FE-C trainer 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #pragma once 19 | 20 | #include "AntStick.h" 21 | 22 | /** Read data and control resistance from an ANT+ FE-C capable trainer. 23 | * Currently, instant power, speed and cadence can be read, and the slope can 24 | * be set. 25 | */ 26 | class FitnessEquipmentControl : public AntChannel 27 | { 28 | public: 29 | 30 | enum EquipmentType { 31 | ET_UNKNOWN = 0, 32 | ET_GENERAL = 16, 33 | ET_TREADMILL = 19, 34 | ET_ELLIPTICAL = 20, 35 | ET_STATIONARY_BIKE = 21, 36 | ET_ROWER = 22, 37 | ET_CLIMBER = 23, 38 | ET_NORDIC_SKIER = 24, 39 | ET_TRAINER = 25 40 | }; 41 | 42 | enum TrainerState { 43 | STATE_RESERVED = 0, 44 | STATE_ASLEEP = 1, 45 | STATE_READY = 2, 46 | STATE_IN_USE = 3, 47 | STATE_FINISHED = 4, // PAUSED 48 | }; 49 | 50 | enum SimulationState { 51 | TS_AT_TARGET_POWER = 0, // at target power, or no target set 52 | TS_SPEED_TOO_LOW = 1, // speed is too low to achieve target power 53 | TS_SPEED_TOO_HIGH = 2, // speed is too high to achieve target power 54 | TS_POWER_LIMIT_REACHED = 3 // undetermined (min or max) power limit reached 55 | }; 56 | 57 | FitnessEquipmentControl(AntStick *stick, uint32_t device_number = 0); 58 | 59 | double InstantPower() const; 60 | double InstantSpeed() const; 61 | bool InstantSpeedIsVirtual() const; 62 | double InstantCadence() const; 63 | 64 | EquipmentType GetEquipmentType() const { return m_EquipmentType; } 65 | 66 | void SetUserParams( 67 | double user_weight, 68 | double bike_weight, 69 | double wheel_diameter); 70 | 71 | void SetSlope(double slope); 72 | 73 | private: 74 | 75 | void OnMessageReceived(const uint8_t *data, int size) override; 76 | void SendUserConfigPage(); 77 | void ProcessGeneralPage(const uint8_t *data, int size); 78 | void ProcessTrainerSpecificPage(const uint8_t *data, int size); 79 | void ProcessCapabilitiesPage(const uint8_t *data, int size); 80 | void OnAcknowledgedDataReply(int tag, AntChannelEvent event); 81 | void OnStateChanged (AntChannel::State old_state, AntChannel::State new_state) override; 82 | 83 | void SendTrackResistanceDataPage(); 84 | 85 | // User configuration 86 | 87 | bool m_UpdateUserConfig; 88 | double m_UserWeight; 89 | double m_BikeWeight; 90 | double m_BikeWheelDiameter; 91 | 92 | // Parameters used when trainer is in simulation mode 93 | 94 | double m_WindResistanceCoefficient; 95 | // negative indicates tailwind 96 | double m_WindSpeed; 97 | double m_DraftingFactor; 98 | double m_Slope; 99 | double m_RollingResistance; 100 | 101 | // Parameters used when trainer is in basic resistance mode 102 | 103 | double m_TargetResistance; // 0 - 100% 104 | 105 | // Parameters used when trainer is in target power mode 106 | 107 | double m_TargetPower; 108 | 109 | // Trainer capabilities 110 | 111 | enum CapabilitiesStatus { 112 | CAPABILITIES_UNKNOWN, 113 | CAPABILITIES_REQUESTED, 114 | CAPABILITIES_RECEIVED 115 | }; 116 | 117 | CapabilitiesStatus m_CapabilitiesStatus; 118 | double m_MaxResistance; 119 | bool m_BasicResistanceControl; 120 | bool m_TargetPowerControl; 121 | bool m_SimulationControl; 122 | EquipmentType m_EquipmentType; 123 | 124 | // Configuration/Calibration status 125 | 126 | bool m_ZeroOffsetCalibrationRequired; 127 | bool m_SpinDownCalibrationRequired; 128 | bool m_UserConfigurationRequired; 129 | 130 | // Trainer output parameters 131 | uint32_t m_InstantPowerTimestamp; 132 | double m_InstantPower; 133 | uint32_t m_InstantSpeedTimestamp; 134 | double m_InstantSpeed; 135 | bool m_InstantSpeedIsVirtual; 136 | uint32_t m_InstantCadenceTimestamp; 137 | double m_InstantCadence; 138 | TrainerState m_TrainerState; 139 | 140 | // Only used if we are in target power mode, otherwise it is 0 -- 141 | // TS_AT_TARGET_POWER 142 | SimulationState m_SimulationState; 143 | }; 144 | 145 | const char *EquipmentTypeAsString (FitnessEquipmentControl::EquipmentType et); 146 | -------------------------------------------------------------------------------- /src/HeartRateMonitor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * HeartRateMonitor -- communicate with an ANT+ HRM 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include "HeartRateMonitor.h" 20 | #include "Tools.h" 21 | #include 22 | 23 | /** IMPLEMENTATION NOTE 24 | * 25 | * Implementation of the ANT+ Heart Rate Device profile is based on the 26 | * "D00000693_-_ANT+_Device_Profile_-_Heart_Rate_Rev_2.1.pdf" document 27 | * available from https://www.thisisant.com 28 | */ 29 | 30 | namespace { 31 | 32 | // Values taken from the HRM ANT+ Device Profile document 33 | enum { 34 | ANT_DEVICE_TYPE = 0x78, 35 | CHANNEL_PERIOD = 8070, 36 | CHANNEL_FREQUENCY = 57, 37 | SEARCH_TIMEOUT = 30 38 | }; 39 | 40 | enum { 41 | STALE_TIMEOUT = 5000 42 | }; 43 | 44 | }; // end anonymous namespace 45 | 46 | HeartRateMonitor::HeartRateMonitor (AntStick *stick, uint32_t device_number) 47 | : AntChannel( 48 | stick, 49 | AntChannel::Id(ANT_DEVICE_TYPE, device_number), 50 | CHANNEL_PERIOD, SEARCH_TIMEOUT, CHANNEL_FREQUENCY) 51 | { 52 | m_LastMeasurementTime = 0; 53 | m_MeasurementTime = 0; 54 | m_HeartBeats = 0; 55 | m_InstantHeartRate = 0; 56 | m_InstantHeartRateTimestamp = 0; 57 | } 58 | 59 | void HeartRateMonitor::OnMessageReceived(const unsigned char *data, int size) 60 | { 61 | if (data[2] != BROADCAST_DATA) 62 | return; 63 | 64 | // NOTE: the last 3 values in the payload are always the same regardless 65 | // of the data page. Also for the data page, we need to observe the 66 | // highest bit toggle, as old HRM's don't have data pages. 67 | m_LastMeasurementTime = m_MeasurementTime; 68 | m_MeasurementTime = data[8] + (data[9] << 8); 69 | m_HeartBeats = data[10]; 70 | m_InstantHeartRate = data[11]; 71 | m_InstantHeartRateTimestamp = CurrentMilliseconds(); 72 | } 73 | 74 | double HeartRateMonitor::InstantHeartRate() const 75 | { 76 | if ((CurrentMilliseconds() - m_InstantHeartRateTimestamp) > STALE_TIMEOUT) { 77 | return 0; 78 | } else { 79 | return m_InstantHeartRate; 80 | } 81 | } 82 | 83 | void HeartRateMonitor::OnStateChanged ( 84 | AntChannel::State old_state, AntChannel::State new_state) 85 | { 86 | if (new_state == AntChannel::CH_OPEN) { 87 | std::cout << "Connected to HRM with serial " << ChannelId().DeviceNumber << std::endl; 88 | } 89 | 90 | 91 | if (new_state != AntChannel::CH_OPEN) { 92 | m_LastMeasurementTime = 0; 93 | m_MeasurementTime = 0; 94 | m_HeartBeats = 0; 95 | m_InstantHeartRate = 0; 96 | m_InstantHeartRateTimestamp = 0; 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /src/HeartRateMonitor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * HeartRateMonitor -- communicate with an ANT+ HRM 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #pragma once 19 | 20 | #include "AntStick.h" 21 | 22 | /** Receive data from an ANT+ heart rate monitor. 23 | * 24 | * @warning At this time, only the InstantHeartRate() is received and there is 25 | * no mechanism implemented to provide average HR information when broadcasts 26 | * are missed (as described in the profile document), "correct" R-R interval 27 | * measurement is also not implemented. 28 | **/ 29 | class HeartRateMonitor : public AntChannel 30 | { 31 | public: 32 | 33 | HeartRateMonitor(AntStick *stick, uint32_t device_number = 0); 34 | double InstantHeartRate() const; 35 | 36 | private: 37 | void OnMessageReceived(const unsigned char *data, int size) override; 38 | void OnStateChanged (AntChannel::State old_state, AntChannel::State new_state) override; 39 | 40 | int m_LastMeasurementTime; 41 | int m_MeasurementTime; 42 | int m_HeartBeats; 43 | uint32_t m_InstantHeartRateTimestamp; 44 | double m_InstantHeartRate; 45 | }; 46 | -------------------------------------------------------------------------------- /src/NetTools.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * NetTools -- convenient wrappers for socket functions 3 | * Copyright (C) 2017, 2018 Alex Harsanyi 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include 20 | #include "NetTools.h" 21 | #include "Tools.h" 22 | #include 23 | #include 24 | 25 | #pragma comment (lib, "ws2_32.lib") 26 | 27 | SOCKET tcp_listen(int port) 28 | { 29 | WSADATA wsaData; 30 | memset(&wsaData, 0, sizeof(wsaData)); 31 | HRESULT r = WSAStartup(MAKEWORD(2, 2), &wsaData); 32 | if (r != 0 ) 33 | throw Win32Error("WSAStartup()", r); 34 | 35 | SOCKET s = socket(AF_INET6, SOCK_STREAM, IPPROTO_TCP); 36 | 37 | if (s == INVALID_SOCKET) 38 | throw Win32Error("socket()", WSAGetLastError()); 39 | 40 | BOOL reuse_addr = TRUE; 41 | r = setsockopt(s, SOL_SOCKET, SO_REUSEADDR, 42 | (char*)&reuse_addr, sizeof(reuse_addr)); 43 | if (r != 0) 44 | throw Win32Error("setsockopt(SO_REUSEADDR)", WSAGetLastError()); 45 | 46 | // Also accept IPv4 connections 47 | BOOL v6only = FALSE; 48 | r = setsockopt(s, IPPROTO_IPV6, IPV6_V6ONLY, 49 | (char*)&v6only, sizeof(v6only)); 50 | if (r != 0) 51 | throw Win32Error("setsockopt(IPV6_V6ONLY)", WSAGetLastError()); 52 | 53 | struct addrinfo *result, hints; 54 | memset(&hints, 0, sizeof(hints)); 55 | hints.ai_family = AF_INET6; 56 | hints.ai_socktype = SOCK_STREAM; 57 | hints.ai_protocol = IPPROTO_TCP; 58 | hints.ai_flags = AI_PASSIVE; 59 | 60 | std::ostringstream port_msg; 61 | port_msg << port; 62 | r = getaddrinfo(NULL, port_msg.str().c_str(), &hints, &result); 63 | if (r != 0) 64 | throw Win32Error("getaddrinfo()", WSAGetLastError()); 65 | 66 | r = bind(s, result->ai_addr, (int)result->ai_addrlen); 67 | freeaddrinfo(result); 68 | 69 | if (r == SOCKET_ERROR) 70 | throw Win32Error("bind()", WSAGetLastError()); 71 | 72 | r = listen(s, /* backlog = */ 5); 73 | if (r == SOCKET_ERROR) 74 | throw Win32Error("listen()", WSAGetLastError()); 75 | 76 | return s; 77 | } 78 | 79 | SOCKET tcp_accept(SOCKET server) 80 | { 81 | SOCKET client = accept (server, NULL, NULL); 82 | 83 | if (client == INVALID_SOCKET) 84 | throw Win32Error ("accept()", WSAGetLastError()); 85 | 86 | // Disable send delay. 87 | unsigned long flag = 1; 88 | int r = setsockopt(client, IPPROTO_TCP, TCP_NODELAY, 89 | reinterpret_cast(&flag), sizeof(flag)); 90 | if (r == SOCKET_ERROR) 91 | throw Win32Error("setsockopt()", WSAGetLastError()); 92 | 93 | return client; 94 | } 95 | 96 | SOCKET tcp_connect (const std::string &server, int port) 97 | { 98 | { 99 | // Setup WinSock. We can initialize WinSock as many time we like so 100 | // we don't bother to check if it was already initialized. We need at 101 | // least version 2.2 of WinSock (MAKEWORD(2, 2)) 102 | WSADATA wsaData; 103 | memset(&wsaData, 0, sizeof(wsaData)); 104 | HRESULT r = WSAStartup(MAKEWORD(2, 2), &wsaData); 105 | if (r != 0 ) 106 | throw Win32Error("WSAStartup()", r); 107 | } 108 | 109 | std::ostringstream service_name; 110 | service_name << port; 111 | 112 | struct addrinfo hints; 113 | memset (&hints, 0, sizeof(hints)); 114 | hints.ai_family = AF_UNSPEC; 115 | hints.ai_socktype = SOCK_STREAM; 116 | hints.ai_protocol = IPPROTO_TCP; 117 | 118 | // A host name can have multiple addresses, use getaddrinfo() to retrieve 119 | // them all. 120 | struct addrinfo *addresses; 121 | 122 | int r = getaddrinfo (server.c_str(), service_name.str().c_str(), &hints, &addresses); 123 | if (r != 0) 124 | throw Win32Error ("getaddrinfo", WSAGetLastError()); 125 | 126 | // Try to connect using each returned address, return the first successful 127 | // connection 128 | 129 | for (struct addrinfo *a = addresses; a != NULL; a = a->ai_next) 130 | { 131 | SOCKET client = socket (a->ai_family, a->ai_socktype, a->ai_protocol); 132 | if (client == INVALID_SOCKET) 133 | { 134 | // The socket() call failed with parameters supplied by the 135 | // getaddrinfo() call. This is not a network issue and needs to 136 | // be reported immediately. 137 | throw Win32Error ("socket", WSAGetLastError()); 138 | } 139 | 140 | r = connect (client, (struct sockaddr*)a->ai_addr, a->ai_addrlen); 141 | if (r == SOCKET_ERROR) 142 | { 143 | // If this is the last address, throw an exception, otherwise try 144 | // connecting to the next address. 145 | if (a->ai_next == NULL) 146 | throw Win32Error ("connect", WSAGetLastError()); 147 | else 148 | continue; 149 | } 150 | 151 | // We have succeeded connecting to this socket, return it 152 | freeaddrinfo (addresses); 153 | 154 | // Disable send delay. 155 | unsigned long flag = 1; 156 | r = setsockopt(client, IPPROTO_TCP, TCP_NODELAY, 157 | reinterpret_cast(&flag), sizeof(flag)); 158 | if (r == SOCKET_ERROR) 159 | throw Win32Error("setsockopt()", WSAGetLastError()); 160 | 161 | return client; 162 | } 163 | 164 | freeaddrinfo (addresses); 165 | throw std::logic_error ("tcp_connect: cannot find suitable address"); 166 | } 167 | 168 | std::string get_peer_name (SOCKET s) 169 | { 170 | struct sockaddr_in6 addr; 171 | int len = sizeof(struct sockaddr_in6); 172 | int r = getpeername(s, (struct sockaddr*)&addr, &len); 173 | if (r == SOCKET_ERROR) 174 | { 175 | throw Win32Error ("getpeername()", WSAGetLastError()); 176 | } 177 | 178 | char hostname[NI_MAXHOST]; 179 | char servname[NI_MAXSERV]; 180 | r = getnameinfo ((struct sockaddr*)&addr, sizeof(struct sockaddr_in6), 181 | hostname, NI_MAXHOST, 182 | servname, NI_MAXSERV, 183 | NI_NUMERICSERV); 184 | char address[50]; 185 | InetNtopA(AF_INET6, &addr, &address[0], sizeof(address)); 186 | 187 | if (r == 0) // success 188 | { 189 | std::ostringstream n; 190 | n << hostname << " at " << address << ":" << ntohs(addr.sin6_port); 191 | return n.str(); 192 | } 193 | else 194 | { 195 | std::ostringstream n; 196 | n << address << ":" << ntohs(addr.sin6_port); 197 | return n.str(); 198 | } 199 | } 200 | 201 | // NOTE: timeout is in milliseconds 202 | std::vector get_socket_status(const std::vector &sockets, uint64_t timeout) 203 | { 204 | if (sockets.size() >= FD_SETSIZE) 205 | throw std::exception("get_socket_status: too many sockets"); 206 | 207 | std::vector result(sockets.size()); // initialized to 0 208 | 209 | fd_set read_fds, write_fds, except_fds; 210 | FD_ZERO (&read_fds); 211 | FD_ZERO (&write_fds); 212 | FD_ZERO (&except_fds); 213 | 214 | std::for_each (begin(sockets), end(sockets), 215 | [&](SOCKET s) { 216 | FD_SET (s, &read_fds); 217 | FD_SET (s, &write_fds); 218 | FD_SET (s, &except_fds); 219 | }); 220 | 221 | auto seconds = timeout / 1000; 222 | auto useconds = (timeout - (seconds * 1000)) * 1000; 223 | 224 | struct timeval to; 225 | to.tv_sec = static_cast(seconds); 226 | to.tv_usec = static_cast(useconds); 227 | 228 | int r = select (sockets.size(), &read_fds, &write_fds, &except_fds, &to); 229 | 230 | if (r == SOCKET_ERROR) { 231 | throw Win32Error ("select()", WSAGetLastError()); 232 | } 233 | 234 | // r == 0 means that no sockets have messages, saves the trouble of 235 | // checking them individually 236 | if (r == 0) 237 | return result; 238 | 239 | for (unsigned i = 0; i < sockets.size(); i++) { 240 | uint8_t val = 0; 241 | if (FD_ISSET (sockets[i], &read_fds)) { 242 | val |= SK_READ; 243 | } 244 | if (FD_ISSET (sockets[i], &write_fds)) { 245 | val |= SK_WRITE; 246 | } 247 | if (FD_ISSET (sockets[i], &except_fds)) { 248 | val |= SK_EXCEPT; 249 | } 250 | result[i] = val; 251 | } 252 | 253 | return std::move (result); 254 | } 255 | -------------------------------------------------------------------------------- /src/NetTools.h: -------------------------------------------------------------------------------- 1 | /** 2 | * NetTools -- convenient wrappers for socket functions 3 | * Copyright (C) 2017, 2018 Alex Harsanyi 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | SOCKET tcp_listen(int port); 27 | SOCKET tcp_accept(SOCKET server); 28 | SOCKET tcp_connect (const std::string &server, int port); 29 | std::string get_peer_name (SOCKET s); 30 | 31 | enum { 32 | SK_READ = 0x01, 33 | SK_WRITE = 0x02, 34 | SK_EXCEPT = 0x04 35 | }; 36 | 37 | // timeout is in milliseconds 38 | std::vector 39 | get_socket_status(const std::vector &sockets, uint64_t timeout); 40 | -------------------------------------------------------------------------------- /src/TelemetryServer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * TelemetryServer -- manage a bike trainer 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include "TelemetryServer.h" 20 | #include "Tools.h" 21 | 22 | std::ostream& operator<<(std::ostream &out, const Telemetry &t) 23 | { 24 | if (t.hr >= 0) 25 | out << "HR: " << t.hr; 26 | if (t.cad >= 0) 27 | out << ";CAD: " << t.cad; 28 | if (t.pwr >= 0) 29 | out << ";PWR: " << t.pwr; 30 | if (t.spd >= 0) 31 | out << ";SPD: " << t.spd; 32 | return out; 33 | } 34 | 35 | bool SendMessage(SOCKET s, const char *msg, int len) 36 | { 37 | int r = send(s, msg, len, 0); 38 | if (r == SOCKET_ERROR) { 39 | auto last_error = WSAGetLastError(); 40 | if (WSAECONNRESET == last_error) 41 | return false; 42 | throw Win32Error("send()", last_error); 43 | } 44 | if (r < len) { 45 | throw std::runtime_error("send_data: short write to server"); 46 | } 47 | return true; 48 | } 49 | 50 | // read a '\n' terminated message from socket s 51 | std::string ReadMessage(SOCKET s) 52 | { 53 | std::string message; 54 | // NOTE: we are very inefficient, as we are reading bytes one-by-one. To 55 | // improve this, we need to associate a receive buffer with a socket, 56 | // because we can't put things back and we don't know how much to read... 57 | while (true) { 58 | char buf[1]; 59 | int len = sizeof(buf); 60 | int r = recv(s, &buf[0], len, 0); 61 | if (r == 0) // socket was closed 62 | return message; 63 | if (r == SOCKET_ERROR) 64 | throw Win32Error("recv()", WSAGetLastError()); 65 | if (buf[0] == '\n') 66 | return message; 67 | message.push_back(buf[0]); 68 | } 69 | } 70 | 71 | TelemetryServer::TelemetryServer (AntStick *stick, int port) 72 | : m_AntStick (stick), 73 | m_Hrm (nullptr), 74 | m_Fec (nullptr) 75 | { 76 | try { 77 | auto server = tcp_listen(port); 78 | std::cout << "Started server on port " << port << std::endl; 79 | m_Clients.push_back(server); 80 | m_Hrm = new HeartRateMonitor (m_AntStick); 81 | m_Fec = new FitnessEquipmentControl (m_AntStick); 82 | } 83 | catch (...) { 84 | if (m_Clients.size() > 0) 85 | closesocket(m_Clients.front()); 86 | delete m_Hrm; 87 | delete m_Fec; 88 | } 89 | } 90 | 91 | TelemetryServer::~TelemetryServer() 92 | { 93 | for (auto i = begin (m_Clients); i != end (m_Clients); ++i) 94 | closesocket (*i); 95 | delete m_Hrm; 96 | delete m_Fec; 97 | } 98 | 99 | void TelemetryServer::Tick() 100 | { 101 | #if 1 102 | TickAntStick (m_AntStick); 103 | CheckSensorHealth(); 104 | #endif 105 | Telemetry t; 106 | #if 1 107 | CollectTelemetry (t); 108 | #else 109 | t.cad = 78; 110 | t.hr = 146; 111 | t.spd = 4.2; 112 | t.pwr = 214; 113 | #endif 114 | ProcessClients (t); 115 | } 116 | 117 | void TelemetryServer::CheckSensorHealth() 118 | { 119 | if (m_Hrm && m_Hrm->ChannelState() == AntChannel::CH_CLOSED) { 120 | std::cout << "Creating new HRM channel" << std::endl; 121 | auto device_number = m_Hrm->ChannelId().DeviceNumber; 122 | delete m_Hrm; 123 | m_Hrm = nullptr; 124 | // Try to connect again, but we now look for the same device, don't 125 | // change HRM sensors mid-simulation. 126 | m_Hrm = new HeartRateMonitor (m_AntStick, device_number); 127 | } 128 | 129 | if (m_Fec && m_Fec->ChannelState() == AntChannel::CH_CLOSED) { 130 | auto device_number = m_Fec->ChannelId().DeviceNumber; 131 | delete m_Fec; 132 | m_Fec = nullptr; 133 | m_Fec = new FitnessEquipmentControl (m_AntStick, device_number); 134 | } 135 | } 136 | 137 | void TelemetryServer::CollectTelemetry (Telemetry &out) 138 | { 139 | if (m_Hrm && m_Hrm->ChannelState() == AntChannel::CH_OPEN) 140 | out.hr = m_Hrm->InstantHeartRate(); 141 | 142 | if (m_Fec && m_Fec->ChannelState() == AntChannel::CH_OPEN) { 143 | out.cad = m_Fec->InstantCadence(); 144 | out.pwr = m_Fec->InstantPower(); 145 | out.spd = m_Fec->InstantSpeed(); 146 | } 147 | } 148 | 149 | void TelemetryServer::ProcessClients(const Telemetry &t) 150 | { 151 | std::ostringstream text; 152 | text << "TELEMETRY " << t << "\n"; 153 | std::string message = text.str(); 154 | 155 | auto status = get_socket_status(m_Clients, 10); 156 | // NOTE: first item in list is the server socket, a SK_READ flag on it 157 | // means there's a client waiting on it 158 | if (status[0] & SK_READ) { 159 | auto client = tcp_accept(m_Clients[0]); 160 | std::cout << "Accepted connection from " << get_peer_name(client) << std::endl; 161 | m_Clients.push_back(client); 162 | } 163 | 164 | std::vector closed_sockets; 165 | 166 | // for the remaining clients, just send some data if they are ready 167 | for (unsigned i = 1; i < status.size(); ++i) { 168 | if (status[i] & SK_WRITE) { 169 | try { 170 | if (!SendMessage(m_Clients[i], message.c_str(), message.length())) { 171 | closed_sockets.push_back(m_Clients[i]); 172 | } 173 | } 174 | catch (const std::exception &e) { 175 | std::cerr << get_peer_name(m_Clients[i]) << ": " << e.what() << std::endl; 176 | closed_sockets.push_back(m_Clients[i]); 177 | } 178 | } 179 | if (status[i] & SK_READ) { 180 | auto message = ReadMessage(m_Clients[i]); 181 | ProcessMessage(message); 182 | } 183 | } 184 | 185 | // remove any closed sockets from the list 186 | auto e = end(m_Clients); 187 | for (auto i = begin(closed_sockets); i != end(closed_sockets); i++) { 188 | std::cout << "Closing socket for " << get_peer_name(*i) << std::endl; 189 | e = std::remove(begin(m_Clients), e, *i); 190 | closesocket(*i); 191 | } 192 | m_Clients.erase(e, end(m_Clients)); 193 | } 194 | 195 | void TelemetryServer::ProcessMessage(const std::string &message) 196 | { 197 | //std::cout << "Received message: <" << message << ">\n"; 198 | std::istringstream input(message); 199 | std::string command; 200 | double param; 201 | input >> command >> param; 202 | if(command == "SET-SLOPE" && m_Fec) { 203 | m_Fec->SetSlope(param); 204 | } 205 | } 206 | -------------------------------------------------------------------------------- /src/TelemetryServer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * TelemetryServer -- manage a bike trainer 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #pragma once 19 | #include 20 | #include "FitnessEquipmentControl.h" 21 | #include "HeartRateMonitor.h" 22 | #include "NetTools.h" 23 | 24 | // Hold information about a "current" reading from the trainer. We quote 25 | // "current" because data comes from different sources and might not be 26 | // completely in sync. 27 | struct Telemetry 28 | { 29 | Telemetry() 30 | : hr(-1), cad(-1), spd(-1), pwr(-1) {} 31 | double hr; 32 | double cad; 33 | double spd; 34 | double pwr; 35 | }; 36 | 37 | std::ostream& operator<<(std::ostream &out, const Telemetry &t); 38 | 39 | class TelemetryServer { 40 | public: 41 | TelemetryServer (AntStick *stick, int port = 7500); 42 | ~TelemetryServer(); 43 | 44 | void Tick(); 45 | 46 | private: 47 | 48 | void CheckSensorHealth(); 49 | void CollectTelemetry (Telemetry &out); 50 | void ProcessClients (const Telemetry &t); 51 | void ProcessMessage(const std::string &message); 52 | 53 | std::vector m_Clients; 54 | AntStick *m_AntStick; 55 | HeartRateMonitor *m_Hrm; 56 | FitnessEquipmentControl *m_Fec; 57 | }; 58 | -------------------------------------------------------------------------------- /src/Tools.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Tools -- various utilities 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include "Tools.h" 20 | 21 | #pragma warning (push) 22 | #pragma warning (disable:4200) 23 | #include 24 | #pragma warning (pop) 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #ifdef WIN32 31 | #pragma comment (lib, "libusb-1.0.lib") 32 | // winmm is needed for the timeGetTime() call 33 | #pragma comment (lib, "winmm.lib") 34 | #endif 35 | 36 | 37 | // ........................................................ LibusbError .... 38 | 39 | LibusbError::~LibusbError() 40 | { 41 | // empty 42 | } 43 | 44 | const char* LibusbError::what() const /*noexcept(true)*/ 45 | { 46 | if (!m_MessageDone) { 47 | std::ostringstream msg; 48 | msg << m_Who << ": (" << m_ErrorCode << ") " 49 | << libusb_error_name(m_ErrorCode); 50 | m_Message = msg.str(); 51 | m_MessageDone = true; 52 | } 53 | return m_Message.c_str(); 54 | } 55 | 56 | 57 | // ......................................................... Win32Error .... 58 | 59 | Win32Error::Win32Error (const std::string &who, unsigned long error) 60 | : m_Who (who), 61 | m_ErrorCode(error), 62 | m_MessageDone(false) 63 | { 64 | if (m_ErrorCode == 0) 65 | m_ErrorCode = GetLastError(); 66 | } 67 | 68 | Win32Error::~Win32Error() 69 | { 70 | // empty 71 | } 72 | 73 | const char* Win32Error::what() const 74 | { 75 | if (! m_MessageDone) { 76 | LPVOID buf = NULL; 77 | 78 | FormatMessageA( 79 | FORMAT_MESSAGE_ALLOCATE_BUFFER | 80 | FORMAT_MESSAGE_FROM_SYSTEM, 81 | NULL, 82 | m_ErrorCode, 83 | MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), 84 | (LPSTR) &buf, 85 | 0, NULL ); 86 | 87 | // remove the newline from the end 88 | char *p = strchr((char*)buf, '\r'); 89 | if (p) { 90 | *p = '\0'; 91 | } 92 | 93 | std::ostringstream msg; 94 | msg << m_Who << " error " << m_ErrorCode << ": " << (char*)buf; 95 | m_Message = msg.str(); 96 | LocalFree(buf); 97 | m_MessageDone = true; 98 | } 99 | 100 | return m_Message.c_str(); 101 | } 102 | 103 | 104 | /** Print a hex dump of 'data' to the stream 'o'. The data is printed on 105 | * lines with the address, character representation and hex representation on 106 | * each line. This hopefully makes it easy to determine the contents of both 107 | * character and binary data. 108 | */ 109 | void DumpData (const unsigned char *data, int size, std::ostream &o) 110 | { 111 | int ncols = 16; 112 | int nrows = size / ncols; 113 | int spill = size - nrows * ncols; 114 | std::ios::fmtflags saved = o.flags(); 115 | 116 | auto pchar = [] (char c, std::locale &loc) -> char 117 | { 118 | char npc = '?'; // char to print if the character is not 119 | // printable 120 | if (std::isspace (c, loc) && c != ' ') 121 | return npc; 122 | 123 | if (std::isprint (c, loc)) 124 | return c; 125 | 126 | return npc; 127 | }; 128 | 129 | std::locale loc = o.getloc(); 130 | o << std::hex << std::setfill ('0'); 131 | 132 | for (int row = 0; row < nrows; ++row) 133 | { 134 | o << std::setw (4) << row*ncols << " "; 135 | for (int col = 0; col < ncols; ++col) 136 | { 137 | int idx = row * ncols + col; 138 | o << pchar(data[idx], loc); 139 | } 140 | o << '\t'; 141 | for (int col = 0; col < ncols; ++col) 142 | { 143 | int idx = row * ncols + col; 144 | o << std::setw (2) << static_cast(data[idx]) << " "; 145 | } 146 | o << '\n'; 147 | } 148 | 149 | if (spill) 150 | { 151 | o << std::setw (4) << nrows*ncols << " "; 152 | for (int idx = size - spill; idx < size; ++idx) 153 | { 154 | o << pchar (data[idx], loc); 155 | } 156 | 157 | for (int idx = 0; idx < ncols - spill; ++idx) 158 | { 159 | o << ' '; 160 | } 161 | 162 | o << '\t'; 163 | for (int idx = size - spill; idx < size; ++idx) 164 | { 165 | o << std::setw (2) << static_cast(data[idx]) << " "; 166 | } 167 | 168 | o << '\n'; 169 | } 170 | 171 | o.flags (saved); 172 | } 173 | 174 | uint32_t CurrentMilliseconds() 175 | { 176 | return timeGetTime(); 177 | } 178 | 179 | #if 0 180 | void PutTimestamp(std::ostream &o) 181 | { 182 | struct timespec tsp; 183 | 184 | if (clock_gettime(CLOCK_REALTIME, &tsp) < 0) { 185 | perror("clock_gettime"); 186 | return; 187 | } 188 | struct tm tm = *localtime(&tsp.tv_sec); 189 | 190 | unsigned msec = tsp.tv_nsec / 1000000; 191 | 192 | o << std::setfill('0') 193 | << std::setw(4) << tm.tm_year + 1900 194 | << '-' << std::setw(2) << tm.tm_mon + 1 195 | << '-' << std::setw(2) << tm.tm_mday 196 | << ' ' << std::setw(2) << tm.tm_hour 197 | << ':' << std::setw(2) << tm.tm_min 198 | << ':' << std::setw(2) << tm.tm_sec 199 | << '.' << std::setw(4) << msec << ' '; 200 | } 201 | #endif 202 | 203 | -------------------------------------------------------------------------------- /src/Tools.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Tools -- various utilities 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | 29 | // .................................................... LibusbError .... 30 | 31 | /** Convenience class to throw exception with USB error codes and get proper 32 | * error names for them. 33 | */ 34 | class LibusbError : public std::exception 35 | { 36 | public: 37 | LibusbError(const char *who, int error_code) 38 | : m_Who(who), m_ErrorCode(error_code), m_MessageDone(false) 39 | { 40 | } 41 | virtual ~LibusbError(); 42 | 43 | const char* what() const /*noexcept(true)*/; 44 | int error_code() const { return m_ErrorCode; } 45 | 46 | private: 47 | std::string m_Who; 48 | int m_ErrorCode; 49 | mutable std::string m_Message; 50 | mutable bool m_MessageDone; 51 | }; 52 | 53 | 54 | // ......................................................... Win32Error .... 55 | 56 | /** Convenience class to throw exceptions with Win32 error codes and get 57 | * proper error names for them. Works with GetLastError() and 58 | * WSAGetLastError() 59 | */ 60 | class Win32Error : public std::exception 61 | { 62 | public: 63 | Win32Error (const std::string &who = "", unsigned long error = 0); 64 | virtual ~Win32Error (); 65 | 66 | const char* what() const; 67 | unsigned long error() const { return m_ErrorCode; } 68 | 69 | private: 70 | std::string m_Who; 71 | unsigned long m_ErrorCode; 72 | mutable std::string m_Message; 73 | mutable bool m_MessageDone; 74 | }; 75 | 76 | 77 | /** Print a hex dump of 'data' to the stream 'o'. The data is printed on 78 | * lines with the address, character representation and hex representation on 79 | * each line. This hopefully makes it easy to determine the contents of both 80 | * character and binary data. 81 | */ 82 | void DumpData (const unsigned char *data, int size, std::ostream &o); 83 | 84 | 85 | /** Return a timestamps in milliseconds from an unspecified epoch. Can be 86 | * used to measure real time from the difference between two subsequent calls 87 | * to the function. Timer resolution is unspecified, but it is not very high, 88 | * measuring times of 50 ms or more should be ok. Less than that, not so 89 | * much. 90 | */ 91 | uint32_t CurrentMilliseconds(); 92 | 93 | #if 0 94 | /** Put the current time on the output stream o. */ 95 | void PutTimestamp(std::ostream &o); 96 | #endif 97 | 98 | /* 99 | Local Variables: 100 | mode: c++ 101 | End: 102 | */ 103 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * TrainServer -- prototype bike trainer application 3 | * Copyright (C) 2017 Alex Harsanyi (AlexHarsanyi@gmail.com) 4 | * 5 | * This program is free software: you can redistribute it and/or modify it 6 | * under the terms of the GNU General Public License as published by the Free 7 | * Software Foundation, either version 3 of the License, or (at your option) 8 | * any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, but WITHOUT 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 13 | * more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program. If not, see . 17 | */ 18 | #include "stdafx.h" 19 | #include "AntStick.h" 20 | #include "NetTools.h" 21 | #include "TelemetryServer.h" 22 | #include "Tools.h" 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | void ProcessChannels(AntStick &s, std::ostream &log) 30 | { 31 | try { 32 | TelemetryServer server (&s); 33 | while (true) { 34 | server.Tick(); 35 | } 36 | } 37 | catch (std::exception &e) { 38 | log << e.what() << std::endl; 39 | } 40 | } 41 | 42 | void ProcessAntSticks(std::ostream &log) 43 | { 44 | while (true) { 45 | try { 46 | AntStick a; 47 | auto t = std::time(nullptr); 48 | auto tm = *std::localtime(&t); 49 | log << std::put_time(&tm, "%c"); 50 | log << " USB Stick: Serial#: " << a.GetSerialNumber() 51 | << ", version " << a.GetVersion() 52 | << ", max " << a.GetMaxNetworks() << " networks, max " 53 | << a.GetMaxChannels() << " channels\n" << std::flush; 54 | a.SetNetworkKey(AntStick::g_AntPlusNetworkKey); 55 | ProcessChannels(a, log); 56 | } 57 | catch (const AntStickNotFound &e) { 58 | auto t = std::time(nullptr); 59 | auto tm = *std::localtime(&t); 60 | log << std::put_time(&tm, "%c"); 61 | log << e.what() << std::endl; 62 | return; 63 | } 64 | catch (std::exception &e) { 65 | auto t = std::time(nullptr); 66 | auto tm = *std::localtime(&t); 67 | log << std::put_time(&tm, "%c"); 68 | log << e.what() << std::endl; 69 | } 70 | } 71 | } 72 | 73 | int main() 74 | { 75 | try { 76 | int r = libusb_init(NULL); 77 | if (r < 0) 78 | throw LibusbError("libusb_init", r); 79 | ProcessAntSticks(std::cout); 80 | } 81 | catch (const std::exception &e) { 82 | std::cout << e.what() << "\n"; 83 | return 1; 84 | } 85 | return 0; 86 | } 87 | 88 | -------------------------------------------------------------------------------- /src/stdafx.cpp: -------------------------------------------------------------------------------- 1 | // stdafx.cpp : source file that includes just the standard includes 2 | // $safeprojectname$.pch will be the pre-compiled header 3 | // stdafx.obj will contain the pre-compiled type information 4 | 5 | #include "stdafx.h" 6 | 7 | // TODO: reference any additional headers you need in STDAFX.H 8 | // and not in this file 9 | -------------------------------------------------------------------------------- /src/stdafx.h: -------------------------------------------------------------------------------- 1 | // stdafx.h : include file for standard system include files, 2 | // or project specific include files that are used frequently, but 3 | // are changed infrequently 4 | // 5 | 6 | #pragma once 7 | #define _CRT_SECURE_NO_WARNINGS 8 | #define _WINSOCK_DEPRECATED_NO_WARNINGS 9 | #define _WINSOCKAPI_ // stops windows.h including winsock.h 10 | #include "targetver.h" 11 | 12 | #include 13 | #include 14 | 15 | 16 | 17 | // TODO: reference additional headers your program requires here 18 | -------------------------------------------------------------------------------- /src/targetver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Including SDKDDKVer.h defines the highest available Windows platform. 4 | 5 | // If you wish to build your application for a previous Windows platform, include WinSDKVer.h and 6 | // set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. 7 | 8 | #include 9 | -------------------------------------------------------------------------------- /vs2017/TrainerControl.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 15 4 | VisualStudioVersion = 15.0.27004.2005 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "TrainerControl", "TrainerControl\TrainerControl.vcxproj", "{AD6F5B83-5FB3-4224-82C0-0D69170A89C1}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|x64 = Debug|x64 11 | Debug|x86 = Debug|x86 12 | Release|x64 = Release|x64 13 | Release|x86 = Release|x86 14 | EndGlobalSection 15 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 16 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Debug|x64.ActiveCfg = Debug|x64 17 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Debug|x64.Build.0 = Debug|x64 18 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Debug|x86.ActiveCfg = Debug|Win32 19 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Debug|x86.Build.0 = Debug|Win32 20 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Release|x64.ActiveCfg = Release|x64 21 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Release|x64.Build.0 = Release|x64 22 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Release|x86.ActiveCfg = Release|Win32 23 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1}.Release|x86.Build.0 = Release|Win32 24 | EndGlobalSection 25 | GlobalSection(SolutionProperties) = preSolution 26 | HideSolutionNode = FALSE 27 | EndGlobalSection 28 | GlobalSection(ExtensibilityGlobals) = postSolution 29 | SolutionGuid = {80295DC2-9CD5-455D-9F33-4FAAB3FC90FE} 30 | EndGlobalSection 31 | EndGlobal 32 | -------------------------------------------------------------------------------- /vs2017/TrainerControl/TrainerControl.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | Create 38 | Create 39 | 40 | 41 | 42 | 43 | 44 | 45 | 15.0 46 | {AD6F5B83-5FB3-4224-82C0-0D69170A89C1} 47 | Win32Proj 48 | TrainerControl 49 | 10.0.17134.0 50 | 51 | 52 | 53 | Application 54 | true 55 | v141 56 | Unicode 57 | 58 | 59 | Application 60 | false 61 | v141 62 | true 63 | Unicode 64 | 65 | 66 | Application 67 | true 68 | v141 69 | Unicode 70 | 71 | 72 | Application 73 | false 74 | v141 75 | true 76 | Unicode 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | true 98 | $(SolutionDir)..\$(Configuration)\ 99 | 100 | 101 | true 102 | 103 | 104 | false 105 | $(SolutionDir)..\$(Configuration)\ 106 | 107 | 108 | false 109 | 110 | 111 | 112 | Use 113 | Level3 114 | Disabled 115 | true 116 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 117 | 118 | 119 | 120 | 121 | Console 122 | true 123 | 124 | 125 | %(AdditionalDependencies) 126 | 127 | 128 | 129 | 130 | Use 131 | Level3 132 | Disabled 133 | true 134 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 135 | 136 | 137 | Console 138 | true 139 | 140 | 141 | 142 | 143 | Use 144 | Level3 145 | MaxSpeed 146 | true 147 | true 148 | true 149 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 150 | 151 | 152 | 153 | 154 | Console 155 | true 156 | true 157 | true 158 | 159 | 160 | %(AdditionalDependencies) 161 | 162 | 163 | 164 | 165 | Use 166 | Level3 167 | MaxSpeed 168 | true 169 | true 170 | true 171 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 172 | 173 | 174 | Console 175 | true 176 | true 177 | true 178 | 179 | 180 | 181 | 182 | 183 | 184 | -------------------------------------------------------------------------------- /vs2017/TrainerControl/TrainerControl.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Header Files 20 | 21 | 22 | Header Files 23 | 24 | 25 | Header Files 26 | 27 | 28 | Header Files 29 | 30 | 31 | Header Files 32 | 33 | 34 | Header Files 35 | 36 | 37 | Header Files 38 | 39 | 40 | Header Files 41 | 42 | 43 | 44 | 45 | Source Files 46 | 47 | 48 | Source Files 49 | 50 | 51 | Source Files 52 | 53 | 54 | Source Files 55 | 56 | 57 | Source Files 58 | 59 | 60 | Source Files 61 | 62 | 63 | Source Files 64 | 65 | 66 | Source Files 67 | 68 | 69 | --------------------------------------------------------------------------------