├── .gitignore ├── README.md ├── include └── README ├── large_app_partition.csv ├── lib └── README ├── platformio.ini ├── src ├── BLEIMUService.h ├── BLEMACAddressService.h ├── BLEServiceHandler.h ├── BLEServiceManager.cpp ├── BLEServiceManager.h ├── BLEUARTService.h ├── BNO055Dummy.h ├── Config.h ├── MQTTClient.cpp ├── MQTTClient.h ├── WiFiSupplicant.cpp ├── WiFiSupplicant.h ├── main.cpp ├── utils.cpp └── utils.h └── test └── README /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/extensions.json 5 | .vscode/launch.json 6 | .vscode/ipch 7 | data/mqtt.config 8 | data/wpa_supplicant.txt 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino BLE IMU 2 | 3 | This software runs on an ESP32 to publish BNO055 orientation data over a 4 | Bluetooth Low Energy (BLE) connection, and optionally MQTT/WiFi, for use with 5 | [osteele/imu-tools](https://github.com/osteele/imu-tools). 6 | 7 | ## BLE Services 8 | 9 | The software defines the following BLE services. 10 | 11 | ### IMU Service (`509B0001-EBE1-4AA5-BC51-11004B78D5CB`) 12 | 13 | Characteristics: 14 | 15 | * Sensor (READ; `509B0002-EBE1-4AA5-BC51-11004B78D5CB`): A packed structure of 16 | sensors readings. The message format is: 17 | * Version (uint8): 1 18 | * Flags (uint8): specifies which of the following fields are present. 19 | * Time (uint16): the low 16 bits of the MCU's millisecond time 20 | * Quaternion (optional; 4 x float32): quaternion w, x, y, z 21 | * [Other sensors are not currently implemented] 22 | 23 | This protocol has the property that a message with just the Quaternion 24 | position is exactly 20 bytes, or one BLE 4.0 packet. 25 | 26 | * Calibration (`509B0003-EBE1-4AA5-BC51-11004B78D5CB`; READ, NOTIFY; 4 x 27 | uint_8): The system, gyro, accel, and mag calibration values, each ranging 28 | 0..3. These are NOTIFYed when they change (from initial assumed values of 0). 29 | 30 | Characteristic data are transmitted in little-endian order, not network order. 31 | This matches the standard GATT profile characteristics such as the heart-rate 32 | characteristic. 33 | 34 | The device is polled 60 times / second. The sensor characteristic is always 35 | written and notified. The polling interval is a constant that is defined in the 36 | code. The system doesn't appear to be capable of transmitting at greater than 37 | ~126 samples/second. For greater rates, consider MQTT, or onboard processing of 38 | the data. 39 | 40 | ### MAC Address Service (`709F0001-37E3-439E-A338-23F00067988B`) 41 | 42 | Characteristics: 43 | 44 | * MAC address (`709F0002-37E3-439E-A338-23F00067988B`; READ; string). imu-tools 45 | uses the MAC address as a device id that persists across connections (the Web 46 | BLE API doesn't make the BLE device id available to code). The MAC address is 47 | used instead of the BLE address, so that a device that publishes both to WiFi 48 | and BLE can be uniquely identified across both protocols. 49 | 50 | * BLE Device Name (`709F0003-37E3-439E-A338-23F00067988B`; READ, WRITE, NOTIFY; 51 | string). This is persisted to Flash via SPIFFS. It's useful as a nickname, to 52 | identify multiple devices in a fleet management scenario. This is the name 53 | that appears in the Web BLE connection dialog. 54 | 55 | ### UART Service 56 | 57 | The **UART Service** (`6E400001-B5A3-F393-E0A9-E50E24DCCA9E`) uses the Nordic 58 | UART Service and Characteristic UUIDs. It currently responds to RX "ping" with 59 | "pong", and "ping\n" with "pong\n". It is for debugging and possible future 60 | extensions. 61 | 62 | Characteristics: 63 | 64 | * RX (`6E400002-B5A3-F393-E0A9-E50E24DCCA9E`; READ) 65 | * TX (`6E400003-B5A3-F393-E0A9-E50E24DCCA9E`; WRITE, NOTIFY) 66 | 67 | ## Installation 68 | 69 | 1. Install [PlatformIO](https://platformio.org). 70 | 2. Install the Arduino ESP32 Board. 71 | 3. Install the "Adafruit BNO055" and "Adafruit Unified Sensor" libraries. 72 | 4. Build and upload the project. 73 | 74 | This can all be done fairly easily by installing either the [PlatformIO Visual 75 | Studio Code extension](https://platformio.org/install/ide?install=vscode), or 76 | [PlatformIO for 77 | Atom](https://docs.platformio.org/en/latest/ide/atom.html#installation), and 78 | using the PlatformIO GUI within the editor. 79 | 80 | It should also be possible to install the code using the [PlatformIO Command 81 | Line](https://docs.platformio.org/en/latest/installation.html), or by opening 82 | `main.cpp` in the Arduino IDE and install the Arduino ESP32 board. 83 | 84 | ## MQTT over WiFi 85 | 86 | In order to publish MQTT messages, create the following files in this project's 87 | `data` directory. Replace `ExampleSsid` by the name of your WiFi network, and 88 | `examplePassword` by your WiFi network's password. Replace the `mqtt.config` 89 | host name, port number, username, and password by the address and credentials of 90 | a valid MQTT broker. 91 | 92 | `wpa_supplicant.txt` 93 | 94 | ExampleSsid 95 | examplePassword 96 | 97 | `mpqtt.config` 98 | 99 | m10.cloudmqtt.com 100 | 1883 101 | username 102 | password 103 | 104 | These files need to be downloaded to the ESP32. In PlatformIO IDE: 105 | 106 | * Open the vscode [command 107 | palette](https://code.visualstudio.com/docs/getstarted/tips-and-tricks#_command-palette). 108 | * Type "Run Task", and select the "Tasks: Run Task" menu item. 109 | * If the following menu ends with an item "Show all tasks", select this item. 110 | * Type "file", and select the "Platform IO: Upload File System Image" menu item. 111 | 112 | This copies the files the files from `./data` to the attached ESP's 113 | [SPiFFS](https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/storage/spiffs.html) 114 | file system. 115 | 116 | If you want to use the command-line, instead of the PlatformIO IDE, instructions 117 | are 118 | [here](https://docs.platformio.org/en/latest/platforms/espressif32.html#uploading-files-to-file-system-spiffs). 119 | 120 | The WPA supplicant file may contain multiple groups of ssid and password, 121 | optionally separated by a blank line. Whhen the device boots, it scans for WiFi 122 | stations, and attempts a connection to the first ssid that is listed in the 123 | supplicant file that is in this scan. If this connection fails, it does not 124 | attempt any other connections, so an invalid password for a valid ssid will 125 | prevent connection to networks that are listed lower in the file. 126 | 127 | Note that the ESP32 can't connect to 5 GHz WiFi networks. 128 | 129 | ## Portability 130 | 131 | The code is currently specific to the ESP32. Porting it to another board that 132 | supports the Arduino APIs requires at least these changes: 133 | 134 | * The persistent configuration code uses SPIFFS. If this is not available, use 135 | the Flash API. 136 | * The BLE MAC address service publishes the WiFi MAC address. If there is no 137 | WiFi MAC address, use the BLE address. 138 | * The BLE Device Name service uses an ESP IDF call set the BLE device name. Wrap 139 | this in an #ifdef. Maybe there is another way to do this on other boards? If 140 | not, some options are: reboot the board, or live with the fact that the device 141 | name change doesn't take effect until the user reboots the board. 142 | 143 | ## License 144 | 145 | MIT 146 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /large_app_partition.csv: -------------------------------------------------------------------------------- 1 | # Name, Type, SubType, Offset, Size, Flags 2 | nvs, data, nvs, 0x9000, 0x5000, 3 | otadata, data, ota, 0xe000, 0x2000, 4 | app0, app, ota_0, 0x10000, 0x300000, 5 | spiffs, data, spiffs, 0x310000,0xF0000, 6 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | [platformio] 2 | default_envs = firebeetle32 3 | 4 | [env] 5 | framework = arduino 6 | monitor_speed = 115200 7 | lib_deps = 8 | Adafruit BNO055@~1.1.11 9 | Adafruit Unified Sensor@~1.0.2 10 | PubSubClient@2.8 11 | 12 | [env:esp32dev] 13 | platform = espressif32 14 | board = esp32dev 15 | board_build.partitions = large_app_partition.csv 16 | 17 | [env:firebeetle32] 18 | platform = espressif32 19 | board = firebeetle32 20 | board_build.partitions = large_app_partition.csv 21 | build_flags = -DFIREBEETLE 22 | -------------------------------------------------------------------------------- /src/BLEIMUService.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _BLEIMUSERVICE_H 3 | #define _BLEIMUSERVICE_H 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "BLEServiceHandler.h" 11 | #include "BNO055Dummy.h" 12 | 13 | const char BLE_IMU_SERVICE_UUID[] = "509B0001-EBE1-4AA5-BC51-11004B78D5CB"; 14 | const char BLE_IMU_SENSOR_CHAR_UUID[] = "509B0002-EBE1-4AA5-BC51-11004B78D5CB"; 15 | const char BLE_IMU_CALIBRATION_CHAR_UUID[] = 16 | "509B0003-EBE1-4AA5-BC51-11004B78D5CB"; 17 | 18 | const uint8_t BLE_IMU_MESSAGE_VERSION = 1; 19 | 20 | enum BLE_IMUFieldBits { 21 | BLE_IMU_ACCEL_FLAG = 0x01, 22 | BLE_IMU_MAG_FLAG = 0x02, 23 | BLE_IMU_GYRO_FLAG = 0x04, 24 | BLE_IMU_CALIBRATION_FLAG = 0x08, 25 | BLE_IMU_EULER_FLAG = 0x10, 26 | BLE_IMU_QUATERNION_FLAG = 0x20, 27 | BLE_IMU_LINEAR_ACCEL_FLAG = 0x40, 28 | BLE_IMU_GRAVITY_FLAG = 0x80 29 | }; 30 | 31 | class BLE_IMUMessage { 32 | public: 33 | BLE_IMUMessage(unsigned long timestamp) : timestamp_(timestamp){}; 34 | 35 | void setAccelerometer(const imu::Vector<3> &vec) { 36 | accel_ = vec; 37 | flags_ |= BLE_IMU_ACCEL_FLAG; 38 | } 39 | 40 | void setGyroscope(imu::Vector<3> vec) { 41 | gyro_ = vec; 42 | flags_ |= BLE_IMU_GYRO_FLAG; 43 | } 44 | 45 | void setMagnetometer(const imu::Vector<3> &vec) { 46 | mag_ = vec; 47 | flags_ |= BLE_IMU_MAG_FLAG; 48 | } 49 | 50 | void setLinearAcceleration(const imu::Vector<3> &vec) { 51 | linear_ = vec; 52 | flags_ |= BLE_IMU_LINEAR_ACCEL_FLAG; 53 | } 54 | 55 | void setQuaternion(const float quat[4]) { 56 | memcpy(quat_, quat, sizeof quat_); 57 | flags_ |= BLE_IMU_QUATERNION_FLAG; 58 | } 59 | 60 | void setQuaternion(const double quat[4]) { 61 | float q[4] = { 62 | static_cast(quat[0]), 63 | static_cast(quat[1]), 64 | static_cast(quat[2]), 65 | static_cast(quat[3]), 66 | }; 67 | setQuaternion(q); 68 | } 69 | 70 | void setQuaternion(double w, double x, double y, double z) { 71 | double q[] = {w, x, y, z}; 72 | setQuaternion(q); 73 | } 74 | 75 | std::vector getPayload() { 76 | uint8_t buf[240] = { 77 | BLE_IMU_MESSAGE_VERSION, 78 | flags_, 79 | static_cast(timestamp_), 80 | static_cast(timestamp_ >> 8), 81 | }; 82 | uint8_t *p = &buf[4]; 83 | if (flags_ & BLE_IMU_QUATERNION_FLAG) { 84 | assert(p - buf + sizeof quat_ <= sizeof buf); 85 | memcpy(p, quat_, sizeof quat_); 86 | p += sizeof quat_; 87 | } 88 | if (flags_ & BLE_IMU_ACCEL_FLAG) { 89 | p = this->appendVector_(buf, sizeof buf, p, accel_); 90 | } 91 | if (flags_ & BLE_IMU_GYRO_FLAG) { 92 | p = this->appendVector_(buf, sizeof buf, p, gyro_); 93 | } 94 | if (flags_ & BLE_IMU_MAG_FLAG) { 95 | p = this->appendVector_(buf, sizeof buf, p, mag_); 96 | } 97 | if (flags_ & BLE_IMU_LINEAR_ACCEL_FLAG) { 98 | p = this->appendVector_(buf, sizeof buf, p, linear_); 99 | } 100 | std::vector vec(buf, p); 101 | return vec; 102 | } 103 | 104 | private: 105 | uint8_t flags_ = 0; 106 | unsigned long timestamp_; 107 | float quat_[4]; 108 | imu::Vector<3> accel_, gyro_, mag_, linear_; 109 | 110 | uint8_t *appendVector_(uint8_t *buf, size_t size, uint8_t *p, 111 | const imu::Vector<3> &vec) { 112 | float v[3] = { 113 | static_cast(vec.x()), 114 | static_cast(vec.y()), 115 | static_cast(vec.z()), 116 | }; 117 | assert(p - buf + sizeof v <= size); 118 | memcpy(p, v, sizeof v); 119 | p += sizeof v; 120 | return p; 121 | }; 122 | }; 123 | 124 | // 60 fps, with headroom 125 | #define BLE_IMU_TX_FREQ 60 126 | static const int BLE_IMU_TX_DELAY = (1000 - 10) / BLE_IMU_TX_FREQ; 127 | 128 | class BLEIMUServiceHandler : public BLEServiceHandler { 129 | public: 130 | const bool INCLUDE_ALL_VALUES = true; 131 | 132 | BLEIMUServiceHandler(BLEServiceManager &manager, BNO055Base &sensor) 133 | : BLEServiceHandler(manager, BLE_IMU_SERVICE_UUID), bno_(sensor) { 134 | imuSensorValueChar_ = bleService_->createCharacteristic( 135 | BLE_IMU_SENSOR_CHAR_UUID, BLECharacteristic::PROPERTY_NOTIFY); 136 | imuSensorValueChar_->addDescriptor(new BLE2902()); 137 | 138 | imuCalibrationChar_ = bleService_->createCharacteristic( 139 | BLE_IMU_CALIBRATION_CHAR_UUID, 140 | BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_READ); 141 | imuCalibrationChar_->addDescriptor(new BLE2902()); 142 | } 143 | 144 | void start() { 145 | BLEServiceHandler::start(); 146 | setCalibrationValue(); 147 | } 148 | 149 | void tick() { 150 | unsigned long now = millis(); 151 | // Doesn't account for time wraparound 152 | if (now > nextTxTimeMs_) { 153 | std::array calibration; 154 | bno_.getCalibration(&calibration[0], &calibration[1], &calibration[2], 155 | &calibration[3]); 156 | if (calibration != calibration_) { 157 | setCalibrationValue(); 158 | imuCalibrationChar_->notify(); 159 | } 160 | 161 | BLE_IMUMessage message(now); 162 | auto quat = bno_.getQuat(); 163 | message.setQuaternion(quat.w(), quat.x(), quat.y(), quat.z()); 164 | if (INCLUDE_ALL_VALUES) { 165 | message.setAccelerometer( 166 | bno_.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER)); 167 | message.setGyroscope(bno_.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE)); 168 | message.setMagnetometer( 169 | bno_.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER)); 170 | message.setLinearAcceleration( 171 | bno_.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL)); 172 | } 173 | 174 | std::vector payload = message.getPayload(); 175 | imuSensorValueChar_->setValue(payload.data(), payload.size()); 176 | imuSensorValueChar_->notify(); 177 | 178 | nextTxTimeMs_ = now + BLE_IMU_TX_DELAY; 179 | } 180 | } 181 | 182 | private: 183 | BLECharacteristic *imuSensorValueChar_; 184 | BLECharacteristic *imuCalibrationChar_; 185 | BNO055Base &bno_; 186 | std::array calibration_ = {{0, 0, 0, 0}}; 187 | unsigned long nextTxTimeMs_ = 0; 188 | 189 | void setCalibrationValue() { 190 | std::array calibration; 191 | bno_.getCalibration(&calibration[0], &calibration[1], &calibration[2], 192 | &calibration[3]); 193 | imuCalibrationChar_->setValue(calibration.data(), calibration.size()); 194 | calibration_ = calibration; 195 | } 196 | }; 197 | 198 | #endif /* _BLEIMUSERVICE_H */ 199 | -------------------------------------------------------------------------------- /src/BLEMACAddressService.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "BLEServiceManager.h" 5 | #include "Config.h" 6 | #include "utils.h" 7 | 8 | const char BLE_MAC_ADDRESS_SERVICE_UUID[] = 9 | "709F0001-37E3-439E-A338-23F00067988B"; 10 | const char BLE_MAC_ADDRESS_CHAR_UUID[] = "709F0002-37E3-439E-A338-23F00067988B"; 11 | const char BLE_DEVICE_NAME_CHAR_UUID[] = "709F0003-37E3-439E-A338-23F00067988B"; 12 | 13 | class BLEDeviceNameCallbacks : public BLECharacteristicCallbacks { 14 | void onWrite(BLECharacteristic *ch) { 15 | std::string deviceName = ch->getValue(); 16 | Config::getInstance().setBLEDeviceName(deviceName); 17 | esp_err_t errRc = ::esp_ble_gap_set_device_name(deviceName.c_str()); 18 | if (errRc != ESP_OK) { 19 | log_e("esp_ble_gap_set_device_name: rc=%d", errRc); 20 | } 21 | ch->setValue((uint8_t *)deviceName.data(), deviceName.length()); 22 | ch->notify(); 23 | } 24 | }; 25 | 26 | class BLEMACAddressServiceHandler : public BLEServiceHandler { 27 | public: 28 | BLEMACAddressServiceHandler(BLEServiceManager &manager) 29 | : BLEServiceHandler(manager, BLE_MAC_ADDRESS_SERVICE_UUID) { 30 | auto *macaddressChar = bleService_->createCharacteristic( 31 | BLE_MAC_ADDRESS_CHAR_UUID, BLECharacteristic::PROPERTY_READ); 32 | macaddressChar->addDescriptor(new BLE2902()); 33 | 34 | std::string macAddress = getMACAddress(); 35 | Serial.printf("MAC address = %s\n", macAddress.c_str()); 36 | macaddressChar->setValue((uint8_t *)macAddress.data(), macAddress.length()); 37 | 38 | auto *deviceNameChar = bleService_->createCharacteristic( 39 | BLE_DEVICE_NAME_CHAR_UUID, BLECharacteristic::PROPERTY_READ | 40 | BLECharacteristic::PROPERTY_WRITE | 41 | BLECharacteristic::PROPERTY_NOTIFY); 42 | std::string deviceName = Config::getInstance().getBLEDeviceName(""); 43 | Serial.printf("Device name = %s\n", deviceName.c_str()); 44 | deviceNameChar->setValue((uint8_t *)deviceName.data(), deviceName.length()); 45 | deviceNameChar->setCallbacks(new BLEDeviceNameCallbacks()); 46 | } 47 | }; 48 | -------------------------------------------------------------------------------- /src/BLEServiceHandler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef BLESERVICEHANDLER_H 3 | #define BLESERVICEHANDLER_H 4 | #include 5 | #include 6 | 7 | class BLEServiceManager; 8 | 9 | class BLEServiceHandler { 10 | public: 11 | std::string uuid; 12 | BLEServiceHandler(BLEServiceManager &manager, const char uuid[]); 13 | 14 | virtual void start() { bleService_->start(); } 15 | 16 | virtual void tick() {} 17 | 18 | protected: 19 | BLEService *bleService_; 20 | }; 21 | #endif /* BLESERVICEHANDLER_H */ 22 | -------------------------------------------------------------------------------- /src/BLEServiceManager.cpp: -------------------------------------------------------------------------------- 1 | #include "BLEServiceManager.h" 2 | 3 | #include "BLEServiceHandler.h" 4 | 5 | BLEServiceHandler::BLEServiceHandler(BLEServiceManager &manager, 6 | const char uuid[]) 7 | : uuid(uuid), bleService_(manager.bleServer.createService(uuid)) { 8 | manager.addServiceHandler(*this); 9 | } 10 | -------------------------------------------------------------------------------- /src/BLEServiceManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef BLESERVICEMANAGER_H 3 | #define BLESERVICEMANAGER_H 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "BLEServiceHandler.h" 11 | 12 | class BLEServiceManager : public BLEServerCallbacks { 13 | public: 14 | BLEServer &bleServer; 15 | 16 | BLEServiceManager() : bleServer(*BLEDevice::createServer()) { 17 | bleServer.setCallbacks(this); 18 | adv_ = bleServer.getAdvertising(); 19 | } 20 | 21 | uint32_t getConnectedCount() { return bleServer.getConnectedCount(); } 22 | 23 | void addServiceHandler(BLEServiceHandler &handler) { 24 | serviceHandlers_.push_back(&handler); 25 | Serial.printf("Advertising %s\n", handler.uuid.c_str()); 26 | adv_->addServiceUUID(handler.uuid.c_str()); 27 | } 28 | 29 | void start() { 30 | std::for_each(serviceHandlers_.begin(), serviceHandlers_.end(), 31 | std::mem_fun(&BLEServiceHandler::start)); 32 | adv_->start(); 33 | } 34 | 35 | void tick() { 36 | uint32_t connectionCount = bleServer.getConnectedCount(); 37 | if (connectionCount > 0) { 38 | std::for_each(serviceHandlers_.begin(), serviceHandlers_.end(), 39 | std::mem_fun(&BLEServiceHandler::tick)); 40 | } 41 | if (connectionCount == 0 && hasBeenConnected_) { 42 | delay(500); 43 | Serial.println("Restart BLE advertising"); 44 | bleServer.startAdvertising(); 45 | } 46 | } 47 | 48 | private: 49 | std::vector serviceHandlers_; 50 | BLEAdvertising *adv_; 51 | bool hasBeenConnected_ = false; 52 | 53 | void onConnect(BLEServer *server) { 54 | Serial.println("BLE connected"); 55 | hasBeenConnected_ = true; 56 | }; 57 | 58 | void onDisconnect(BLEServer *server) { Serial.println("BLE disconnected"); } 59 | }; 60 | #endif /* BLESERVICEMANAGER_H */ 61 | -------------------------------------------------------------------------------- /src/BLEUARTService.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "BLEServiceHandler.h" 3 | 4 | static const char NF_UART_SERVICE_UUID[] = 5 | "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"; 6 | static const char NF_UART_RX_CHAR_UUID[] = 7 | "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"; 8 | static const char NF_UART_TX_CHAR_UUID[] = 9 | "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"; 10 | 11 | // If true, send the timestamp every UART_TX_HEARTBEAT_DELAY ticks 12 | static const bool SEND_UART_TX_HEARTBEAT = false; 13 | static const int UART_TX_HEARTBEAT_DELAY = (1000 - 10) / 10; 14 | 15 | class UARTRxCallbacks : public BLECharacteristicCallbacks { 16 | public: 17 | UARTRxCallbacks(BLECharacteristic *txChar) : txChar_(txChar) {} 18 | 19 | private: 20 | void onWrite(BLECharacteristic *ch) { 21 | std::string value = ch->getValue(); 22 | if (value.length() > 0) { 23 | Serial.print("Rx: "); 24 | Serial.write(value.c_str()); 25 | if (value[value.length() - 1] != '\n') { 26 | Serial.println(); 27 | } 28 | if (value == "ping" || value == "ping\n") { 29 | Serial.write("Tx: pong\n"); 30 | static uint8_t data[] = "pong\n"; 31 | txChar_->setValue(data, sizeof data - 1); 32 | txChar_->notify(); 33 | } 34 | } 35 | } 36 | BLECharacteristic *txChar_; 37 | }; 38 | 39 | class BLEUARTServiceHandler : public BLEServiceHandler { 40 | public: 41 | BLEUARTServiceHandler(BLEServiceManager &manager) 42 | : BLEServiceHandler(manager, NF_UART_SERVICE_UUID) { 43 | txChar_ = bleService_->createCharacteristic( 44 | NF_UART_TX_CHAR_UUID, BLECharacteristic::PROPERTY_NOTIFY); 45 | txChar_->addDescriptor(new BLE2902()); 46 | 47 | rxChar_ = bleService_->createCharacteristic( 48 | NF_UART_RX_CHAR_UUID, BLECharacteristic::PROPERTY_WRITE); 49 | rxChar_->setCallbacks(new UARTRxCallbacks(txChar_)); 50 | } 51 | 52 | void tick() { 53 | unsigned long now = millis(); 54 | if (SEND_UART_TX_HEARTBEAT && now > nextTxTimeMs_) { 55 | static char buffer[10]; 56 | int len = snprintf(buffer, sizeof buffer, "%ld\n", now); 57 | if (0 <= len && len <= sizeof buffer) { 58 | txChar_->setValue((uint8_t *)buffer, len); 59 | txChar_->notify(); 60 | } else { 61 | Serial.println("Tx buffer overflow"); 62 | } 63 | nextTxTimeMs_ = now + UART_TX_HEARTBEAT_DELAY; 64 | } 65 | } 66 | 67 | private: 68 | BLECharacteristic *txChar_; 69 | BLECharacteristic *rxChar_; 70 | unsigned long nextTxTimeMs_ = 0; 71 | }; 72 | -------------------------------------------------------------------------------- /src/BNO055Dummy.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include "utils.h" 8 | 9 | class BNO055Base { 10 | public: 11 | virtual bool begin() = 0; 12 | // virtual void setExtCrystalUse(bool); 13 | virtual imu::Vector<3> getVector( 14 | Adafruit_BNO055::adafruit_vector_type_t vector_type) = 0; 15 | virtual void getCalibration(uint8_t* system, uint8_t* gyro, uint8_t* accel, 16 | uint8_t* mag) = 0; 17 | virtual imu::Quaternion getQuat() = 0; 18 | }; 19 | 20 | template 21 | class BNO055Adaptor : public BNO055Base { 22 | public: 23 | BNO055Adaptor(T& bno) : _base(bno) {} 24 | bool begin() { return _base.begin(); } 25 | // virtual setExtCrystalUse(bool flag) : {} 26 | virtual imu::Vector<3> getVector( 27 | Adafruit_BNO055::adafruit_vector_type_t vector_type) { 28 | return _base.getVector(vector_type); 29 | } 30 | void getCalibration(uint8_t* system, uint8_t* gyro, uint8_t* accel, 31 | uint8_t* mag) { 32 | _base.getCalibration(system, gyro, accel, mag); 33 | }; 34 | 35 | imu::Quaternion getQuat() { return _base.getQuat(); } 36 | 37 | private: 38 | T _base; 39 | }; 40 | 41 | class BNO055Dummy : public BNO055Base { 42 | public: 43 | BNO055Dummy() : createdAt_(millis()) {} 44 | bool begin() { return true; } 45 | void setExtCrystalUse(bool) {} 46 | 47 | void getCalibration(uint8_t* system, uint8_t* gyro, uint8_t* accel, 48 | uint8_t* mag) { 49 | uint8_t c = std::min(3, static_cast((millis() - createdAt_) / 1000)); 50 | *system = c; 51 | *gyro = c; 52 | *accel = c; 53 | *mag = c; 54 | } 55 | 56 | imu::Quaternion getQuat() { 57 | unsigned long now = millis(); 58 | static const float pi = std::acos(-1); 59 | const float s = now / 1000.0; 60 | const float euler[] = {static_cast(pi / 10 * cos(1.2 * s)), 61 | static_cast(pi / 10 * cos(1.4 * s)), 62 | static_cast(fmod(s, 2 * pi))}; 63 | float quat[4]; 64 | euler2quat(euler, quat); 65 | return imu::Quaternion( 66 | static_cast(quat[0]), static_cast(quat[1]), 67 | static_cast(quat[2]), static_cast(quat[3])); 68 | } 69 | 70 | imu::Vector<3> getVector(Adafruit_BNO055::adafruit_vector_type_t) { 71 | return imu::Vector<3>(0, 0, 0); 72 | } 73 | 74 | private: 75 | unsigned long createdAt_; 76 | }; 77 | -------------------------------------------------------------------------------- /src/Config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | class Config { 5 | public: 6 | static Config& getInstance() { 7 | static Config instance; 8 | return instance; 9 | } 10 | Config(Config const&) = delete; 11 | void operator=(Config const&) = delete; 12 | 13 | std::string getBLEDeviceName(const std::string defaultValue) { 14 | if (!SPIFFS.begin(true)) { 15 | Serial.println("Unable to mount SPIFFS"); 16 | return defaultValue; 17 | } 18 | File file = SPIFFS.open(BLE_DEVICE_NAME_FILE); 19 | if (!file) return defaultValue; 20 | 21 | std::string value; 22 | while (file.available()) { 23 | value.append(1, file.read()); 24 | } 25 | file.close(); 26 | return value.length() > 0 ? value : defaultValue; 27 | } 28 | 29 | void setBLEDeviceName(const std::string value) { 30 | File file = SPIFFS.open(BLE_DEVICE_NAME_FILE, FILE_WRITE); 31 | if (file) { 32 | file.print(value.c_str()); 33 | file.close(); 34 | Serial.print("Set BLE device name: "); 35 | Serial.println(value.c_str()); 36 | } 37 | } 38 | 39 | private: 40 | Config() {} 41 | 42 | static constexpr const char* BLE_DEVICE_NAME_FILE = "/ble-name.txt"; 43 | }; 44 | -------------------------------------------------------------------------------- /src/MQTTClient.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "MQTTClient.h" 4 | 5 | static const char MQTT_CONFIG_FNAME[] = "/mqtt.config"; 6 | 7 | class MQTTConfig { 8 | public: 9 | std::string host; 10 | int port = 18623; 11 | std::string user; 12 | std::string password; 13 | 14 | bool read() { 15 | if (!SPIFFS.begin(true)) { 16 | Serial.println("Unable to mount SPIFFS"); 17 | return false; 18 | } 19 | if (!SPIFFS.exists(MQTT_CONFIG_FNAME)) { 20 | Serial.println("MQTT config file: does not exist"); 21 | return false; 22 | } 23 | File file = SPIFFS.open(MQTT_CONFIG_FNAME); 24 | if (!file) { 25 | Serial.println("MQTT config file: Unable to open"); 26 | return false; 27 | } 28 | 29 | static std::vector fields = readFields(file); 30 | 31 | if (fields.size() < 4) { 32 | Serial.printf("MQTT config file: %d fields found; 4 expected\n", 33 | fields.size()); 34 | return false; 35 | } 36 | 37 | host = fields[0]; 38 | port = atoi(fields[1].c_str()); 39 | user = fields[2]; 40 | password = fields[3]; 41 | return true; 42 | } 43 | 44 | private: 45 | std::vector readFields(File file) { 46 | static std::vector fields; 47 | std::string field; 48 | while (file.available()) { 49 | int c = file.read(); 50 | if (c == '\n') { 51 | fields.push_back(field); 52 | field.clear(); 53 | } else { 54 | field.append(1, c); 55 | } 56 | } 57 | if (!field.empty()) { 58 | fields.push_back(field); 59 | } 60 | return fields; 61 | } 62 | }; 63 | 64 | bool MQTTClient::connect() { 65 | MQTTConfig config; 66 | if (!config.read()) return false; 67 | 68 | pubSubClient_.setServer(config.host.c_str(), config.port); 69 | Serial.printf("Connecting to mqtt://%s@%s:%d...", config.user.c_str(), 70 | config.host.c_str(), config.port); 71 | if (!pubSubClient_.connect("ESP32Client", config.user.c_str(), 72 | config.password.c_str())) { 73 | Serial.printf("failed with state %d\n", pubSubClient_.state()); 74 | return false; 75 | } 76 | Serial.println("connected"); 77 | 78 | std::string device_id(WiFi.macAddress().c_str()); 79 | device_id.erase(std::remove(device_id.begin(), device_id.end(), ':'), 80 | device_id.end()); 81 | std::transform(device_id.begin(), device_id.end(), device_id.begin(), 82 | ::tolower); 83 | Serial.printf("device_id = %s\n", device_id.c_str()); 84 | 85 | topic_ = "imu/" + device_id; 86 | return true; 87 | } 88 | 89 | bool MQTTClient::publish(const char payload[]) { 90 | bool status = pubSubClient_.publish(topic_.c_str(), payload); 91 | if (!status) { 92 | Serial.print("mqtt publish: "); 93 | Serial.println(status); 94 | } 95 | return status; 96 | } 97 | -------------------------------------------------------------------------------- /src/MQTTClient.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _MQTTCLIENT_H_ 3 | #define _MQTTCLIENT_H_ 4 | #include 5 | #include 6 | #include 7 | 8 | class MQTTClient { 9 | public: 10 | MQTTClient() : pubSubClient_(wifiClient_) {} 11 | bool connect(); 12 | bool connected() { return wifiClient_.connected(); } 13 | bool publish(const char payload[]); 14 | bool publish(std::vector& v) { 15 | return pubSubClient_.publish(topic_.c_str(), &v[0], v.size()); 16 | } 17 | 18 | private: 19 | WiFiClient wifiClient_; 20 | PubSubClient pubSubClient_; 21 | std::string topic_; 22 | }; 23 | 24 | #endif /* _MQTTCLIENT_H_ */ 25 | -------------------------------------------------------------------------------- /src/WiFiSupplicant.cpp: -------------------------------------------------------------------------------- 1 | #include "WiFiSupplicant.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | unsigned int WIFI_CONNECT_TIMEOUT_MS = 5000; 9 | 10 | static const char WPA_SUPPLICANT_FNAME[] = "/wpa_supplicant.txt"; 11 | 12 | /** (ssid, password), ordered by preference. */ 13 | typedef std::vector> WiFiConfig; 14 | 15 | /** ssid -> auth_mode */ 16 | typedef std::map WiFiScanMap; 17 | 18 | static const WiFiConfig readSupplicants() { 19 | WiFiConfig ssidPasswords; 20 | 21 | if (!SPIFFS.begin(true)) { 22 | Serial.println("Unable to mount SPIFFS"); 23 | return ssidPasswords; 24 | } 25 | if (!SPIFFS.exists(WPA_SUPPLICANT_FNAME)) { 26 | Serial.println("WPA supplicant file: does not exist"); 27 | return ssidPasswords; 28 | } 29 | File file = SPIFFS.open(WPA_SUPPLICANT_FNAME); 30 | if (!file) { 31 | Serial.println("WPA supplicant file: Unable to open"); 32 | return ssidPasswords; 33 | } 34 | 35 | std::string line, ssid; 36 | while (file.available()) { 37 | int c = file.read(); 38 | if (c == '\n') { 39 | if (ssid.empty()) { 40 | ssid = line; 41 | } else { 42 | ssidPasswords.push_back( 43 | std::pair(ssid, line)); 44 | ssid.clear(); 45 | } 46 | line.clear(); 47 | } else 48 | line.append(1, c); 49 | } 50 | return ssidPasswords; 51 | } 52 | 53 | static const WiFiScanMap scanWiFi() { 54 | WiFiScanMap ssids; 55 | int16_t n = WiFi.scanNetworks(); 56 | for (int16_t i = 0; i < n; ++i) { 57 | std::string ssid(WiFi.SSID(i).c_str()); 58 | ssids[ssid] = WiFi.encryptionType(i); // WIFI_AUTH_OPEN 59 | } 60 | return ssids; 61 | } 62 | 63 | bool WiFiSupplicant::connect() { 64 | const WiFiConfig supplicantSsids = readSupplicants(); 65 | const WiFiScanMap localSsids = scanWiFi(); 66 | auto item = std::find_if( 67 | supplicantSsids.begin(), supplicantSsids.end(), 68 | [&localSsids](const std::pair& item) { 69 | const std::string& ssid = item.first; 70 | auto entry = localSsids.find(ssid); 71 | return entry != localSsids.end(); 72 | }); 73 | if (item == supplicantSsids.end()) { 74 | Serial.print("No known WiFi network found in "); 75 | int count = 0; 76 | for (const auto& item : localSsids) { 77 | if (++count > 1) Serial.print(", "); 78 | Serial.print(item.first.c_str()); 79 | } 80 | Serial.print("\nSupplicant networks are "); 81 | for (const auto& item : supplicantSsids) { 82 | if (&item != &supplicantSsids.front()) Serial.print(", "); 83 | Serial.print(item.first.c_str()); 84 | } 85 | Serial.println(); 86 | return false; 87 | } 88 | 89 | auto ssid = item->first; 90 | auto password = item->second; 91 | 92 | Serial.printf("Connecting to WiFi network %s...", ssid.c_str()); 93 | 94 | WiFi.begin(ssid.c_str(), password.c_str()); 95 | wl_status_t wifi_status; 96 | const int WIFI_CONTINUE_MASK = (1 << WL_IDLE_STATUS) | (1 << WL_DISCONNECTED); 97 | const unsigned int timeout = millis() + WIFI_CONNECT_TIMEOUT_MS; 98 | while ((1 << (wifi_status = WiFi.status())) & WIFI_CONTINUE_MASK) { 99 | if (millis() > timeout) { 100 | break; 101 | } 102 | Serial.print("."); 103 | delay(500); 104 | } 105 | if (wifi_status != WL_CONNECTED) { 106 | Serial.printf("failed with status=%d\n", wifi_status); 107 | return false; 108 | } 109 | Serial.println("success"); 110 | Serial.printf("MAC address = %s\n", WiFi.macAddress().c_str()); 111 | Serial.print("IP address = "); 112 | Serial.println(WiFi.localIP()); 113 | return true; 114 | } 115 | -------------------------------------------------------------------------------- /src/WiFiSupplicant.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _WIFISUPPLICANTCLIENT_H_ 3 | #define _WIFISUPPLICANTCLIENT_H_ 4 | 5 | class WiFiSupplicant { 6 | public: 7 | bool connect(); 8 | }; 9 | 10 | #endif /* _WIFISUPPLICANTCLIENT_H_ */ 11 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "BLEIMUService.h" 9 | #include "BLEMACAddressService.h" 10 | #include "BLEServiceManager.h" 11 | #include "BLEUARTService.h" 12 | #include "BNO055Dummy.h" 13 | #include "Config.h" 14 | #include "MQTTClient.h" 15 | #include "WiFiSupplicant.h" 16 | 17 | static const char BLE_ADV_NAME[] = "ESP32 IMU"; 18 | 19 | static Adafruit_BNO055 bno055; 20 | static BLEServiceManager* bleServiceManager; 21 | static MQTTClient mqttClient; 22 | static WiFiSupplicant wifiSupplicant; 23 | static BNO055Base* imuSensor; 24 | 25 | #ifdef FIREBEETLE 26 | #define SPICLK 22 27 | #define SPIDAT 21 28 | #else 29 | #define SPICLK 22 30 | #define SPIDAT 23 31 | #endif 32 | 33 | static BNO055Base* getIMU() { 34 | Serial.printf("SPI = {data: %d, clock: %d}\n", SPIDAT, SPICLK); 35 | Wire.begin(SPIDAT, SPICLK); 36 | 37 | if (bno055.begin()) { 38 | Serial.println("Connected to BNO055"); 39 | return new BNO055Adaptor(bno055); 40 | } else { 41 | Serial.println("Simulating BNO055"); 42 | return new BNO055Dummy(); 43 | } 44 | } 45 | 46 | void setup() { 47 | Serial.begin(115200); 48 | 49 | if (wifiSupplicant.connect()) mqttClient.connect(); 50 | 51 | std::string bleDeviceName = 52 | Config::getInstance().getBLEDeviceName(BLE_ADV_NAME); 53 | imuSensor = getIMU(); 54 | 55 | BLEDevice::init(bleDeviceName.c_str()); 56 | bleServiceManager = new BLEServiceManager(); 57 | new BLEIMUServiceHandler(*bleServiceManager, *imuSensor); 58 | new BLEMACAddressServiceHandler(*bleServiceManager); 59 | new BLEUARTServiceHandler(*bleServiceManager); 60 | 61 | Serial.printf("Starting BLE (device name=%s)\n", bleDeviceName.c_str()); 62 | bleServiceManager->start(); 63 | } 64 | 65 | static const int MQTT_IMU_TX_FREQ = 60; 66 | static const int MQTT_TX_DELAY = (1000 - 10) / MQTT_IMU_TX_FREQ; 67 | static unsigned long nextTxTimeMs = 0; 68 | 69 | void loop() { 70 | bleServiceManager->tick(); 71 | 72 | unsigned long now = millis(); 73 | if (bleServiceManager->getConnectedCount() == 0 && mqttClient.connected() && 74 | now > nextTxTimeMs) { 75 | BLE_IMUMessage message(now); 76 | auto q = imuSensor->getQuat(); 77 | message.setQuaternion(q.w(), q.x(), q.y(), q.z()); 78 | message.setAccelerometer( 79 | imuSensor->getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER)); 80 | message.setGyroscope( 81 | imuSensor->getVector(Adafruit_BNO055::VECTOR_GYROSCOPE)); 82 | message.setMagnetometer( 83 | imuSensor->getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER)); 84 | message.setLinearAcceleration( 85 | imuSensor->getVector(Adafruit_BNO055::VECTOR_LINEARACCEL)); 86 | 87 | std::vector payload = message.getPayload(); 88 | if (!mqttClient.publish(payload)) { 89 | Serial.println("MQTT publish: failed"); 90 | } 91 | nextTxTimeMs = now + MQTT_TX_DELAY; 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | #include 3 | 4 | std::string getMACAddress() { 5 | byte macAddress[6]; 6 | WiFi.macAddress(macAddress); 7 | uint8_t macString[2 * sizeof macAddress + 1]; 8 | BLEUtils().buildHexData(macString, macAddress, sizeof macAddress); 9 | return std::string((const char*)macString); 10 | } 11 | 12 | void euler2quat(const float euler[], float q[]) { 13 | float yaw = euler[0], pitch = euler[1], roll = euler[2]; 14 | float c1 = cos(yaw / 2), s1 = sin(yaw / 2), c2 = cos(pitch / 2), 15 | s2 = sin(pitch / 2), c3 = cos(roll / 2), s3 = sin(roll / 2); 16 | float w = c1 * c2 * c3 - s1 * s2 * s3, x = s1 * s2 * c3 + c1 * c2 * s3, 17 | y = s1 * c2 * c3 + c1 * s2 * s3, z = c1 * s2 * c3 - s1 * c2 * s3; 18 | q[0] = x; 19 | q[1] = y; 20 | q[2] = z; 21 | q[3] = w; 22 | } 23 | -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | void euler2quat(const float euler[], float q[]); 6 | 7 | std::string getMACAddress(); 8 | -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | --------------------------------------------------------------------------------