389 lines
14 KiB
C++
389 lines
14 KiB
C++
#include "ChronoBLE.h"
|
|
#include <NimBLEDevice.h>
|
|
|
|
// Flag to signal that a new reading is ready to be processed
|
|
volatile bool newReadingAvailable = false;
|
|
volatile int lastReadFPS = 0;
|
|
static NimBLEScan* pScan;
|
|
static NimBLEClient* _pClient = nullptr;
|
|
static ChronoBLE::ConnectionState currentState;
|
|
static const NimBLEAdvertisedDevice* advDevice = nullptr;
|
|
static uint32_t scanTimeMs = 1000; //5000
|
|
static uint32_t scanTimeoutMs = 1000;
|
|
static NimBLEUUID deviceUUID("0000180a-0000-1000-8000-00805f9b34fb");
|
|
static NimBLEUUID serviceUUID("00001623-88EC-688C-644B-3FA706C0BB75");
|
|
static NimBLEUUID speedCharacteristicUUID("00001624-88EC-688C-644B-3FA706C0BB75");
|
|
static NimBLEUUID profileSettingLowUUID("00001626-88EC-688C-644B-3FA706C0BB75");
|
|
static NimBLEUUID profileSettingHighUUID("00001628-88EC-688C-644B-3FA706C0BB75");
|
|
static const char * charUUIDs[] = {
|
|
"00001624-88EC-688C-644B-3FA706C0BB75", //notify for speed value?
|
|
"00001625-88EC-688C-644B-3FA706C0BB75", //00
|
|
"00001626-88EC-688C-644B-3FA706C0BB75", //power setting low?
|
|
"00001627-88EC-688C-644B-3FA706C0BB75", //battery level
|
|
"00001628-88EC-688C-644B-3FA706C0BB75", // power setting high?
|
|
"00001629-88EC-688C-644B-3FA706C0BB75", //00
|
|
"0000162A-88EC-688C-644B-3FA706C0BB75",
|
|
"00002902-0000-1000-8000-00805f9b34fb"
|
|
};
|
|
|
|
void (*speedCallback)(int speed);
|
|
|
|
//specific to FX chrono settings
|
|
uint8_t profile_bytes[2][5] = {{0x32, 0x32, 0x32, 0x32, 0x64},{0x17, 0x1E, 0x2D, 0x43, 0x5A}};
|
|
|
|
|
|
//connection callback
|
|
class ClientCallbacks : public NimBLEClientCallbacks {
|
|
void onConnect(NimBLEClient* pClient) override { Serial.printf("Connected\n"); }
|
|
|
|
void onDisconnect(NimBLEClient* pClient, int reason) override {
|
|
Serial.printf("%s Disconnected, reason = %d\n", pClient->getPeerAddress().toString().c_str(), reason);
|
|
//NimBLEDevice::getScan()->start(scanTimeMs, false, true);
|
|
currentState = ChronoBLE::IDLE;
|
|
}
|
|
} clientCallbacks;
|
|
|
|
//device scan callback
|
|
class ScanCallbacks : public NimBLEScanCallbacks {
|
|
void onResult(const NimBLEAdvertisedDevice* advertisedDevice) override {
|
|
//Serial.printf("Advertised Device found: %s\n", advertisedDevice->toString().c_str());
|
|
if (advertisedDevice->isAdvertisingService(deviceUUID)) {
|
|
//Serial.printf("Found device!!!\n");
|
|
/** stop scan before connecting */
|
|
NimBLEDevice::getScan()->stop();
|
|
/** Save the device reference in a global for the client to use*/
|
|
advDevice = advertisedDevice;
|
|
}
|
|
}
|
|
|
|
/** Callback to process the results of the completed scan or restart it */
|
|
void onScanEnd(const NimBLEScanResults& results, int reason) override {
|
|
//Serial.printf("Scan Ended, reason: %d, device count: %d; Restarting scan\n", reason, results.getCount());
|
|
delay(scanTimeoutMs);
|
|
pScan->start(scanTimeMs, false, false);
|
|
}
|
|
} scanCallbacks;
|
|
|
|
//notification callback
|
|
void notifyCB(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) {
|
|
uint16_t speed;
|
|
|
|
// Extract speed from data - only what's needed, avoid String allocations
|
|
if (length >= 2) {
|
|
speed = pData[0];
|
|
speed <<= 8;
|
|
speed |= pData[1];
|
|
|
|
if (speed > 0) {
|
|
// Convert to FPS
|
|
float fspeed = speed * 0.0475111859;
|
|
|
|
// Store the reading for later processing in main loop
|
|
// This avoids heap fragmentation and display updates in interrupt context
|
|
lastReadFPS = (int)fspeed;
|
|
newReadingAvailable = true;
|
|
|
|
Serial.printf("%d FPS\n", (int)fspeed);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// Constructor implementation
|
|
ChronoBLE::ChronoBLE() {
|
|
_setState(IDLE);
|
|
}
|
|
void ChronoBLE::init() {
|
|
NimBLEDevice::init("FX-Chrono-Client");
|
|
NimBLEDevice::setPower(3); /** 3dbm */
|
|
pScan = NimBLEDevice::getScan();
|
|
pScan->setScanCallbacks(&scanCallbacks, false);
|
|
pScan->setInterval(100);
|
|
pScan->setWindow(99);
|
|
pScan->setActiveScan(false);
|
|
}
|
|
void ChronoBLE::_setState(ConnectionState state){
|
|
currentState=state;
|
|
}
|
|
void ChronoBLE::setSpeedCallback(void (*pFunc)(int speed)){speedCallback=pFunc;}
|
|
bool ChronoBLE::isIdle(){if (currentState==IDLE){return true;}return false;}
|
|
bool ChronoBLE::isConnected(){
|
|
if (currentState==CONNECTED){
|
|
if (_pClient!= nullptr){
|
|
if (!_pClient->isConnected()){
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
_disconnectDevice();
|
|
}
|
|
return false;
|
|
}
|
|
bool ChronoBLE::foundDevice(){if (advDevice!=nullptr){return true;}return false;}
|
|
void ChronoBLE::startScan(){
|
|
Serial.println("starting scan!!!");
|
|
_setState(SCANNING);
|
|
pScan->start(scanTimeMs, false, false);
|
|
}
|
|
void ChronoBLE::_disconnectDevice(){
|
|
_setState(IDLE);
|
|
advDevice = nullptr;
|
|
}
|
|
bool ChronoBLE::connectToDevice() {
|
|
_setState(CONNECTING);
|
|
|
|
/** Check if we have a client we should reuse first **/
|
|
if (NimBLEDevice::getCreatedClientCount()) {
|
|
/**
|
|
* Special case when we already know this device, we send false as the
|
|
* second argument in connect() to prevent refreshing the service database.
|
|
* This saves considerable time and power.
|
|
*/
|
|
_pClient = NimBLEDevice::getClientByPeerAddress(advDevice->getAddress());
|
|
if (_pClient) {
|
|
if (!_pClient->connect(advDevice, false)) {
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
} else {
|
|
/**
|
|
* We don't already have a client that knows this device,
|
|
* check for a client that is disconnected that we can use.
|
|
*/
|
|
_pClient = NimBLEDevice::getDisconnectedClient();
|
|
}
|
|
}
|
|
|
|
/** No client to reuse? Create a new one. */
|
|
if (!_pClient) {
|
|
if (NimBLEDevice::getCreatedClientCount() >= NIMBLE_MAX_CONNECTIONS) {
|
|
Serial.printf("Max clients reached - no more connections available\n");
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
|
|
_pClient = NimBLEDevice::createClient();
|
|
|
|
_pClient->setClientCallbacks(&clientCallbacks, false);
|
|
/**
|
|
* Set initial connection parameters:
|
|
* These settings are safe for 3 clients to connect reliably, can go faster if you have less
|
|
* connections. Timeout should be a multiple of the interval, minimum is 100ms.
|
|
* Min interval: 12 * 1.25ms = 15, Max interval: 12 * 1.25ms = 15, 0 latency, 150 * 10ms = 1500ms timeout
|
|
*/
|
|
_pClient->setConnectionParams(12, 12, 0, 150);
|
|
|
|
/** Set how long we are willing to wait for the connection to complete (milliseconds), default is 30000. */
|
|
_pClient->setConnectTimeout(5 * 1000);
|
|
|
|
if (!_pClient->connect(advDevice)) {
|
|
/** Created a client but failed to connect, don't need to keep it as it has no data */
|
|
NimBLEDevice::deleteClient(_pClient);
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (!_pClient->isConnected()) {
|
|
if (!_pClient->connect(advDevice)) {
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/** Now we can read/write/subscribe the characteristics of the services we are interested in */
|
|
NimBLERemoteService* pSvc = nullptr;
|
|
NimBLERemoteCharacteristic* pChr = nullptr;
|
|
NimBLERemoteDescriptor* pDsc = nullptr;
|
|
|
|
//set speed profile to default: FAC
|
|
/* if (!setSpeedProfile(0x64, 0x5A)){
|
|
Serial.println("unable to set speed profile!!!!");
|
|
return false;
|
|
}*/
|
|
|
|
pSvc = _pClient->getService(serviceUUID);
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(profileSettingLowUUID);
|
|
if (pChr) {
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
if (pChr->canWrite()) {
|
|
if (pChr->writeValue(profile_bytes[0][1])) {
|
|
Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
|
|
} else {
|
|
_pClient->disconnect();
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf("unable to set profile low speed setting\n");
|
|
}
|
|
|
|
//then, set the high speed profile on the device
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(profileSettingHighUUID);
|
|
if (pChr) {
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
if (pChr->canWrite()) {
|
|
if (pChr->writeValue(profile_bytes[1][1])) {
|
|
Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
|
|
} else {
|
|
_pClient->disconnect();
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf("unable to set profile high speed setting\n");
|
|
}
|
|
|
|
//finally, connect to speed characteristic and register for notifications
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(speedCharacteristicUUID);
|
|
if (pChr) {
|
|
if (pChr->canNotify()) {
|
|
if (!pChr->subscribe(true, notifyCB)) {
|
|
_pClient->disconnect();
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
}
|
|
else{
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
} else {
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
}
|
|
else {
|
|
_disconnectDevice();
|
|
return false;
|
|
}
|
|
|
|
//device is fully setup and connected
|
|
_setState(CONNECTED);
|
|
|
|
return true;
|
|
}
|
|
bool ChronoBLE::changeGunType(int type) {
|
|
|
|
if (_pClient == nullptr){return false;}
|
|
|
|
if (!_pClient->isConnected()) {
|
|
return false;
|
|
}
|
|
|
|
/** Now we can read/write/subscribe the characteristics of the services we are interested in */
|
|
NimBLERemoteService* pSvc = nullptr;
|
|
NimBLERemoteCharacteristic* pChr = nullptr;
|
|
NimBLERemoteDescriptor* pDsc = nullptr;
|
|
|
|
pSvc = _pClient->getService(serviceUUID);
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(profileSettingLowUUID);
|
|
if (pChr) {
|
|
if (pChr->canWrite()) {
|
|
if (pChr->writeValue(profile_bytes[0][type])) {
|
|
Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
|
|
} else {
|
|
return false;
|
|
}
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf("unable to set profile low speed setting\n");
|
|
}
|
|
|
|
//then, set the high speed profile on the device
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(profileSettingHighUUID);
|
|
if (pChr) {
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
if (pChr->canWrite()) {
|
|
if (pChr->writeValue(profile_bytes[1][type])) {
|
|
Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
|
|
} else {
|
|
return false;
|
|
}
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf("unable to set profile high speed setting\n");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
bool ChronoBLE::setSpeedProfile(uint8_t low, uint8_t high){
|
|
//first set the low speed profile on the device
|
|
/*NimBLERemoteService* pSvc = nullptr;
|
|
NimBLERemoteCharacteristic* pChr = nullptr;
|
|
pSvc = pClient->getService(serviceUUID);
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(profileSettingLowUUID);
|
|
if (pChr) {
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
if (pChr->canWrite()) {
|
|
if (pChr->writeValue(0x32)) {
|
|
Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
|
|
} else {
|
|
pClient->disconnect();
|
|
return false;
|
|
}
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf("unable to set profile low speed setting\n");
|
|
}
|
|
|
|
//then, set the high speed profile on the device
|
|
if (pSvc) {
|
|
pChr = pSvc->getCharacteristic(profileSettingHighUUID);
|
|
if (pChr) {
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
if (pChr->canWrite()) {
|
|
if (pChr->writeValue(0x17)) {
|
|
Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
|
|
} else {
|
|
pClient->disconnect();
|
|
return false;
|
|
}
|
|
//if (pChr->canRead()) {
|
|
//Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
|
|
//}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf("unable to set profile high speed setting\n");
|
|
}*/
|
|
} |