469 lines
15 KiB
C++
469 lines
15 KiB
C++
#include "esp_system.h"
|
||
#include "esp_mac.h"
|
||
#include <Wire.h>
|
||
#include <ICM20948_WE.h>
|
||
#include "FS.h"
|
||
#include <LittleFS.h>
|
||
#include <ArduinoJson.h>
|
||
#include <esp_bt.h>
|
||
#include <NimBLEDevice.h>
|
||
#include <Adafruit_GFX.h>
|
||
#include <Adafruit_SSD1306.h>
|
||
|
||
#define FORMAT_LITTLEFS_IF_FAILED true
|
||
|
||
|
||
// ICM20948 parameters
|
||
#define ICM20948_ADDR 0x68
|
||
#define SCL_PIN 9
|
||
#define SDA_PIN 8
|
||
|
||
// Screen parameters
|
||
|
||
#define SCREEN_WIDTH 128
|
||
#define SCREEN_HEIGHT 64
|
||
#define OLED_RESET -1 // Reset pin (pas nécessaire avec l’ESP32 + I2C)
|
||
|
||
|
||
class Config {
|
||
public:
|
||
JsonDocument json;
|
||
|
||
Config() {}
|
||
|
||
Config(uint16_t default_sample_delay, const char *default_device_name) {
|
||
File file;
|
||
if(!LittleFS.begin(FORMAT_LITTLEFS_IF_FAILED)){
|
||
printf("LittleFS Mount Failed");
|
||
return;
|
||
}
|
||
file = LittleFS.open("/config.json", "r");
|
||
if (!file) {
|
||
printf("Config(): No configuration file found, creating a new one !\n");
|
||
json["sample_delay"] = default_sample_delay;
|
||
json["device_name"] = strdup(default_device_name);
|
||
this->save_config();
|
||
} else {
|
||
printf("Config(): Configuration file found !\n");
|
||
deserializeJson(json, file);
|
||
printf("Config(): Will use following parameters:\n");
|
||
printf("\tsample_delay = %d\n", (uint8_t) json["sample_delay"]);
|
||
printf("\tdevice_name = %s\n", (const char *)json["device_name"]);
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void save_config()
|
||
{
|
||
int written;
|
||
File file = LittleFS.open("/config.json", "w");
|
||
if (!file) {
|
||
printf("save_config(): Error while opening /config.json !\n");
|
||
} else {
|
||
written = serializeJson(json, file);
|
||
printf("save_config(): %d bytes written !\n", written);
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
const char * get_device_name() {
|
||
return json["device_name"];
|
||
}
|
||
|
||
void set_device_name(const char *name) {
|
||
json["device_name"] = name;
|
||
save_config();
|
||
}
|
||
|
||
short int get_sample_delay() {
|
||
return json["sample_delay"];
|
||
}
|
||
|
||
void set_sample_delay(uint16_t sample_delay) {
|
||
json["sample_delay"] = sample_delay;
|
||
save_config();
|
||
}
|
||
};
|
||
|
||
|
||
/* Global variables */
|
||
struct context_t {
|
||
Config conf;
|
||
ICM20948_WE imu;
|
||
Adafruit_SSD1306 *display;
|
||
|
||
NimBLEServer *srv;
|
||
NimBLEService *srv_data;
|
||
|
||
NimBLECharacteristic *chr_pitch;
|
||
NimBLEDescriptor *chr_pitch_info_descriptor;
|
||
NimBLE2904 *chr_pitch_type_descriptor;
|
||
|
||
NimBLECharacteristic *chr_roll;
|
||
NimBLEDescriptor *chr_roll_info_descriptor;
|
||
NimBLE2904 *chr_roll_type_descriptor;
|
||
|
||
NimBLECharacteristic *chr_sample_delay;
|
||
NimBLEDescriptor *chr_sample_delay_info_descriptor;
|
||
NimBLE2904 *chr_sample_delay_type_descriptor;
|
||
|
||
NimBLECharacteristic *chr_device_name;
|
||
NimBLEDescriptor *chr_device_name_info_descriptor;
|
||
NimBLE2904 *chr_device_name_type_descriptor;
|
||
|
||
NimBLEAdvertising *advertising;
|
||
|
||
float pitch, pitch_off;
|
||
float roll, roll_off;
|
||
} ctx;
|
||
|
||
|
||
|
||
void printMessage(String msg)
|
||
{
|
||
ctx.display->clearDisplay();
|
||
ctx.display->setTextSize(1);
|
||
ctx.display->setTextColor(SSD1306_WHITE);
|
||
ctx.display->setCursor(2, (SCREEN_HEIGHT / 2) - 5);
|
||
ctx.display->println(msg);
|
||
ctx.display->display();
|
||
}
|
||
|
||
void manageLeds(float roll)
|
||
{
|
||
for (int j=0; j<3; j++)
|
||
digitalWrite(j, LOW);
|
||
if (roll <= -0.5)
|
||
digitalWrite(0, HIGH);
|
||
else if(roll >= 0.5)
|
||
digitalWrite(2, HIGH);
|
||
else
|
||
digitalWrite(1, HIGH);
|
||
}
|
||
|
||
|
||
/* Initialize OLed screen */
|
||
void initDisplay() {
|
||
ctx.display = new Adafruit_SSD1306(SCREEN_WIDTH, SCREEN_HEIGHT);
|
||
if(!ctx.display->begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
|
||
printf("SSD1306 allocation failed\n");
|
||
}
|
||
printMessage("Starting up !");
|
||
delay(1000);
|
||
}
|
||
|
||
|
||
|
||
/* Draw on crosshair on screen */
|
||
void drawCrosshair(float pitch, float roll) {
|
||
// Centre de l'écran
|
||
const int centerX = SCREEN_WIDTH / 2;
|
||
const int centerY = SCREEN_HEIGHT / 2;
|
||
|
||
// Rayon du cercle de la mire
|
||
const int radius = 20;
|
||
|
||
// Échelle de déplacement du point en fonction pitch/roll
|
||
// Ajuste ce facteur selon la plage de ton IMU
|
||
const float scale = 2;
|
||
|
||
ctx.display->clearDisplay();
|
||
ctx.display->drawLine(centerX, 0, centerX, SCREEN_HEIGHT, SSD1306_WHITE); // axe Y
|
||
ctx.display->drawLine(0, centerY, SCREEN_WIDTH, centerY, SSD1306_WHITE); // axe X
|
||
ctx.display->drawCircle(centerX, centerY, radius, SSD1306_WHITE);
|
||
|
||
int pointX = centerX + roll * scale;
|
||
int pointY = centerY - pitch * scale; // Y inversé car origine en haut à gauche
|
||
|
||
if (pointX < 0) pointX = 0;
|
||
if (pointX >= SCREEN_WIDTH) pointX = SCREEN_WIDTH - 1;
|
||
if (pointY < 0) pointY = 0;
|
||
if (pointY >= SCREEN_HEIGHT) pointY = SCREEN_HEIGHT - 1;
|
||
|
||
|
||
ctx.display->fillCircle(pointX, pointY, 2, SSD1306_WHITE);
|
||
|
||
ctx.display->setTextSize(1);
|
||
ctx.display->setTextColor(SSD1306_WHITE);
|
||
ctx.display->setCursor(0, 0);
|
||
ctx.display->print("R: ");
|
||
ctx.display->println(roll,2);
|
||
ctx.display->print("P: ");
|
||
ctx.display->println(pitch,2);
|
||
|
||
ctx.display->display();
|
||
}
|
||
|
||
|
||
|
||
void scanI2C() {
|
||
byte error, address;
|
||
int nDevices = 0;
|
||
|
||
printf("Scanning I2C bus...\n");
|
||
for(address = 1; address < 127; address++ ) {
|
||
Wire.beginTransmission(address);
|
||
error = Wire.endTransmission();
|
||
|
||
if (error == 0) {
|
||
printf("I2C device found at 0x%2X\n", address);
|
||
nDevices++;
|
||
}
|
||
}
|
||
if (nDevices == 0) printf("No device found !\n");
|
||
else printf("Scan ended.\n");
|
||
}
|
||
|
||
|
||
class ServerCallbacks : public NimBLEServerCallbacks {
|
||
void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override {
|
||
printf("Client address: %s\n", connInfo.getAddress().toString().c_str());
|
||
pServer->updateConnParams(connInfo.getConnHandle(), 24, 48, 0, 180);
|
||
}
|
||
|
||
void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override {
|
||
printf("Client disconnected - start advertising\n");
|
||
NimBLEDevice::startAdvertising();
|
||
}
|
||
|
||
void onMTUChange(uint16_t MTU, NimBLEConnInfo& connInfo) override {
|
||
printf("MTU updated: %u for connection ID: %u\n", MTU, connInfo.getConnHandle());
|
||
}
|
||
|
||
/********************* Security handled here *********************/
|
||
uint32_t onPassKeyDisplay() override {
|
||
printf("Server Passkey Display\n");
|
||
return 123456;
|
||
}
|
||
|
||
void onConfirmPassKey(NimBLEConnInfo& connInfo, uint32_t pass_key) override {
|
||
printf("The passkey YES/NO number: %" PRIu32 "\n", pass_key);
|
||
NimBLEDevice::injectConfirmPasskey(connInfo, true);
|
||
}
|
||
|
||
void onAuthenticationComplete(NimBLEConnInfo& connInfo) override {
|
||
/** Check that encryption was successful, if not we disconnect the client */
|
||
if (!connInfo.isEncrypted()) {
|
||
NimBLEDevice::getServer()->disconnect(connInfo.getConnHandle());
|
||
printf("Encrypt connection failed - disconnecting client\n");
|
||
return;
|
||
}
|
||
|
||
printf("Secured connection to: %s\n", connInfo.getAddress().toString().c_str());
|
||
}
|
||
} serverCallbacks;
|
||
|
||
/** Handler class for characteristic actions */
|
||
class CharacteristicCallbacks : public NimBLECharacteristicCallbacks {
|
||
void onRead(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override {
|
||
printf("%s : onRead(), value: %s\n",
|
||
pCharacteristic->getUUID().toString().c_str(),
|
||
pCharacteristic->getValue().c_str());
|
||
}
|
||
|
||
void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override {
|
||
if (pCharacteristic == ctx.chr_device_name) {
|
||
ctx.conf.set_device_name(pCharacteristic->getValue().c_str());
|
||
printf("Device name set to %s, resetting device !\n", pCharacteristic->getValue().c_str());
|
||
abort();
|
||
}
|
||
if (pCharacteristic == ctx.chr_sample_delay) {
|
||
ctx.conf.set_sample_delay(pCharacteristic->getValue<uint16_t>());
|
||
printf("Sample delay set to %i\n", (uint16_t) pCharacteristic->getValue<uint16_t>());
|
||
}
|
||
}
|
||
|
||
/**
|
||
* The value returned in code is the NimBLE host return code.
|
||
*/
|
||
void onStatus(NimBLECharacteristic* pCharacteristic, int code) override {
|
||
printf("Notification/Indication return code: %d, %s\n", code, NimBLEUtils::returnCodeToString(code));
|
||
}
|
||
|
||
/** Peer subscribed to notifications/indications */
|
||
void onSubscribe(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, uint16_t subValue) override {
|
||
std::string str = "Client ID: ";
|
||
str += connInfo.getConnHandle();
|
||
str += " Address: ";
|
||
str += connInfo.getAddress().toString();
|
||
if (subValue == 0) {
|
||
str += " Unsubscribed to ";
|
||
} else if (subValue == 1) {
|
||
str += " Subscribed to notifications for ";
|
||
} else if (subValue == 2) {
|
||
str += " Subscribed to indications for ";
|
||
} else if (subValue == 3) {
|
||
str += " Subscribed to notifications and indications for ";
|
||
}
|
||
str += std::string(pCharacteristic->getUUID());
|
||
|
||
printf("%s\n", str.c_str());
|
||
}
|
||
} chrCallbacks;
|
||
|
||
/** Handler class for descriptor actions */
|
||
class DescriptorCallbacks : public NimBLEDescriptorCallbacks {
|
||
void onWrite(NimBLEDescriptor* pDescriptor, NimBLEConnInfo& connInfo) override {
|
||
std::string dscVal = pDescriptor->getValue();
|
||
printf("Descriptor written value: %s\n", dscVal.c_str());
|
||
}
|
||
|
||
void onRead(NimBLEDescriptor* pDescriptor, NimBLEConnInfo& connInfo) override {
|
||
printf("%s Descriptor read\n", pDescriptor->getUUID().toString().c_str());
|
||
}
|
||
} dscCallbacks;
|
||
|
||
|
||
|
||
|
||
/* There are several ways to create your ICM20948 object:
|
||
* ICM20948_WE myIMU = ICM20948_WE() -> uses Wire / I2C Address = 0x68
|
||
* ICM20948_WE myIMU = ICM20948_WE(ICM20948_ADDR) -> uses Wire / ICM20948_ADDR
|
||
* ICM20948_WE myIMU = ICM20948_WE(&wire2) -> uses the TwoWire object wire2 / ICM20948_ADDR
|
||
* ICM20948_WE myIMU = ICM20948_WE(&wire2, ICM20948_ADDR) -> all together
|
||
* ICM20948_WE myIMU = ICM20948_WE(CS_PIN, spi); -> uses SPI, spi is just a flag, see SPI example
|
||
* ICM20948_WE myIMU = ICM20948_WE(&SPI, CS_PIN, spi); -> uses SPI / passes the SPI object, spi is just a flag, see SPI example
|
||
*/
|
||
|
||
void setup() {
|
||
delay(2000); // maybe needed for some MCUs, in particular for startup after power off
|
||
|
||
ctx.conf = Config(200, (const char *) "TLDLevel");
|
||
|
||
/* Prepare LEDs */
|
||
pinMode(0, OUTPUT);
|
||
pinMode(1, OUTPUT);
|
||
pinMode(2, OUTPUT);
|
||
|
||
|
||
|
||
/* Initializer serial */
|
||
printf("Launching ...\n");
|
||
Serial.begin(115200);
|
||
while(!Serial) {}
|
||
|
||
/* Initialize display */
|
||
initDisplay();
|
||
|
||
Wire.begin(SDA_PIN, SCL_PIN);
|
||
ctx.imu = ICM20948_WE(ICM20948_ADDR);
|
||
delay(1000);
|
||
scanI2C();
|
||
delay(1000);
|
||
/* Initializer 9DOF sensor */
|
||
if(!ctx.imu.init()){
|
||
printf("ICM20948 does not respond\n");
|
||
}
|
||
else{
|
||
printf("ICM20948 is connected\n");
|
||
}
|
||
|
||
|
||
/* Initializer BLE stuff */
|
||
NimBLEDevice::init(ctx.conf.get_device_name());
|
||
//NimBLEDevice::init("TLDLevel");
|
||
|
||
ctx.srv = NimBLEDevice::createServer();
|
||
ctx.srv->setCallbacks(&serverCallbacks);
|
||
|
||
ctx.srv_data = ctx.srv->createService("0000aaaa-0000-1000-8000-00805f9b34fb");
|
||
|
||
/* A descriptor for pitch angle */
|
||
/* Notification is enabled */
|
||
ctx.chr_pitch = ctx.srv_data->createCharacteristic("00000001-0000-1000-8000-00805f9b34fb", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
|
||
ctx.chr_pitch->setValue(0);
|
||
ctx.chr_pitch->setCallbacks(&chrCallbacks);
|
||
ctx.chr_pitch_info_descriptor = ctx.chr_pitch->createDescriptor("2901", NIMBLE_PROPERTY::READ, 11);
|
||
ctx.chr_pitch_info_descriptor->setValue("Pitch angle");
|
||
ctx.chr_pitch_type_descriptor = ctx.chr_pitch->create2904();
|
||
ctx.chr_pitch_type_descriptor->setFormat(NimBLE2904::FORMAT_FLOAT32);
|
||
|
||
|
||
/* A descriptor for roll angle */
|
||
/* Notification is enabled */
|
||
ctx.chr_roll = ctx.srv_data->createCharacteristic("00000002-0000-1000-8000-00805f9b34fb", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
|
||
ctx.chr_roll->setValue(0);
|
||
ctx.chr_roll->setCallbacks(&chrCallbacks);
|
||
ctx.chr_roll_info_descriptor = ctx.chr_roll->createDescriptor("2901", NIMBLE_PROPERTY::READ, 10);
|
||
ctx.chr_roll_info_descriptor->setValue("Roll angle");
|
||
ctx.chr_roll_type_descriptor = ctx.chr_roll->create2904();
|
||
ctx.chr_roll_type_descriptor->setFormat(NimBLE2904::FORMAT_FLOAT32);
|
||
|
||
/* A descriptor to get/set the sample delay */
|
||
ctx.chr_sample_delay = ctx.srv_data->createCharacteristic("00000100-0000-1000-8000-00805f9b34fb", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE);
|
||
ctx.chr_sample_delay->setValue(ctx.conf.get_sample_delay());
|
||
ctx.chr_sample_delay->setCallbacks(&chrCallbacks);
|
||
ctx.chr_sample_delay_info_descriptor = ctx.chr_sample_delay->createDescriptor("2901", NIMBLE_PROPERTY::READ, 12);
|
||
ctx.chr_sample_delay_info_descriptor->setValue("Sample Delay");
|
||
ctx.chr_sample_delay_type_descriptor = ctx.chr_sample_delay->create2904();
|
||
ctx.chr_sample_delay_type_descriptor->setFormat(NimBLE2904::FORMAT_UINT16);
|
||
|
||
/* A descriptor to get/set BLE device name */
|
||
ctx.chr_device_name = ctx.srv_data->createCharacteristic("00000101-0000-1000-8000-00805f9b34fb", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE);
|
||
ctx.chr_device_name->setValue(ctx.conf.get_device_name());
|
||
ctx.chr_device_name->setCallbacks(&chrCallbacks);
|
||
ctx.chr_device_name_info_descriptor = ctx.chr_device_name->createDescriptor("2901", NIMBLE_PROPERTY::READ, 18);
|
||
ctx.chr_device_name_info_descriptor->setValue("My TLD Device Name");
|
||
ctx.chr_device_name_type_descriptor = ctx.chr_device_name->create2904();
|
||
ctx.chr_device_name_type_descriptor->setFormat(NimBLE2904::FORMAT_UTF8);
|
||
|
||
|
||
ctx.srv_data->start();
|
||
|
||
NimBLEAdvertisementData advertisementData;
|
||
ctx.advertising = NimBLEDevice::getAdvertising();
|
||
ctx.advertising->addServiceUUID(ctx.srv_data->getUUID());
|
||
advertisementData.setCompleteServices(NimBLEUUID(ctx.srv_data->getUUID()));
|
||
advertisementData.setName(ctx.conf.get_device_name());
|
||
ctx.advertising->setAdvertisementData(advertisementData);
|
||
|
||
ctx.advertising->start();
|
||
|
||
/* Initializer sensor */
|
||
printMessage("Calibrating !\n");
|
||
printf("Position your ICM20948 flat and don't move it - calibrating...\n");
|
||
delay(1000);
|
||
ctx.imu.autoOffsets();
|
||
printf("Done!\n");
|
||
ctx.imu.setAccRange(ICM20948_ACC_RANGE_2G);
|
||
ctx.imu.setAccDLPF(ICM20948_DLPF_6);
|
||
|
||
printMessage("Ready to operate !");
|
||
for (int i=0; i<3; i++) {
|
||
for (int j=0; j<3; j++)
|
||
digitalWrite(j, HIGH);
|
||
delay(200);
|
||
for (int j=0; j<3; j++)
|
||
digitalWrite(j, LOW);
|
||
delay(200);
|
||
}
|
||
|
||
/* Get our address */
|
||
uint8_t mac[6];
|
||
esp_read_mac(mac, ESP_MAC_BT);
|
||
printf("MAC BLE: %02X:%02X:%02X:%02X:%02X:%02X\n",
|
||
mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
|
||
delay(1000);
|
||
}
|
||
|
||
void loop() {
|
||
xyzFloat gValue;
|
||
xyzFloat angle;
|
||
ctx.imu.readSensor();
|
||
ctx.imu.getGValues(&gValue);
|
||
ctx.imu.getAngles(&angle);
|
||
ctx.pitch = ctx.imu.getPitch();
|
||
ctx.roll = ctx.imu.getRoll();
|
||
ctx.chr_pitch->setValue(ctx.pitch);
|
||
ctx.chr_roll->setValue(ctx.roll);
|
||
manageLeds(ctx.roll);
|
||
drawCrosshair(ctx.pitch, ctx.roll);
|
||
//printf("Roll: %f Pitch: %f\n", ctx.roll, ctx.pitch);
|
||
if (ctx.srv->getConnectedCount()) {
|
||
ctx.chr_roll->notify();
|
||
ctx.chr_pitch->notify();
|
||
}
|
||
delay(ctx.conf.get_sample_delay());
|
||
//delay(400);
|
||
}
|