enrich code

projet_TLD_leveler.ino

@@ -2,45 +2,124 @@
 #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 <esp_bt_device.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 *pitch_label_descriptor;
+  NimBLEDescriptor *chr_pitch_info_descriptor;
-  NimBLE2904 *pitch_type_descriptor;
+  NimBLE2904 *chr_pitch_type_descriptor;
+  
   NimBLECharacteristic *chr_roll;
-  NimBLEDescriptor *roll_label_descriptor;
+  NimBLEDescriptor *chr_roll_info_descriptor;
-  NimBLE2904 *roll_type_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;
+  float pitch, pitch_off;
-  float roll;
+  float roll, roll_off;
 } ctx;
 
 
-void printMessage(char *msg)
+
+void printMessage(String msg)
 {
   ctx.display->clearDisplay();
   ctx.display->setTextSize(1);      
@@ -50,6 +129,18 @@ void printMessage(char *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() {
@@ -57,7 +148,7 @@ void initDisplay() {
   if(!ctx.display->begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
     printf("SSD1306 allocation failed\n");
   }
-  printMessage("Starting up !");
+   printMessage("Starting up !");
   delay(1000);
 }
 
@@ -96,9 +187,9 @@ void drawCrosshair(float pitch, float roll) {
   ctx.display->setTextColor(SSD1306_WHITE);
   ctx.display->setCursor(0, 0);
   ctx.display->print("R: ");
-  ctx.display->println(roll,3);
+  ctx.display->println(roll,2);
   ctx.display->print("P: ");
-  ctx.display->println(pitch,3);
+  ctx.display->println(pitch,2);
   
   ctx.display->display();
 }
@@ -171,9 +262,15 @@ class CharacteristicCallbacks : public NimBLECharacteristicCallbacks {
     }
 
     void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override {
-        printf("%s : onWrite(), value: %s\n",
+        if (pCharacteristic == ctx.chr_device_name) {
-                      pCharacteristic->getUUID().toString().c_str(),
+          ctx.conf.set_device_name(pCharacteristic->getValue().c_str());
-                      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>());
+        }
     }
 
     /**
@@ -230,6 +327,15 @@ class DescriptorCallbacks : public NimBLEDescriptorCallbacks {
 
 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");
@@ -254,34 +360,53 @@ void setup() {
 
 
   /* Initializer BLE stuff */
-  NimBLEDevice::init("TLDLevel");
+  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.pitch_label_descriptor = ctx.chr_pitch->createDescriptor("2901", NIMBLE_PROPERTY::READ, 11);
+  ctx.chr_pitch_info_descriptor = ctx.chr_pitch->createDescriptor("2901", NIMBLE_PROPERTY::READ, 11);
-  ctx.pitch_label_descriptor->setValue("Pitch angle");
+  ctx.chr_pitch_info_descriptor->setValue("Pitch angle");
-  ctx.pitch_label_descriptor->setCallbacks(&dscCallbacks);
+  ctx.chr_pitch_type_descriptor = ctx.chr_pitch->create2904();
-  ctx.pitch_type_descriptor = ctx.chr_pitch->create2904();
+  ctx.chr_pitch_type_descriptor->setFormat(NimBLE2904::FORMAT_FLOAT32);
-  ctx.pitch_type_descriptor->setFormat(NimBLE2904::FORMAT_FLOAT32);
-  ctx.pitch_type_descriptor->setCallbacks(&dscCallbacks);
-  
   
+
+  /* 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.roll_label_descriptor = ctx.chr_roll->createDescriptor("2901", NIMBLE_PROPERTY::READ, 10);
+  ctx.chr_roll_info_descriptor = ctx.chr_roll->createDescriptor("2901", NIMBLE_PROPERTY::READ, 10);
-  ctx.roll_label_descriptor->setValue("Roll angle");
+  ctx.chr_roll_info_descriptor->setValue("Roll angle");
-  ctx.roll_label_descriptor->setCallbacks(&dscCallbacks);
+  ctx.chr_roll_type_descriptor = ctx.chr_roll->create2904();
-  ctx.roll_type_descriptor = ctx.chr_roll->create2904();
+  ctx.chr_roll_type_descriptor->setFormat(NimBLE2904::FORMAT_FLOAT32);
-  ctx.roll_type_descriptor->setFormat(NimBLE2904::FORMAT_FLOAT32);
+
-  ctx.roll_type_descriptor->setCallbacks(&dscCallbacks);
+  /* 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();
     
@@ -289,7 +414,7 @@ void setup() {
   ctx.advertising = NimBLEDevice::getAdvertising();
   ctx.advertising->addServiceUUID(ctx.srv_data->getUUID());
   advertisementData.setCompleteServices(NimBLEUUID(ctx.srv_data->getUUID())); 
-  advertisementData.setName("TLDLevel");
+  advertisementData.setName(ctx.conf.get_device_name());
   ctx.advertising->setAdvertisementData(advertisementData);
 
   ctx.advertising->start(); 
@@ -304,7 +429,15 @@ void setup() {
   ctx.imu.setAccDLPF(ICM20948_DLPF_6);
 
   printMessage("Ready to operate !");
-  delay(1000);
+  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);
@@ -323,11 +456,13 @@ void loop() {
   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(100);
+  delay(ctx.conf.get_sample_delay());
+  //delay(400);
 }