CodexPad Arduino Lib 5.0.0
Loading...
Searching...
No Matches
client.cpp
1#include "client.h"
2
3#include <Arduino.h>
4
5#include <memory>
6
7#include "WString.h"
8
9namespace codex_pad {
10namespace {
11constexpr uint16_t kGapServiceUuid{0x1800};
12constexpr uint16_t kGapDeviceNameUuid{0x2A00};
13
14constexpr uint16_t kInputsServiceUuid{0xFFA0};
15constexpr uint16_t kInputsCharacteristicUuid{0xFFA1};
16
17constexpr uint16_t kBatteryServiceUuid{0x180F};
18constexpr uint16_t kBatteryLevelCharacteristicUuid{0x2A19};
19
20constexpr uint16_t kDeviceInfoServiceUuid{0x180A};
21constexpr uint16_t kModelNumberCharacteristicUuid{0x2A24};
22constexpr uint16_t kSerialNumberCharacteristicUuid{0x2A25};
23constexpr uint16_t kFirmwareRevisionCharacteristicUuid{0x2A26};
24constexpr uint16_t kManufacturerNameCharacteristicUuid{0x2A29};
25
26constexpr uint16_t kConnectionParamMinInterval{6};
27constexpr uint16_t kConnectionParamMaxInterval{8};
28constexpr uint16_t kConnectionParamLatency{5};
29constexpr uint16_t kConnectionParamSupervisionTimeout{30};
30
31static_assert(kConnectionParamSupervisionTimeout * 10.0 >
32 (1 + kConnectionParamLatency) * kConnectionParamMaxInterval * 4 * 1.25);
33
34constexpr uint64_t kUpdateTxPowerIntervalMs{1000};
35
36// RSSI 滞环量化器 (HysteresisQuantizer) 参数
37constexpr int32_t kRssiQuantizerMin = -90; // RSSI 最小值(信号最差)
38constexpr int32_t kRssiQuantizerMax = -40; // RSSI 最大值(信号最好)
39constexpr uint32_t kRssiQuantizerLevels = 4; // 量化级别数量(0 ~ 3)
40constexpr int32_t kRssiQuantizerDivisor = 3; // 滞环大小除数
41
42constexpr uint32_t kRssiQuantizerMaxLevel = kRssiQuantizerLevels - 1;
43constexpr int32_t kTxPowerLevelMultiplier = 3; // 功率等级到实际 dBm(或单位)的乘数
44} // namespace
45
47 : rssi_quantizer_(kRssiQuantizerMin, kRssiQuantizerMax, kRssiQuantizerLevels, kRssiQuantizerDivisor) {}
48
49Client::~Client() noexcept { Reset(); }
50
51void Client::Init() noexcept {
52 if (!NimBLEDevice::isInitialized()) {
53 NimBLEDevice::init("CodexPadClient");
54 }
55}
56
57bool Client::Connect(const std::string& address, const uint32_t timeout_ms) noexcept {
58 // check mac address is valid
59 if (address.length() != 17 || address[2] != ':' || address[5] != ':' || address[8] != ':' || address[11] != ':' ||
60 address[14] != ':') {
61 abort();
62 return false;
63 }
64
65 return Connect(NimBLEAddress(address, 0), false, timeout_ms);
66}
67
68bool Client::Connect(const gamepad::input::Button buttons) noexcept {
69 auto scanner = NimBLEDevice::getScan();
70 scanner->setActiveScan(true); // active scan uses more power, but get results faster
71 scanner->setInterval(1000);
72 scanner->setWindow(999); // less or equal setInterval value
73
74 if (!scanner->start(1000)) {
75 scanner->stop();
76 scanner->clearResults();
77 // printf("Scan failed\n");
78 return false;
79 }
80
81 while (scanner->isScanning()) {
82 delay(100);
83 }
84
85 // printf("Scan done, device count: %d\n", scanner->getResults().getCount());
86
87 int8_t rssi = INT8_MIN;
88 NimBLEAddress address;
89
90#pragma pack(push, 1)
91 struct ManufacturerSpecificData {
92 uint16_t company_id = 0xFFFF;
93 uint8_t header[8] = {'C', 'o', 'd', 'e', 'x', 'P', 'a', 'd'};
94 uint8_t version_major = 0;
95 uint8_t version_minor = 0;
96 uint8_t version_patch = 0;
97 uint32_t button_state = 0;
98 uint8_t button_states_duration_seconds = 0;
99 };
100#pragma pack(pop)
101
102 for (const auto device : scanner->getResults()) {
103 if (device->haveName() && String(device->getName().c_str()).startsWith("CodexPad-") && device->haveManufacturerData()) {
104 // printf("Name: %s\n", device->getName().c_str());
105 const auto manufacturer_data = device->getManufacturerData();
106 if (manufacturer_data.length() >= sizeof(ManufacturerSpecificData)) {
107 const auto data = reinterpret_cast<const ManufacturerSpecificData*>(manufacturer_data.c_str());
108 if (data->company_id == 0xFFFF // company id
109 && memcmp(data->header, "CodexPad", 8) == 0 // header
110 && data->button_state == static_cast<uint32_t>(buttons) // button mask
111 && device->getRSSI() > rssi // rssi
112 && (data->version_major < 2 || data->button_states_duration_seconds >= 1) // button states duration
113 ) {
114 rssi = device->getRSSI();
115 address = device->getAddress();
116 // printf("Found device, rssi: %d, address: %s\n", rssi, address.toString().c_str());
117 }
118 }
119 }
120 }
121
122 scanner->clearResults();
123
124 return address.isNull() ? false : Connect(address, 2000);
125}
126
127const gamepad::input::Tracker& Client::Update() noexcept {
128 if (ble_client_ == nullptr) {
129 return input_tracker_;
130 }
131
132 if (!ble_client_->isConnected()) {
133 Reset();
134 return input_tracker_;
135 }
136
137 input_tracker_.Tick();
138
139 do {
140 std::lock_guard<std::mutex> l(mutex_);
141 if (inputs_queue_.empty()) {
142 break;
143 }
144
145 input_tracker_.Update(inputs_queue_.front());
146 inputs_queue_.pop();
147 } while (false);
148
149 // Dynamic TX power adjustment is only reliable on ESP32 Arduino Core >= 3.1.0
150 // because older versions have unstable (drifting) RSSI readings.
151#if !defined(ESP_ARDUINO_VERSION) || ESP_ARDUINO_VERSION >= ESP_ARDUINO_VERSION_VAL(3, 1, 0)
152 UpdateTxPower();
153#endif
154
155 return input_tracker_;
156}
157
158bool Client::is_connected() const noexcept { return ble_client_ != nullptr && ble_client_->isConnected(); }
159
160bool Client::Connect(const NimBLEAddress& address, bool async_connect, const uint32_t timeout_ms) {
161 Reset();
162 assert(ble_client_ == nullptr);
163 ble_client_ = NimBLEDevice::createClient(address);
164 ble_client_->setConnectTimeout(timeout_ms);
165 ble_client_->setConnectionParams(kConnectionParamMinInterval, kConnectionParamMaxInterval, kConnectionParamLatency,
166 kConnectionParamSupervisionTimeout);
167 ble_client_->setClientCallbacks(this, false);
168 auto ret = ble_client_->connect(true, async_connect, true);
169
170 if (!ret || !ble_client_->isConnected()) {
171 goto FAILED;
172 }
173
174 remote_device_name_ = ble_client_->getValue(kGapServiceUuid, kGapDeviceNameUuid);
175 remote_model_number_ = ble_client_->getValue(kDeviceInfoServiceUuid, kModelNumberCharacteristicUuid);
176 {
177 auto firmware_revision = ble_client_->getValue(kDeviceInfoServiceUuid, kFirmwareRevisionCharacteristicUuid);
178 if (firmware_revision.length() == remote_firmware_version_.size()) {
179 memcpy(remote_firmware_version_.data(), firmware_revision.data(), firmware_revision.length());
180 }
181 }
182
183 {
184 auto remote_service = ble_client_->getService(kInputsServiceUuid);
185 if (remote_service == nullptr) {
186 goto FAILED;
187 }
188
189 auto remote_characteristic = remote_service->getCharacteristic(kInputsCharacteristicUuid);
190 if (remote_characteristic == nullptr) {
191 goto FAILED;
192 }
193
194 if (!remote_characteristic->canNotify()) {
195 goto FAILED;
196 }
197
198 if (!remote_characteristic->subscribe(true, std::bind(&Client::OnNotify, this, std::placeholders::_1, std::placeholders::_2,
199 std::placeholders::_3, std::placeholders::_4))) {
200 goto FAILED;
201 }
202 }
203
204 ble_client_->updatePhy(BLE_GAP_LE_PHY_CODED_MASK, BLE_GAP_LE_PHY_CODED_MASK, BLE_GAP_LE_PHY_CODED_S8);
205
206 if (local_tx_power_ != INT32_MIN) {
207 NimBLEDevice::setPower(local_tx_power_, NimBLETxPowerType::All);
208 }
209
210 return ret;
211
212FAILED:
213 Reset();
214 return false;
215}
216
217void Client::OnNotify(const NimBLERemoteCharacteristic* remote_characteristic, const uint8_t* data, const size_t length,
218 const bool is_notify) {
219 if (remote_characteristic != nullptr && remote_characteristic->getUUID().equals(kInputsCharacteristicUuid)) {
220 if (length != sizeof(gamepad::input::State)) {
221 printf("WARNING: length != sizeof(Inputs)\n");
222 return;
223 }
224
225 std::lock_guard<std::mutex> l(mutex_);
226 if (inputs_queue_.size() > kInputsQueueMax) {
227 inputs_queue_.pop();
228 }
229 inputs_queue_.emplace(gamepad::input::State::FromBytes(data));
230 }
231}
232
233void Client::Reset() {
234 if (ble_client_ != nullptr) {
235 ble_client_->cancelConnect();
236 ble_client_->disconnect();
237 NimBLEDevice::deleteClient(ble_client_);
238 ble_client_ = nullptr;
239 }
240
241 remote_device_name_.clear();
242 remote_model_number_.clear();
243 remote_firmware_version_.fill(0);
244 input_tracker_.Reset();
245 std::lock_guard<std::mutex> l(mutex_);
246 inputs_queue_ = {};
247 rssi_quantizer_.Reset();
248 last_update_tx_power_ms_ = 0;
249}
250
251void Client::UpdateTxPower() {
252 if (!dynamic_tx_power_enabled_) {
253 return;
254 }
255
256 if (ble_client_ == nullptr) {
257 return;
258 }
259
260 if (!ble_client_->isConnected()) {
261 return;
262 }
263
264 if (last_update_tx_power_ms_ != 0 && last_update_tx_power_ms_ + kUpdateTxPowerIntervalMs > millis()) {
265 return;
266 }
267
268 last_update_tx_power_ms_ = millis();
269
270 const int32_t rssi = ble_client_->getRssi();
271 const uint32_t tx_power_level = kRssiQuantizerMaxLevel - rssi_quantizer_.Update(rssi);
272 NimBLEDevice::setPower(tx_power_level * kTxPowerLevelMultiplier, NimBLETxPowerType::All);
273}
274
275int32_t Client::rssi() const noexcept { return ble_client_ == nullptr ? 0 : ble_client_->getRssi(); }
276
277void Client::onPhyUpdate(NimBLEClient*, [[maybe_unused]] uint8_t tx_phy, [[maybe_unused]] uint8_t rx_phy) {
278 // CLOGI("tx_phy: %d, rx_phy: %d", tx_phy, rx_phy);
279}
280} // namespace codex_pad
Client() noexcept
Constructor.
Definition client.cpp:46
const gamepad::input::Tracker & Update() noexcept
Update, need to be called in Loop.
Definition client.cpp:127
bool is_connected() const noexcept
Is connected.
Definition client.cpp:158
void Init() noexcept
Initialize.
Definition client.cpp:51
int32_t rssi() const noexcept
Get the RSSI of the connected CodexPad.
Definition client.cpp:275
~Client() noexcept
Destructor.
Definition client.cpp:49
bool Connect(const std::string &address, uint32_t timeout_ms=5000) noexcept
Connect.
Definition client.cpp:57