Request capabilites on boot

Author: cujomalainey

src/ANTPLUS_Defines.h

@@ -5,9 +5,11 @@
 #define ANTPLUS_MAX_CHANNELS_POSSIBLE 15
 #define ANTPLUS_NETWORKKEY_INDEX 0
 #define ANTPLUS_DRIVER_RESET_TIMEOUT 2000
+#define ANTPLUS_DRIVER_REQUEST_TIMEOUT 1000
 
 // Errors
 #define ANTPLUS_RESET_RADIO_FAILED 1
+#define ANTPLUS_MAX_CHANNEL_CHECK_FAILED 2
 
 // State
 #define ANTPLUS_DRIVER_STATE_UNKNOWN 0

src/MainClasses/ANTPLUS_AntPlusRouter.cpp

@@ -3,7 +3,7 @@
 #include <ANTPLUS_PrivateDefines.h>
 
 AntPlusRouter::AntPlusRouter() {
-    // TODO
+
 }
 
 AntPlusRouter::AntPlusRouter(BaseAntWithCallbacks* driver) {
@@ -16,6 +16,9 @@ AntPlusRouter::AntPlusRouter(BaseAntWithCallbacks* driver, const uint8_t* key) {
 }
 
 uint8_t AntPlusRouter::setDriver(BaseAntWithCallbacks* driver) {
+    // Synchronous locking nature needed to gurantee all configs were recieved
+    RequestMessage rm = RequestMessage();
+    Capabilities cap = Capabilities();
     _ant = driver;
     // register callbacks
     _ant->onPacketError(onPacketErrorCallback, (uintptr_t)this);
@@ -37,7 +40,11 @@ uint8_t AntPlusRouter::setDriver(BaseAntWithCallbacks* driver) {
         return ANTPLUS_RESET_RADIO_FAILED;
     }
     pushNetworkKey();
-    // get max channels
+    rm.setRequestedMessage(CAPABILITIES);
+    _ant->send(rm);
+    if (_ant->waitFor(cap, ANTPLUS_DRIVER_REQUEST_TIMEOUT)) {
+        return ANTPLUS_MAX_CHANNEL_CHECK_FAILED;
+    }
     return 0;
 }