Added application files

.vscode/settings.json

@@ -31,5 +31,7 @@
     "--background-index",
     "--query-driver=**",
     "--compile-commands-dir=c:\\EspProjects\\bat-sense\\build"
-  ]
+  ],
+  "idf.flashType": "UART",
+  "idf.portWin": "COM6"
 }

CMakeLists.txt

@@ -2,6 +2,11 @@
 # CMakeLists in this exact order for cmake to work correctly
 cmake_minimum_required(VERSION 3.22)
 
+add_compile_definitions(
+    ARDUINO_USB_MODE=1
+    ARDUINO_USB_CDC_ON_BOOT=1
+)
+
 include($ENV{IDF_PATH}/tools/cmake/project.cmake)
 # "Trim" the build. Include the minimal set of components, main, and anything it depends on.
 idf_build_set_property(MINIMAL_BUILD ON)

components/arduino

@@ -0,0 +1 @@
+Subproject commit 96b3d0ef8de2b6a90d36fd84f06d4acbfea091dd

main/CMakeLists.txt

@@ -42,3 +42,8 @@ idf_component_register(
     REQUIRES
         arduino
 )
+
+add_compile_definitions(
+    NBUS_NODE_TYPE_SLAVE
+    ARDUINO_CORE_3X
+)

main/lib/Adafruit_BME280_Library

@@ -0,0 +1 @@
+Subproject commit ef712e95147de926b4545e379f6296ad271529b5

main/lib/Adafruit_BusIO

@@ -0,0 +1 @@
+Subproject commit 3b8364267c3ee6e16bad91bc2101aefbd5b5915f

main/lib/Adafruit_Sensor

@@ -0,0 +1 @@
+Subproject commit 0a9127a1e886ff1adb4c1b6f5958b24108d55aa6

main/lib/SparkFun_ICM-20948_ArduinoLibrary

@@ -0,0 +1 @@
+Subproject commit 32cf0614bccf0d8cff32fbc4260097ac10e5b714

main/lib/arduino-lib-hc-sr04

@@ -0,0 +1 @@
+Subproject commit 46a8be5ab2211152601d16748d95df1702bcee72

main/lib/nbus

@@ -0,0 +1 @@
+Subproject commit 6c083eb7dfdd3c16ab29004bbdb07b496fb2d3d9

main/lib/nbus-app-driver-for-esp32

@@ -0,0 +1 @@
+Subproject commit 4509e889e1d1159b9d65f193185ff7ab03ef31da

main/lib/wireless-serial-for-esp32

@@ -0,0 +1 @@
+Subproject commit 8829dc0554a1a7b7667c5b3f1868ee02404c5a37

main/src/acc.txt

@@ -0,0 +1,233 @@
+
+#include "Arduino.h"
+
+#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
+
+//#define USE_SPI       // Uncomment this to use SPI
+
+#define SERIAL_PORT Serial
+
+#define SPI_PORT SPI // Your desired SPI port.       Used only when "USE_SPI" is defined
+
+
+#define CS_PIN 8     // Which pin you connect CS to. Used only when "USE_SPI" is defined
+#define MISO_PIN 7
+#define SCLK_PIN 6
+#define MOSI_PIN 5
+
+#define USE_SPI 1
+// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
+#define AD0_VAL 1
+
+#ifdef USE_SPI
+ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
+#else
+ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
+#endif
+
+
+void printScaledAGMT(ICM_20948_SPI *sensor);
+void printFormattedFloat(float val, uint8_t leading, uint8_t decimals);
+
+void setup()
+{
+
+  SERIAL_PORT.begin(115200);
+  while (!SERIAL_PORT)
+  {
+  };
+
+#ifdef USE_SPI
+  SPI_PORT.begin(SCLK_PIN, MISO_PIN, MOSI_PIN);
+#else
+  WIRE_PORT.begin();
+  WIRE_PORT.setClock(400000);
+#endif
+
+  //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+
+  bool initialized = false;
+  while (!initialized)
+  {
+
+#ifdef USE_SPI
+    myICM.begin(CS_PIN, SPI_PORT);
+#else
+    myICM.begin(WIRE_PORT, AD0_VAL);
+#endif
+
+    SERIAL_PORT.print(F("Initialization of the sensor returned: "));
+    SERIAL_PORT.println(myICM.statusString());
+    if (myICM.status != ICM_20948_Stat_Ok)
+    {
+      SERIAL_PORT.println("Trying again...");
+      delay(500);
+    }
+    else
+    {
+      initialized = true;
+    }
+  }
+}
+
+void loop()
+{
+
+  if (myICM.dataReady())
+  {
+    myICM.getAGMT();         // The values are only updated when you call 'getAGMT'
+                             //    printRawAGMT( myICM.agmt );     // Uncomment this to see the raw values, taken directly from the agmt structure
+    printScaledAGMT(&myICM); // This function takes into account the scale settings from when the measurement was made to calculate the values with units
+    delay(30);
+  }
+  else
+  {
+    SERIAL_PORT.println("Waiting for data");
+    delay(500);
+  }
+}
+
+// Below here are some helper functions to print the data nicely!
+
+void printPaddedInt16b(int16_t val)
+{
+  if (val > 0)
+  {
+    SERIAL_PORT.print(" ");
+    if (val < 10000)
+    {
+      SERIAL_PORT.print("0");
+    }
+    if (val < 1000)
+    {
+      SERIAL_PORT.print("0");
+    }
+    if (val < 100)
+    {
+      SERIAL_PORT.print("0");
+    }
+    if (val < 10)
+    {
+      SERIAL_PORT.print("0");
+    }
+  }
+  else
+  {
+    SERIAL_PORT.print("-");
+    if (abs(val) < 10000)
+    {
+      SERIAL_PORT.print("0");
+    }
+    if (abs(val) < 1000)
+    {
+      SERIAL_PORT.print("0");
+    }
+    if (abs(val) < 100)
+    {
+      SERIAL_PORT.print("0");
+    }
+    if (abs(val) < 10)
+    {
+      SERIAL_PORT.print("0");
+    }
+  }
+  SERIAL_PORT.print(abs(val));
+}
+
+void printRawAGMT(ICM_20948_AGMT_t agmt)
+{
+  SERIAL_PORT.print("RAW. Acc [ ");
+  printPaddedInt16b(agmt.acc.axes.x);
+  SERIAL_PORT.print(", ");
+  printPaddedInt16b(agmt.acc.axes.y);
+  SERIAL_PORT.print(", ");
+  printPaddedInt16b(agmt.acc.axes.z);
+  SERIAL_PORT.print(" ], Gyr [ ");
+  printPaddedInt16b(agmt.gyr.axes.x);
+  SERIAL_PORT.print(", ");
+  printPaddedInt16b(agmt.gyr.axes.y);
+  SERIAL_PORT.print(", ");
+  printPaddedInt16b(agmt.gyr.axes.z);
+  SERIAL_PORT.print(" ], Mag [ ");
+  printPaddedInt16b(agmt.mag.axes.x);
+  SERIAL_PORT.print(", ");
+  printPaddedInt16b(agmt.mag.axes.y);
+  SERIAL_PORT.print(", ");
+  printPaddedInt16b(agmt.mag.axes.z);
+  SERIAL_PORT.print(" ], Tmp [ ");
+  printPaddedInt16b(agmt.tmp.val);
+  SERIAL_PORT.print(" ]");
+  SERIAL_PORT.println();
+}
+
+void printFormattedFloat(float val, uint8_t leading, uint8_t decimals)
+{
+  float aval = abs(val);
+  if (val < 0)
+  {
+    SERIAL_PORT.print("-");
+  }
+  else
+  {
+    SERIAL_PORT.print(" ");
+  }
+  for (uint8_t indi = 0; indi < leading; indi++)
+  {
+    uint32_t tenpow = 0;
+    if (indi < (leading - 1))
+    {
+      tenpow = 1;
+    }
+    for (uint8_t c = 0; c < (leading - 1 - indi); c++)
+    {
+      tenpow *= 10;
+    }
+    if (aval < tenpow)
+    {
+      SERIAL_PORT.print("0");
+    }
+    else
+    {
+      break;
+    }
+  }
+  if (val < 0)
+  {
+    SERIAL_PORT.print(-val, decimals);
+  }
+  else
+  {
+    SERIAL_PORT.print(val, decimals);
+  }
+}
+
+#ifdef USE_SPI
+void printScaledAGMT(ICM_20948_SPI *sensor)
+{
+#else
+void printScaledAGMT(ICM_20948_I2C *sensor)
+{
+#endif
+  SERIAL_PORT.print("Scaled. Acc (mg) [ ");
+  printFormattedFloat(sensor->accX(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->accY(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->accZ(), 5, 2);
+  SERIAL_PORT.print(" ], Gyr (DPS) [ ");
+  printFormattedFloat(sensor->gyrX(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->gyrY(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->gyrZ(), 5, 2);
+  SERIAL_PORT.print(" ], Mag (uT) [ ");
+  printFormattedFloat(sensor->magX(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->magY(), 5, 2);
+  SERIAL_PORT.print(", ");
+  printFormattedFloat(sensor->magZ(), 5, 2);
+  SERIAL_PORT.print(" ], Tmp (C) [ ");
+  printFormattedFloat(sensor->temp(), 5, 2);
+  SERIAL_PORT.print(" ]");
+  SERIAL_PORT.println();
+}

main/src/bme.txt

@@ -0,0 +1,62 @@
+#include <Arduino.h>
+
+// hw includes
+#include <Wire.h>
+
+// MAPPING
+#define I2C0_SCL  4
+#define I2C0_SDA  3
+
+#define BME280_ADDR   0X76
+
+TwoWire I2CBus0(0);
+
+
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BME280.h>
+
+Adafruit_BME280 bme; // I2C
+//Adafruit_BME280 bme(BME_CS); // hardware SPI
+//Adafruit_BME280 bme(BME_CS, BME_MOSI, BME_MISO, BME_SCK); // software SPI
+
+unsigned long delayTime = 1000;
+void printValues();
+
+void setup()
+{
+ Serial.begin(9600);
+
+
+  I2CBus0.setPins(I2C0_SDA, I2C0_SCL);
+
+
+   unsigned status;
+    
+    // default settings
+    status = bme.begin(BME280_ADDR, &I2CBus0);
+    delayTime = 1000;
+}
+
+void loop() { 
+    printValues();
+    delay(delayTime);
+}
+
+
+void printValues() {
+    Serial.print("Temperature = ");
+    Serial.print(bme.readTemperature());
+    Serial.println(" °C");
+
+    Serial.print("Pressure = ");
+
+    Serial.print(bme.readPressure() / 100.0F);
+    Serial.println(" hPa");
+
+    Serial.print("Humidity = ");
+    Serial.print(bme.readHumidity());
+    Serial.println(" %");
+
+    Serial.println();
+}
+

main/src/config/app_config.h

@@ -0,0 +1,86 @@
+/**
+ * @file  app_config.h
+ * @brief Application-logic configuration file.
+ * @author Matus Necas
+ */
+
+#ifndef _APP_CONFIG_H_
+#define _APP_CONFIG_H_
+
+#include <cstdint>
+
+// Timing Values
+#define APP_LOOP_TIMEOUT_MS    10
+#define APP_CLEAR_DELAY_MS    200 
+
+// Return Flags
+#define APP_ALWAYS_READY        1
+#define APP_HAS_NO_PARAM        0
+
+// Number of Onboard Sensors
+#define APP_SENSOR_CNT          5
+
+/**
+ * @brief Sensor ID enum type.
+ */
+typedef enum
+{
+    ALL_ID = 0,     ///< all sensors
+    TOF_ID = 1,     ///< TOF distance sensor
+    ACC_ID = 2,     ///< accelerometer
+    GYR_ID = 3,     ///< gyroscope
+    TMP_ID = 4,     ///< thermometer
+    HUM_ID = 5      ///< humidity gauge
+
+} app_sid_t;
+
+// Sensor Value Multipliers
+#define APP_TOF_VMUL    10  
+#define APP_ACC_VMUL    10  
+#define APP_GYR_VMUL    10
+#define APP_TMP_VMUL   100
+#define APP_HUM_VMUL   100
+
+// Sensor Value Signs
+#define APP_TOF_VSGN    0    // unsigned
+#define APP_ACC_VSGN    1    // signed
+#define APP_GYR_VSGN    1    // signed
+#define APP_TMP_VSGN    1    // signed
+#define APP_HUM_VSGN    0    // unsigned
+
+// Sensor Logarithmic Unit Multipliers
+#define APP_TOF_ULMUL  -3   // mm
+#define APP_ACC_ULMUL   0   // g's (m/s^2)
+#define APP_GYR_ULMUL   0   // deg/s
+#define APP_TMP_ULMUL   0   // *C
+#define APP_HUM_ULMUL   0   // %
+
+// Sensor Logarithmic Value Multipliers
+#define APP_TOF_VLMUL   0   
+#define APP_ACC_VLMUL  -1   // div 10
+#define APP_GYR_VLMUL  -1   // div 10
+#define APP_TMP_VLMUL  -2   // div 100
+#define APP_HUM_VLMUL  -2   // div 100
+
+// Number of bytes per sensor value
+#define APP_TOF_BCNT    2
+#define APP_ACC_BCNT    2
+#define APP_GYR_BCNT    2
+#define APP_TMP_BCNT    2
+#define APP_HUM_BCNT    2
+
+// Number of sensor values
+#define APP_TOF_VCNT    3
+#define APP_ACC_VCNT    3
+#define APP_GYR_VCNT    3
+#define APP_TMP_VCNT    1
+#define APP_HUM_VCNT    1
+
+#define APP_TOF_NAN   UINT16_MAX
+#define APP_ACC_NAN   INT16_MAX
+#define APP_GYR_NAN   INT16_MAX
+#define APP_TMP_NAN   INT16_MAX
+#define APP_HUM_NAN   UINT16_MAX
+
+
+#endif // _APP_CONFIG_H_

main/src/config/hardware_config.h

@@ -0,0 +1,39 @@
+/**
+ * @file  hardware_config.h
+ * @brief Hardware configuration file.
+ * @author Matus Necas
+ */
+
+#ifndef _HARDWARE_CONFIG_H_
+#define _HARDWARE_CONFIG_H_
+
+
+// Serial Baud
+#define SERIAL_BAUD           921600
+
+// I2C #0
+#define I2C0_BUS              Wire
+#define I2C0_SCL              4
+#define I2C0_SDA              3
+
+// SPI #0
+#define SPI0_BUS              SPI
+#define SPI0_CS               8
+#define SPI0_MISO             7
+#define SPI0_SCLK             6
+#define SPI0_MOSI             5
+
+// HC-SR04
+#define HCSR04_TRIG_X         9
+#define HCSR04_ECHO_X        10
+
+#define HCSR04_TRIG_Y        20
+#define HCSR04_ECHO_Y        21
+
+#define HCSR04_TRIG_Z         2
+#define HCSR04_ECHO_Z         1
+
+// BME280
+#define BME280_ADDR        0x76
+
+#endif // _HARDWARE_CONFIG_H_

main/src/config/master_config.h

@@ -0,0 +1,16 @@
+/**
+ * @file  master_config.h
+ * @brief Master (all-in-one) configuration file.
+ * @author Matus Necas
+ */
+
+#ifndef _MASTER_CONFIG_H_
+#define _MASTER_CONFIG_H_
+
+#include "hardware_config.h"
+#include "nbus_config.h"
+#include "app_config.h"
+#include "task_config.h"
+#include "device/device.h"
+
+#endif // _MASTER_CONFIG_H_

main/src/config/nbus_config.h

@@ -0,0 +1,37 @@
+/**
+ * @file  nbus_config.h
+ * @brief NBus configuration file.
+ * @author Matus Necas
+ */
+
+#ifndef __NBUS_CONFIG_H__
+#define __NBUS_CONFIG_H__
+
+// Host Platform Config
+#define USE_ARDUINO_FRAMWORK 1
+
+// Master-Slave Config
+#define MODULE_MASTER 0  
+#define MODULE_SLAVE 1
+
+// Module Config
+#define MODULE_ADDRESS 5
+#define VERSION_FW "01"         // MAJOR MINOR (MUST BE 2 BYTES LONG)
+#define VERSION_HW "02"         // MAJOR MINOR (MUST BE 2 BYTES LONG)
+#define MODULE_NAME "RhumbaBM"  // MUST BE 8 BYTES LONG
+#define MODULE_TYPE "TAG"       // TOF, ACC, GYRO (MUST BE 3 BYTES LONG) 
+
+// App Config
+#define CRC8_INIT_VALUE 0x0
+
+
+/** @brief Macro for header size in bridge-cast. **/
+#define NBUS_AUTO_HEADER_SIZE		    10
+/** @brief Macro for header byte in bridge-cast. **/
+#define NBUS_AUTO_HEADER_SEQ			0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0xDD, 0xCC, 0xBB, 0xAA
+
+#define NBUS_USE_LED 0
+#define NBUS_LED_POLARITY_REVERSED 1
+#define NBUS_LED_PIN 8
+
+#endif // __NBUS_CONFIG_H__

main/src/config/task_config.h

@@ -0,0 +1,27 @@
+/**
+ * @file  task_config.h
+ * @brief Task-logic configuration file.
+ * @author Matus Necas
+ */
+
+#ifndef _TASK_CONFIG_H_
+#define _TASK_CONFIG_H_
+
+
+// Task Stack Depth
+#define TASK_TOF_STACK_DEPTH       4096
+#define TASK_IMU_STACK_DEPTH       4096
+#define TASK_ENV_STACK_DEPTH       4096
+
+// Task Priority
+#define TASK_TOF_PRIORITY          1 
+#define TASK_IMU_PRIORITY          1
+#define TASK_ENV_PRIORITY          1
+
+// Task Names
+#define TASK_TOF_NAME             "read_tof_task"      
+#define TASK_IMU_NAME             "read_imu_task"      
+#define TASK_ENV_NAME             "read_env_task"         
+
+
+#endif // _TASK_CONFIG_H_

main/src/data_acquirer/data_acquirer.cpp

@@ -0,0 +1,175 @@
+#include "data_acquirer.h"
+
+SharedData<tof_values_t> tof_data_reg;
+SharedData<imu_values_t> imu_data_reg;
+SharedData<env_values_t> env_data_reg;
+
+bool in_continuous_mode = false;
+
+bool _read_tof(tof_values_t &data)
+{
+    data.x = int16_t(Dev.TofX.measureDistanceCm() * APP_TOF_VMUL);
+    data.y = int16_t(Dev.TofY.measureDistanceCm() * APP_TOF_VMUL);
+    data.z = int16_t(Dev.TofZ.measureDistanceCm() * APP_TOF_VMUL);
+    return true;
+}
+
+
+bool _read_imu(imu_values_t &data)
+{
+    if (Dev.Imu.dataReady())
+    {
+        Dev.Imu.getAGMT();    
+        data.ax = int16_t(Dev.Imu.accX() * APP_ACC_VMUL);
+        data.ay = int16_t(Dev.Imu.accY() * APP_ACC_VMUL);
+        data.az = int16_t(Dev.Imu.accZ() * APP_ACC_VMUL);
+        data.gx = int16_t(Dev.Imu.gyrX() * APP_GYR_VMUL);
+        data.gy = int16_t(Dev.Imu.gyrY() * APP_GYR_VMUL);
+        data.gz = int16_t(Dev.Imu.gyrZ() * APP_GYR_VMUL);
+        return true;
+    }
+
+    return false;
+}
+
+
+bool _read_env(env_values_t &data)
+{
+    data.temp = int16_t(Dev.Env.readTemperature()  * APP_TMP_VMUL);
+    data.hum  = int16_t(Dev.Env.readHumidity() * APP_HUM_VMUL);
+    return true;
+}
+
+
+void set_working_mode(bool option)
+{
+    in_continuous_mode = option;
+}
+
+
+void read_tof_task(void *params) 
+{
+    tof_values_t tof;
+  
+    while (1) 
+    {
+        if (in_continuous_mode && _read_tof(tof))
+        {
+            tof_data_reg.write(tof);
+        }
+        else
+        {
+            yield();
+        }
+    }
+}
+
+
+void read_imu_task(void *params) 
+{
+    imu_values_t imu;
+
+    while (1) 
+    {
+        if (in_continuous_mode && _read_imu(imu))
+        {
+            imu_data_reg.write(imu);
+        }
+        else
+        {
+            yield();
+        }
+    }
+}
+
+
+void read_env_task(void *params)
+{
+    env_values_t env;
+
+    while (1)
+    {    
+        if (in_continuous_mode && _read_env(env))
+        {
+            env_data_reg.write(env);
+        }
+        else
+        {
+            yield();
+        }
+    }
+}
+
+void read_tof_data(tof_data_t &data)
+{
+    bool data_ready = false;
+    
+    if (in_continuous_mode)
+    {
+        data_ready = tof_data_reg.read(data.payload);
+    }
+    else
+    {
+        data_ready = _read_tof(data.payload);
+    }
+
+    if (data_ready == 0)
+    {
+        data.payload.x = 0;
+        data.payload.y = 0;
+        data.payload.z = 0;
+        data.data_ready = 1;
+    }
+}
+
+void read_imu_data(imu_data_t &data)
+{
+    if (in_continuous_mode > 0)
+    {
+        data.data_ready = imu_data_reg.read(data.payload);
+    }
+    else
+    {
+        while(_read_imu(data.payload) == false)
+        {
+           yield();
+        }
+
+        data.data_ready = false;
+    }
+
+    if (data.data_ready == false)
+    {
+        data.payload.ax = 0;
+        data.payload.ay = 0;
+        data.payload.az = 0;
+        data.payload.gx = 0;
+        data.payload.gy = 0;
+        data.payload.gz = 0;
+        data.data_ready = 1;
+    }
+}
+
+
+void read_env_data(env_data_t &data)
+{
+    if (in_continuous_mode)
+    {
+        data.data_ready = env_data_reg.read(data.payload);
+    }
+    else
+    {
+        data.data_ready = _read_env(data.payload);
+    }
+
+    
+    if (data.data_ready == 0)
+    {
+        data.payload.temp = 0;
+        data.payload.hum = 0;
+        data.data_ready = 1;
+    }
+
+    
+}
+

main/src/data_acquirer/data_acquirer.h

@@ -0,0 +1,22 @@
+#ifndef _DATA_ACQUIRER_H_
+#define _DATA_ACQUIRER_H_
+
+
+#include "master_config.h"
+#include "data_structures/shared_data.h"
+
+#include "data_structures/sensor_data.h"
+
+void set_working_mode(bool option);
+
+void read_tof_data(tof_data_t &data);
+void read_imu_data(imu_data_t &data);
+void read_env_data(env_data_t &data);
+
+void read_tof_task(void *params);
+void read_imu_task(void *params);
+void read_env_task(void *params);
+
+
+
+#endif // _DATA_ACQUIRER_H_

main/src/data_structures/sensor_data.h

@@ -0,0 +1,73 @@
+#ifndef _SENSOR_DATA_H_
+#define _SENSOR_DATA_H_
+
+#include <cinttypes>
+
+typedef union 
+{
+    struct __attribute__((packed, aligned(2))) 
+    {
+        int16_t x;
+        int16_t y;
+        int16_t z;
+    };
+
+    int16_t coordinates[3]; 
+} tof_values_t;
+
+
+typedef struct 
+{
+    tof_values_t payload;
+    bool data_ready;
+} tof_data_t;
+
+
+typedef union 
+{
+    struct __attribute__((packed, aligned(2))) 
+    {
+        int16_t ax;
+        int16_t ay;
+        int16_t az;
+        int16_t gx;
+        int16_t gy;
+        int16_t gz;
+    };
+
+    struct __attribute__((packed, aligned(2))) 
+    {
+        int16_t acc[3]; 
+        int16_t gyr[3];
+    };
+
+} imu_values_t;
+
+typedef struct 
+{
+    imu_values_t payload;
+    bool data_ready;
+} imu_data_t;
+
+
+
+typedef union 
+{
+    struct __attribute__((packed, aligned(2))) 
+    {
+        int16_t temp;
+        int16_t hum;
+    };
+
+    int16_t env_data[2];
+
+} env_values_t;
+
+typedef struct 
+{
+    env_values_t payload;
+    bool data_ready;
+} env_data_t;
+
+
+#endif // _SENSOR_DATA_H_

main/src/data_structures/shared_data.h

@@ -0,0 +1,69 @@
+#ifndef _SHARED_DATA_H_
+#define _SHARED_DATA_H_
+
+#include "freertos/semphr.h"
+
+
+template <typename T>
+class SharedData 
+{
+public:
+    SharedData() 
+    {
+        _mutex = xSemaphoreCreateMutex();
+        _data_ready = false;
+    }
+
+    ~SharedData() 
+    {
+        if (_mutex != NULL) 
+        {
+            vSemaphoreDelete(_mutex);
+        }
+    }
+
+    bool write(const T& data) 
+    {
+        if (xSemaphoreTake(_mutex, portMAX_DELAY) == pdTRUE) 
+        {
+            _data = data;
+            _data_ready = true;
+            xSemaphoreGive(_mutex);
+            return true;
+        }
+        return false;
+    }
+
+    bool read(T& data) 
+    {
+        if (xSemaphoreTake(_mutex, pdMS_TO_TICKS(portMAX_DELAY)) == pdTRUE) 
+        {
+            if (_data_ready) 
+            {
+                data = _data;
+                _data_ready = false;
+                xSemaphoreGive(_mutex);
+                return true;
+            }
+            xSemaphoreGive(_mutex);
+        }
+    
+        return false;
+    }
+
+    void clear()
+    {
+        if (xSemaphoreTake(_mutex, pdMS_TO_TICKS(portMAX_DELAY)) == pdTRUE) 
+        {
+            _data_ready = false;
+            xSemaphoreGive(_mutex);
+        }
+    }
+    
+private:
+    T _data;
+    bool _data_ready;
+    SemaphoreHandle_t _mutex;
+};
+
+#endif // _SHARED_DATA_H_

main/src/device/device.cpp

@@ -0,0 +1,46 @@
+#include "device.h"
+
+esp_err_t Device::hardware_init()
+{
+  // HW Buses Init
+  Serial.begin(SERIAL_BAUD);
+  I2C0_BUS.setPins(I2C0_SDA, I2C0_SCL);
+  SPI0_BUS.begin(SPI0_SCLK, SPI0_MISO, SPI0_MOSI);
+
+  // IMU Init
+  Imu.begin(SPI0_CS, SPI0_BUS);
+
+  if (Imu.status != ICM_20948_Stat_Ok)
+  {
+    ESP_LOGE("IMU", "IMU failed to connect!");
+    return ESP_ERR_NOT_FOUND;
+  }
+
+  // 1. Vytvorte si štruktúru pre delič frekvencie
+ICM_20948_smplrt_t mySmplrt;
+
+// 2. Vypočítajte a zadajte delič (Divider)
+// Vzorec: Sample Rate = 1125 / (1 + Divider)
+// Príklad pre ~100 Hz: 1125 / (1 + 10) = 102.2 Hz
+mySmplrt.a = 4; // Delič pre akcelerometer
+mySmplrt.g = 4; // Delič pre gyroskop
+
+// 3. (Aktivujte DLPF - bez toho delič nefunguje!)
+Imu.enableDLPF(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, true);
+
+// 4. Aplikujte nastavenie
+Imu.setSampleRate(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, mySmplrt);
+
+  // ENV Init
+  bool env_status = Env.begin(BME280_ADDR, &I2C0_BUS);
+
+  if (env_status != 1)
+  {
+    ESP_LOGE("ENV", "ENV failed to connect!");
+    return ESP_ERR_NOT_FOUND;
+  }
+
+  return ESP_OK;
+}
+
+Device Dev; // Global Instance

main/src/device/device.h

@@ -0,0 +1,34 @@
+#ifndef _DEVICE_H_
+#define _DEVICE_H_
+
+#include "master_config.h"
+
+// Bus Includes
+#include <SPI.h>
+#include <Wire.h>
+
+// Sensor Drivers Inclues
+#include <HCSR04.h>
+#include <ICM_20948.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BME280.h>
+
+class Device
+{
+public:
+    UltraSonicDistanceSensor TofX{HCSR04_TRIG_X, HCSR04_ECHO_X};            ///< tof  x-sensor
+    UltraSonicDistanceSensor TofY{HCSR04_TRIG_Y, HCSR04_ECHO_Y};            ///< tof  y-sensor
+    UltraSonicDistanceSensor TofZ{HCSR04_TRIG_Z, HCSR04_ECHO_Z};            ///< tof  z-sensor
+    ICM_20948_SPI Imu;                                                      ///< imu sensor
+    Adafruit_BME280 Env;                                                    ///< environmental sensor
+
+    /**
+     * @brief Initialize hardware.
+     * @return error code
+     */
+    esp_err_t hardware_init();
+};
+
+extern Device Dev;
+
+#endif // _DEVICE_H_

main/src/main.cpp

@@ -1,11 +1,49 @@
-#include "Arduino.h"
+// Arduino Core
+#include <Arduino.h>
+
+// nBus Core
+#include "nbus_app.h"
+#include "memory_dummy.h"
+
+// nBus ESP32 Platform Driver
+#include "NbusSlaveDriver/nbus_app_interface_esp32.h"
+#include "NbusLoopDriver/NbusLoopEsp32.h"
+
+// nBus Application Driver
+#include "nbus_app_driver/nbus_app_driver.h"
+
+// Application Config
+#include "master_config.h"
+
+#include "EspNowSerial/EspNowSerial.h"
+
+EspNowSerial EnSerial;
+//uint8_t peer_address[] = {0x54, 0x32, 0x04, 0x03, 0x2a, 0x78}; //C6
+uint8_t peer_address[] = {0x9c, 0x9e, 0x6e, 0xe0,0x7c, 0x68}; //C3+
+NbusLoopEsp32 loop_driver(EnSerial, 10);
+
 
 void setup()
 {
+  Dev.hardware_init();
+
+  EnSerial.begin(11, false);
+  EnSerial.add_peer(peer_address);
+
+  // initialize nBus stack
+  nbus_init(getAppdrvDriver(), nbus_platform_init(&loop_driver));
+  nbus_init_memory_driver(getDummyMemDriver());
+  nbus_init_app(NULL, NULL);
 
+  // initialize measuring tasks
+  xTaskCreate(read_tof_task, TASK_TOF_NAME, TASK_TOF_STACK_DEPTH, NULL, TASK_TOF_PRIORITY, NULL);
+  xTaskCreate(read_imu_task, TASK_IMU_NAME, TASK_IMU_STACK_DEPTH, NULL, TASK_IMU_PRIORITY, NULL);
+  xTaskCreate(read_env_task, TASK_ENV_NAME, TASK_ENV_STACK_DEPTH, NULL, TASK_ENV_PRIORITY, NULL);
 }
 
 void loop()
 {
-    
+  // run nBus
+  nbus_stack();
+  nbus_platform_loop();
 }

main/src/nbus_app_driver/nbus_app_driver.cpp

@@ -0,0 +1,249 @@
+#include "nbus_app_driver.h"
+
+ tof_data_t tof_data;
+ imu_data_t imu_data;
+ env_data_t env_data;
+
+ int counter = 0;
+ int i = 0;
+
+static nBusAppInterface_t appdrv = {
+    appdrv_init,     appdrv_reset,    appdrv_getType, appdrv_getSensorCount,    appdrv_getData, appdrv_setData,   appdrv_hasParam,
+    appdrv_getParam, appdrv_setParam, appdrv_start,   appdrv_stop, appdrv_read, appdrv_store,   appdrv_calibrate, appdrv_getSensorFormat, 
+    appdrv_find,     appdrv_device_ready
+};
+
+
+nBusAppInterface_t *getAppdrvDriver()
+{
+    return &appdrv;
+}
+
+void appdrv_init(void *hw_interface, void *hw_config)
+{
+
+}
+
+void appdrv_reset()
+{
+    return; // do nothing
+}
+
+nBus_sensorType_t appdrv_getType(uint8_t sensor_index)
+{
+    switch (sensor_index)
+    {
+    case TOF_ID:
+        return TYPE_LENGTH_GAUGE;
+        break;
+    case ACC_ID:
+        return TYPE_ACCELEROMETER;
+        break;
+    case GYR_ID:
+        return TYPE_GYROSCOPE;
+        break;
+    case TMP_ID:
+        return TYPE_THERMOMETER;
+        break;
+    case HUM_ID:
+        return TYPE_HYGROMETER;
+        break;   
+    default:
+        return TYPE_UNKNOWN;
+    }
+}
+
+nBus_sensorCount_t appdrv_getSensorCount()
+{
+    return {.read_only_count=APP_SENSOR_CNT, .read_write_count=0};
+}
+
+uint8_t appdrv_getData(uint8_t sensor_index, uint8_t *data)
+{
+    uint16_t data_size = 0;
+
+    if (_or(sensor_index, ALL_ID, TOF_ID))
+    {
+        read_tof_data(tof_data);
+        //tof_data.payload.x = i++;
+        data_size = _write_sensor_values(data, data_size, TOF_ID, tof_data.payload.coordinates, APP_TOF_VCNT);
+    }
+
+
+    if (_or(sensor_index, ALL_ID, ACC_ID, GYR_ID))
+    {
+        read_imu_data(imu_data);
+        
+        if (_or(sensor_index, ALL_ID, TOF_ID))
+            data_size = _write_sensor_values(data, data_size, ACC_ID, imu_data.payload.acc, APP_ACC_VCNT);
+
+        if (_or(sensor_index, ALL_ID, GYR_ID))
+            data_size = _write_sensor_values(data, data_size, GYR_ID, imu_data.payload.gyr, APP_GYR_VCNT);
+    }
+
+
+    if (_or(sensor_index, ALL_ID, TMP_ID, HUM_ID))
+    {
+        read_env_data(env_data);
+
+        if (_or(sensor_index, ALL_ID, TMP_ID))
+            data_size = _write_sensor_values(data, data_size, TMP_ID, &env_data.payload.temp, APP_TMP_VCNT);
+
+        if (_or(sensor_index, ALL_ID, HUM_ID))
+            data_size = _write_sensor_values(data, data_size, HUM_ID, &env_data.payload.hum, APP_HUM_VCNT);
+    }
+
+    counter++;
+
+    return data_size;
+}
+
+nBus_statusType_t appdrv_setData(uint8_t *data, uint8_t count, uint8_t *response)
+{
+    return STATUS_NOT_SUPPORTED;
+}
+
+uint8_t appdrv_hasParam(uint8_t sensor_index, nBus_param_t param_name)
+{
+    return APP_HAS_NO_PARAM;
+}
+
+int32_t appdrv_getParam(uint8_t sensor_index, nBus_param_t param_name)
+{
+    return PARAM_VALUE_NONE;
+}
+
+nBus_statusType_t appdrv_setParam(uint8_t sensor_index, nBus_param_t param_name, int32_t param_value)
+{
+    return STATUS_NOT_SUPPORTED;
+}
+
+nBus_statusType_t appdrv_start(void)
+{
+    counter = 0;
+    i = 0;
+    set_working_mode(true);
+    return STATUS_SUCCESS;
+}
+
+nBus_statusType_t appdrv_stop(void)
+{
+    Serial.println(counter);
+    set_working_mode(false);
+    return STATUS_SUCCESS;
+}
+
+void appdrv_read(void)
+{
+    return; // do nothing
+}
+
+uint8_t appdrv_store(void)
+{
+    return 0;   // do nothing
+}
+
+nBus_statusType_t appdrv_calibrate(uint8_t sensor_index)
+{
+    return STATUS_SUCCESS;
+}
+
+
+nBus_sensorFormat_t appdrv_getSensorFormat(uint8_t sensor_index)
+{
+    switch (sensor_index)
+    {
+    case TOF_ID:
+        return (nBus_sensorFormat_t) {
+            .sign = APP_TOF_VSGN,           
+            .unit_multiplier = APP_TOF_ULMUL,
+            .value_multiplier = APP_TOF_VLMUL,
+            .byte_length = APP_TOF_BCNT,    
+            .samples = APP_TOF_VCNT
+        };
+        break;
+    
+    case ACC_ID:
+        return (nBus_sensorFormat_t) {
+            .sign = APP_ACC_VSGN,
+            .unit_multiplier = APP_ACC_ULMUL,      
+            .value_multiplier = APP_ACC_VLMUL,
+            .byte_length = APP_ACC_BCNT,
+            .samples = APP_ACC_VCNT
+        };
+        break;
+
+    case GYR_ID:
+        return (nBus_sensorFormat_t) {
+            .sign = APP_GYR_VSGN,
+            .unit_multiplier = APP_GYR_ULMUL,
+            .value_multiplier = APP_GYR_VLMUL,
+            .byte_length = APP_GYR_BCNT,
+            .samples = APP_GYR_VCNT
+        };
+        break;
+
+     case TMP_ID:
+        return (nBus_sensorFormat_t) {
+            .sign = APP_TMP_VSGN,
+            .unit_multiplier = APP_TMP_ULMUL,
+            .value_multiplier = APP_TMP_VLMUL,
+            .byte_length = APP_TMP_BCNT,
+            .samples = APP_TMP_VCNT
+        };
+        break;
+
+    case HUM_ID:
+        return (nBus_sensorFormat_t) {
+            .sign = APP_HUM_VSGN,
+            .unit_multiplier = APP_HUM_ULMUL,
+            .value_multiplier = APP_HUM_VLMUL,
+            .byte_length = APP_HUM_BCNT,
+            .samples = APP_HUM_VCNT
+        };
+        break;
+    
+    default:
+        return (nBus_sensorFormat_t) {
+                .sign = 0,
+                .unit_multiplier = 0,
+                .value_multiplier = 0,      
+                .byte_length = 0,           
+                .samples = 0
+            };
+    };
+}
+
+nBus_statusType_t appdrv_find(uint8_t enable)
+{
+    return STATUS_NOT_SUPPORTED;
+}
+
+uint8_t appdrv_device_ready()
+{
+    return APP_ALWAYS_READY;
+}
+
+
+uint16_t _write_sensor_values(uint8_t *data_out, uint16_t offset_out, uint8_t sid, int16_t *values, uint16_t size)
+{
+    data_out[offset_out++] = sid;
+
+    for (int8_t i = 0; i < size; i++)
+    {
+        data_out[offset_out++] = values[i] & 0xFF;
+        data_out[offset_out++] = values[i] >> 8;
+    }
+
+    return offset_out;
+}
+
+bool _or(uint8_t x, app_sid_t a, app_sid_t b)
+{
+    return x == a || x == b;
+}
+
+bool _or(uint8_t x, app_sid_t a, app_sid_t b, app_sid_t c)
+{
+    return x == a || x == b || x == c;
+}

main/src/nbus_app_driver/nbus_app_driver.h

@@ -0,0 +1,87 @@
+#ifndef _NBUS_APPDRV_H_
+#define _NBUS_APPDRV_H_
+
+/** 
+ * @brief Implementation of nbus app driver
+ * Embed NBus driver.
+ */
+
+#include <stdint.h>
+#include "app_bridge.h"
+#include <stdbool.h>
+
+#include "master_config.h"
+#include "data_acquirer/data_acquirer.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+///////////////////////////////////////// Extended functionality of driver //////////////////////////////////////////
+
+/** 
+ * @brief Constructor of appdrv Driver.
+ * @param: data buffer for measures
+ * @return: pointer to interface
+ */
+nBusAppInterface_t *getAppdrvDriver();
+
+ ///////////////////////////////////////// Implementation of nBusAppInterface_t ////////////////////////////////////////
+
+void appdrv_init(void *hw_interface, void *hw_config);
+void appdrv_reset();
+nBus_sensorType_t appdrv_getType(uint8_t sensor_index);
+nBus_sensorCount_t appdrv_getSensorCount();
+uint8_t appdrv_getData(uint8_t sensor_index, uint8_t *data);
+nBus_statusType_t appdrv_setData(uint8_t *data, uint8_t count, uint8_t *response);
+uint8_t appdrv_hasParam(uint8_t sensor_index, nBus_param_t param_name);
+int32_t appdrv_getParam(uint8_t sensor_index, nBus_param_t param_name);
+nBus_statusType_t appdrv_setParam(uint8_t sensor_index, nBus_param_t param_name, int32_t param_value);
+nBus_statusType_t appdrv_start(void);
+nBus_statusType_t appdrv_stop(void);
+void appdrv_read(void);
+uint8_t appdrv_store(void);
+nBus_statusType_t appdrv_calibrate(uint8_t sensor_index);
+nBus_sensorFormat_t appdrv_getSensorFormat(uint8_t sensor_index);
+nBus_statusType_t appdrv_find(uint8_t enable);
+uint8_t appdrv_device_ready();
+
+#ifdef __cplusplus
+}
+#endif
+
+//////////////////////////////////////////////// Helper Functions //////////////////////////////////////////////////
+
+
+/**
+ * @brief Write sensor values into data buffer.
+ * @param data_out output byte buffer
+ * @param offset_out buffer offset
+ * @param sid sensor id
+ * @param values sensor values to write
+ * @param size number of values to write
+ */
+uint16_t _write_sensor_values(uint8_t *data_out, uint16_t offset_out, uint8_t sid, int16_t *values, uint16_t size);
+
+/**
+ * @brief Logic or for 3 operands. Equivalent to x == a || x == b.
+ * @param x testing value
+ * @param a option 1 (sensor ID)
+ * @param b option 2 (sensor ID)
+ * @return logic value
+ */
+bool _or(uint8_t x, app_sid_t a, app_sid_t b);
+
+/**
+ * @brief Logic or for 4 operands. Equivalent to x == a || x == b || x == c.
+ * @param x testing value
+ * @param a option 1 (sensor ID)
+ * @param b option 2 (sensor ID)
+ * @param b option 3 (sensor ID)
+ * @return logic value
+ */
+bool _or(uint8_t x, app_sid_t a, app_sid_t b, app_sid_t c);
+
+
+#endif // _NBUS_APPDRV_H_

main/src/test.txt

@@ -0,0 +1,236 @@
+#include <Arduino.h>
+#include <HCSR04.h>
+#include <SPI.h>
+#include <Wire.h>
+#include <ICM_20948.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BME280.h>
+
+
+#include "freertos/semphr.h"
+
+
+
+typedef struct
+{
+  uint8_t data_ready;
+  SemaphoreHandle_t mutex;
+
+} data_flag_t;
+
+
+
+
+
+// =====================================================
+// ULTRASONIC SENSORS
+// =====================================================
+
+UltraSonicDistanceSensor distanceSensor1(9, 10);
+UltraSonicDistanceSensor distanceSensor2(20, 21);
+UltraSonicDistanceSensor distanceSensor3(2, 0);
+
+
+// =====================================================
+// ICM-20948
+// =====================================================
+
+#define CS_PIN   8
+#define MISO_PIN 7
+#define SCLK_PIN 6
+#define MOSI_PIN 5
+
+#define AD0_VAL 1
+
+ICM_20948_SPI myICM;
+
+
+// =====================================================
+// BME280
+// =====================================================
+
+#define I2C0_SCL 4
+#define I2C0_SDA 3
+
+#define BME280_ADDR 0x76
+
+TwoWire I2CBus0(0);
+
+Adafruit_BME280 bme;
+
+
+// =====================================================
+// SETUP
+// =====================================================
+
+void setup()
+{
+  Serial.begin(115200);
+
+  delay(1000);
+
+  Serial.println();
+  Serial.println("================================");
+  Serial.println("      START SENZOROV");
+  Serial.println("================================");
+
+
+  // ---------------------------------------------------
+  // ICM-20948 SPI
+  // ---------------------------------------------------
+
+  Serial.println("Inicializujem ICM-20948...");
+
+  SPI.begin(SCLK_PIN, MISO_PIN, MOSI_PIN);
+
+  bool icmOK = false;
+
+  while (!icmOK)
+  {
+    myICM.begin(CS_PIN, SPI);
+
+    Serial.print("ICM-20948: ");
+    Serial.println(myICM.statusString());
+
+    if (myICM.status == ICM_20948_Stat_Ok)
+    {
+      icmOK = true;
+      Serial.println("ICM-20948 OK!");
+    }
+    else
+    {
+      Serial.println("ICM-20948 chyba - skusam znova...");
+      delay(1000);
+    }
+  }
+
+
+  // ---------------------------------------------------
+  // BME280 I2C
+  // ---------------------------------------------------
+
+  Serial.println();
+  Serial.println("Inicializujem BME280...");
+
+  I2CBus0.setPins(I2C0_SDA, I2C0_SCL);
+
+  bool bmeOK = bme.begin(BME280_ADDR, &I2CBus0);
+
+  if (bmeOK)
+  {
+    Serial.println("BME280 OK!");
+  }
+  else
+  {
+    Serial.println("BME280 CHYBA!");
+  }
+}
+
+
+// =====================================================
+// LOOP
+// =====================================================
+
+void loop()
+{
+  // ===================================================
+  // ULTRASONIC
+  // ===================================================
+
+  float distance1 = distanceSensor1.measureDistanceCm();
+  float distance2 = distanceSensor2.measureDistanceCm();
+  float distance3 = distanceSensor3.measureDistanceCm();
+
+
+  // ===================================================
+  // ICM-20948
+  // ===================================================
+
+  if (myICM.dataReady())
+  {
+    myICM.getAGMT();
+  }
+
+
+  // ===================================================
+  // BME280
+  // ===================================================
+
+  float temperatureBME = bme.readTemperature();
+  float pressureBME = bme.readPressure() / 100.0F;
+  float humidityBME = bme.readHumidity();
+
+
+  // ===================================================
+  // SERIAL OUTPUT
+  // ===================================================
+
+  Serial.println("--------------------------------");
+
+  // Ultrasonic
+  Serial.println("ULTRASONIC:");
+
+  Serial.print("US1: ");
+  Serial.print(distance1);
+  Serial.println(" cm");
+
+  Serial.print("US2: ");
+  Serial.print(distance2);
+  Serial.println(" cm");
+
+  Serial.print("US3: ");
+  Serial.print(distance3);
+  Serial.println(" cm");
+
+
+  // ICM-20948
+  Serial.println();
+  Serial.println("ICM-20948:");
+
+  Serial.print("ACC [mg]  X: ");
+  Serial.print(myICM.accX());
+  Serial.print("  Y: ");
+  Serial.print(myICM.accY());
+  Serial.print("  Z: ");
+  Serial.println(myICM.accZ());
+
+  Serial.print("GYR [DPS] X: ");
+  Serial.print(myICM.gyrX());
+  Serial.print("  Y: ");
+  Serial.print(myICM.gyrY());
+  Serial.print("  Z: ");
+  Serial.println(myICM.gyrZ());
+
+  Serial.print("MAG [uT]  X: ");
+  Serial.print(myICM.magX());
+  Serial.print("  Y: ");
+  Serial.print(myICM.magY());
+  Serial.print("  Z: ");
+  Serial.println(myICM.magZ());
+
+  Serial.print("IMU TEMP: ");
+  Serial.print(myICM.temp());
+  Serial.println(" C");
+
+
+  // BME280
+  Serial.println();
+  Serial.println("BME280:");
+
+  Serial.print("Temperature: ");
+  Serial.print(temperatureBME);
+  Serial.println(" C");
+
+  Serial.print("Pressure:    ");
+  Serial.print(pressureBME);
+  Serial.println(" hPa");
+
+  Serial.print("Humidity:    ");
+  Serial.print(humidityBME);
+  Serial.println(" %");
+
+
+  Serial.println("--------------------------------");
+
+  delay(500);
+}