From 314d4962a57d0bb516be8ebb15dda5853528ef26 Mon Sep 17 00:00:00 2001
From: Martin Locker <martin.locker@seznam.cz>
Date: Mon, 4 Aug 2025 13:27:12 +0200
Subject: [PATCH 1/7] Add IMU, Servo, Neopixel

---
 esp/Robot/lib/Ina219/INA219_WE.cpp |   7 +-
 esp/Robot/lib/Ina219/INA219_WE.h   |  13 +-
 esp/Robot/src/communication.cpp    |   5 +-
 esp/Robot/src/communication.h      |   2 +-
 esp/Robot/src/data.h               |   6 +
 esp/Robot/src/main.cpp             |  40 +++-
 esp/Robot/src/power.cpp            |   2 +-
 esp/Robot/src/power.h              |   2 +-
 esp/lib/GPS/gps.cpp                | 105 ++++++++-
 esp/lib/GPS/gps.h                  |  26 ++-
 esp/lib/IMU/IMU.cpp                | 322 ++++++++++++++++++++++++++
 esp/lib/IMU/IMU.h                  |  70 ++++++
 esp/lib/IMU/IMU_9DOF.h             | 352 +++++++++++++++++++++++++++++
 esp/lib/IMU/i2c.cpp                |   9 +
 esp/lib/IMU/i2c.h                  |  18 ++
 esp/lib/IMU/quaternionFilters.cpp  | 200 ++++++++++++++++
 esp/lib/IMU/quaternionFilters.h    |  12 +
 esp/lib/NeoPixel/ws2812.cpp        |  63 ++++++
 esp/lib/NeoPixel/ws2812.h          |  91 ++++++++
 esp/lib/Servo/servo.cpp            |  25 ++
 esp/lib/Servo/servo.h              |  22 ++
 esp/{matty-v4.txt => matty-v8.txt} |  61 +++--
 22 files changed, 1387 insertions(+), 66 deletions(-)
 create mode 100644 esp/lib/IMU/IMU.cpp
 create mode 100644 esp/lib/IMU/IMU.h
 create mode 100644 esp/lib/IMU/IMU_9DOF.h
 create mode 100644 esp/lib/IMU/i2c.cpp
 create mode 100644 esp/lib/IMU/i2c.h
 create mode 100644 esp/lib/IMU/quaternionFilters.cpp
 create mode 100644 esp/lib/IMU/quaternionFilters.h
 create mode 100644 esp/lib/NeoPixel/ws2812.cpp
 create mode 100644 esp/lib/NeoPixel/ws2812.h
 create mode 100644 esp/lib/Servo/servo.cpp
 create mode 100644 esp/lib/Servo/servo.h
 rename esp/{matty-v4.txt => matty-v8.txt} (69%)

diff --git a/esp/Robot/lib/Ina219/INA219_WE.cpp b/esp/Robot/lib/Ina219/INA219_WE.cpp
index 0fedd37..e0e7f67 100644
--- a/esp/Robot/lib/Ina219/INA219_WE.cpp
+++ b/esp/Robot/lib/Ina219/INA219_WE.cpp
@@ -220,18 +220,22 @@ void INA219_WE::powerUp(){
 
 #ifndef USE_TINY_WIRE_M_
 uint8_t INA219_WE::writeRegister(uint8_t reg, uint16_t val){
+    _wire->lock();
     _wire->beginTransmission(i2cAddress);
     uint8_t lVal = val & 255;
     uint8_t hVal = val >> 8;
     _wire->write(reg);
     _wire->write(hVal);
     _wire->write(lVal);
-    return _wire->endTransmission();
+    uint8_t ret = _wire->endTransmission(); 
+    _wire->unlock();
+    return ret;
 }
   
 uint16_t INA219_WE::readRegister(uint8_t reg){
     uint8_t MSByte = 0, LSByte = 0;
     uint16_t regValue = 0;
+    _wire->lock();
     _wire->beginTransmission(i2cAddress);
     _wire->write(reg);
     _wire->endTransmission(false);
@@ -240,6 +244,7 @@ uint16_t INA219_WE::readRegister(uint8_t reg){
         MSByte = _wire->read();
         LSByte = _wire->read();
     }
+    _wire->unlock();
     regValue = (MSByte<<8) + LSByte;
     return regValue;
 }
diff --git a/esp/Robot/lib/Ina219/INA219_WE.h b/esp/Robot/lib/Ina219/INA219_WE.h
index e4e020a..0372bf2 100644
--- a/esp/Robot/lib/Ina219/INA219_WE.h
+++ b/esp/Robot/lib/Ina219/INA219_WE.h
@@ -26,12 +26,7 @@
 #endif
 #include "ina219_config.h"
 
-#ifdef USE_TINY_WIRE_M_
- #include <TinyWireM.h>
-#endif
-#ifndef USE_TINY_WIRE_M_
- #include <Wire.h>
-#endif
+#include "i2c.h"
 
 typedef enum INA219_ADC_MODE{
     BIT_MODE_9      = 0b00000000,   
@@ -85,8 +80,8 @@ class INA219_WE
     
         // Constructors: if not passed 0x40 / Wire will be set as address / wire object
 #ifndef USE_TINY_WIRE_M_   
-        INA219_WE(const uint8_t addr = 0x40) : _wire{&Wire}, i2cAddress{addr} {}
-        INA219_WE(TwoWire *w, const uint8_t addr = 0x40) : _wire{w}, i2cAddress{addr} {}
+        INA219_WE(const uint8_t addr = 0x40) : _wire{&Wire2}, i2cAddress{addr} {}
+        INA219_WE(I2C *w, const uint8_t addr = 0x40) : _wire{w}, i2cAddress{addr} {}
 #else
         INA219_WE(const uint8_t addr = 0x40) : i2cAddress{addr} {}
 #endif   
@@ -118,7 +113,7 @@ class INA219_WE
         INA219_PGAIN devicePGain;
         INA219_BUS_RANGE deviceBusRange;
 #ifndef USE_TINY_WIRE_M_    
-        TwoWire *_wire;
+        I2C *_wire;
 #endif
         uint8_t i2cAddress;
         uint16_t calVal;
diff --git a/esp/Robot/src/communication.cpp b/esp/Robot/src/communication.cpp
index 20a14d0..fc2043d 100644
--- a/esp/Robot/src/communication.cpp
+++ b/esp/Robot/src/communication.cpp
@@ -21,7 +21,7 @@ void Comm::send(uint8_t message, void* data, uint8_t length) {
   transmit(&tx, length + 2);
 }
 
-void Comm::send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4]) {
+void Comm::send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4], float roll, float pitch, float yaw) {
   txData.counter = infoCounter++;
   txData.message = 'I';
   txData.status = status;
@@ -34,5 +34,8 @@ void Comm::send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current
   txData.encoder[1] = encoder[1] * STEP;
   txData.encoder[2] = encoder[2] * STEP;
   txData.encoder[3] = encoder[3] * STEP;
+  txData.roll  =  round(roll * 100);
+  txData.pitch =  round(pitch * 100);
+  txData.yaw   =  round(yaw * 100);
   transmit(&txData, sizeof(TransmitPacket));
 }
diff --git a/esp/Robot/src/communication.h b/esp/Robot/src/communication.h
index 09a473c..54be68e 100644
--- a/esp/Robot/src/communication.h
+++ b/esp/Robot/src/communication.h
@@ -11,7 +11,7 @@ class Comm : public Robbus {
     uint8_t   get(); 
     void      confirm(char message);
     void      send(uint8_t message, void* data = NULL, uint8_t length = 0);
-    void      send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4]);
+    void      send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4], float roll, float pitch, float yaw);
     ReceivePacket   rxData;
   private:
     uint8_t   infoCounter = 0;
diff --git a/esp/Robot/src/data.h b/esp/Robot/src/data.h
index ab68e00..778a686 100644
--- a/esp/Robot/src/data.h
+++ b/esp/Robot/src/data.h
@@ -30,6 +30,11 @@ struct __attribute__((packed)) ReceivePacket {
       uint16_t period;    // ms
       uint16_t timeout;   // ms
     };
+    struct {
+      int index:8;
+      int rgb:24;
+    };
+    uint32_t data;
   };
 };
 
@@ -43,6 +48,7 @@ struct __attribute__((packed)) TransmitPacket {
   int16_t  speed;         // mm/s
   int16_t  angle;         // 0,01°
   uint16_t encoder[4];    // mm
+  int16_t  roll, pitch, yaw;
 };
 
 struct __attribute__((packed)) TransmitData {
diff --git a/esp/Robot/src/main.cpp b/esp/Robot/src/main.cpp
index 33a48a7..017ac00 100644
--- a/esp/Robot/src/main.cpp
+++ b/esp/Robot/src/main.cpp
@@ -62,20 +62,27 @@ void receiveCommand() {
         case 'L': robot.setLimits(comm.rxData.speed, comm.rxData.steer);
                   break;
         case 'T': robot.setTime(comm.rxData.period, comm.rxData.timeout);
+                  robot.led(0, COLOR::GREEN);
                   break;
         case 'P': gps.config(comm.rxData.mode);
                   break;
-        case 'V': {
-          // send current version
-          uint8_t buffer[2];
-          buffer[0] = MATTY;  // serial number
-          buffer[1] = ROBOT_FW_VERSION;
-          uint8_t length = 2;
-          comm.send('V', buffer, length);
-          break;
-        }
         case 'R': robot.reset();
                   break;
+        case 'V': { // send serial number, current version
+                    uint8_t buffer[] = {MATTY, ROBOT_FW_VERSION};
+                    comm.send('V', buffer, 2);
+                  }
+                  break;
+#ifdef SERVO_PIN                  
+        case 'C': robot.setServo(comm.rxData.period); // container
+                  break;
+#endif                  
+#ifdef LED_PIN                  
+        case 'D': robot.led(comm.rxData.index, comm.rxData.rgb); // neopixel led
+                  break;
+#endif                  
+        case 'X': robot.writePosition((uint8_t)comm.rxData.period, comm.rxData.timeout, 1000);
+                  break;
       }
       if (robot.mode == AUTONOMOUS) {
         switch (comm.rxData.command) {
@@ -113,13 +120,24 @@ void loop() {
   receiveCommand();
 
   if (robot.process()) { // periodicke odesilani dat
-    comm.send(robot.status, robot.mode, robot.voltage, robot.current, robot.actualSpeed, robot.joint, robot.encoder);
+    comm.send(robot.status, robot.mode, robot.voltage, robot.current, robot.actualSpeed, robot.joint, robot.encoder, robot.roll, robot.pitch, robot.yaw);
+    static uint8_t lastStatus;
+    if ((lastStatus ^ robot.status) & ROBOT_STATUS::EMERGENCY_STOP) {
+      if (robot.status & ROBOT_STATUS::EMERGENCY_STOP) {
+        robot.led(1, COLOR::RED);
+      } else {
+        robot.led(1, COLOR::GREEN);
+      }
+    }
+    lastStatus = robot.status;
   }
 
   if (gps.process()) {
     uint8_t buffer[80];
     uint8_t length = gps.get(buffer);
-    comm.send('P', buffer, length);
+    if (length != 0) {
+      comm.send('P', buffer, length);
+    }
   }
   
 }
diff --git a/esp/Robot/src/power.cpp b/esp/Robot/src/power.cpp
index fd7dd6e..5d53e33 100644
--- a/esp/Robot/src/power.cpp
+++ b/esp/Robot/src/power.cpp
@@ -4,7 +4,7 @@ Power::Power() : INA219_WE(INA219_ADDRESS) {
 }
 
 bool Power::init() {
-  Wire.begin(I2C_SDA, I2C_SCL);
+  Wire2.begin(I2C_SDA, I2C_SCL, 400000);
   if (!INA219_WE::init()) {
     return 0;
   }
diff --git a/esp/Robot/src/power.h b/esp/Robot/src/power.h
index d953a8f..bf30277 100644
--- a/esp/Robot/src/power.h
+++ b/esp/Robot/src/power.h
@@ -5,7 +5,7 @@
 #define I2C_SCL 33
 
 #include <INA219_WE.h>
-#include <Wire.h>
+#include <i2c.h>
 
 #define INA219_ADDRESS 0x42
 
diff --git a/esp/lib/GPS/gps.cpp b/esp/lib/GPS/gps.cpp
index 39244fd..5e804ec 100644
--- a/esp/lib/GPS/gps.cpp
+++ b/esp/lib/GPS/gps.cpp
@@ -16,14 +16,14 @@ bool GPS::process() {
   while (serial->available()) {
     uint8_t a = serial->read();
     if (a == '$') { // zacatek vety
-      nmea_ready = 0;
-      nmea_index = 0;
-      nmea_buffer[nmea_index++] = a;
-    } else if (nmea_index > 0 && nmea_index < NMEA_LENGTH) {
-      nmea_buffer[nmea_index++] = a;
+      nmeaReady = 0;
+      nmeaIndex = 0;
+      nmeaBuffer[nmeaIndex++] = a;
+    } else if (nmeaIndex > 0 && nmeaIndex < NMEA_LENGTH) {
+      nmeaBuffer[nmeaIndex++] = a;
       if (a == LF) { // konec vety
-        if (strncmp(&nmea_buffer[3], nmea_filter[0], 3) == 0 || strncmp(&nmea_buffer[3], nmea_filter[1], 3) == 0) {
-          nmea_ready = 1;
+        if (strncmp(&nmeaBuffer[3], nmeaFilter[0], 3) == 0 || strncmp(&nmeaBuffer[3], nmeaFilter[1], 3) == 0) {
+          nmeaReady = 1;
           return (mode != 0);
         }
       }
@@ -33,10 +33,93 @@ bool GPS::process() {
 }
 
 uint8_t GPS::get(uint8_t* buffer) {
-  if (nmea_ready) {
-    memcpy(buffer, nmea_buffer, nmea_index);
-    nmea_ready = 0;
-    return nmea_index;
+  if (nmeaReady) {
+    if (mode == 1) { // raw data
+      memcpy(buffer, nmeaBuffer, nmeaIndex);
+      nmeaReady = 0;
+      return nmeaIndex;
+    } else {
+      if (parse()) {
+        memcpy(buffer, &gpsInfo, sizeof(GPSinfo));      
+        return sizeof(GPSinfo);
+      }
+    }
   } 
   return 0;
 }
+
+int32_t GPS::atoi(char* p) {
+  int32_t x = 0;
+  int8_t sign = 0;
+  if (*p == '-') {
+    sign = 1;
+    p++;
+  }
+  while (*p >= '0' && *p <= '9') {
+    x = x * 10 + (*p - '0');
+    p++;
+  }
+  if (sign) x = -x;
+  return x;
+} 
+
+bool GPS::parse() {
+	uint8_t i;
+	char* endptr;
+	int32_t  whole, frac;
+
+  memset(&gpsInfo, 0, sizeof(GPSinfo));
+  
+  #define next(a)		while(nmeaBuffer[i++] != a);
+  #define decimal() { whole = atoi(&nmeaBuffer[i]); \
+		                  if (whole != 0)	{ \
+			                  next('.'); \
+			                  frac = atoi(&nmeaBuffer[i]); \
+		                  } \
+                    }
+
+  if (strncmp(&nmeaBuffer[3], "GGA", 3) == 0) { // GGA
+		i = 7;  		                              // start parsing just after "GGA"		
+		if(nmeaBuffer[i] == ',' && nmeaBuffer[i+1] == ',') // attempt to reject empty packets right away
+			return 0;
+			
+		gpsInfo.timeFix = atoi(&nmeaBuffer[i]);   // get UTC time [hhmmss]   ... TODO convert to unix timestamp
+
+		next(',');				                        // next field: latitude
+    decimal();                                // get latitude [ddmm.mmmmm]
+		gpsInfo.latitude = ((whole/100)*600000 + ((whole%100)*10000 + (frac+5)/10))*6; // convert to seconds [ssssssss*10e3] format
+		next(',');				                        // next field: N/S indicator
+		if (nmeaBuffer[i] == 'S') gpsInfo.latitude = -gpsInfo.latitude;  		// correct latitute for N/S
+
+		next(',');				                        // next field: longitude	
+    decimal();                                // get longitude [ddmm.mmmmm]
+		gpsInfo.longitude = ((whole/100)*600000 + ((whole%100)*10000 + (frac+5)/10))*6; // convert to seconds [ssssssss*10e3] format
+		next(',');				                        // next field: E/W indicator
+		if (nmeaBuffer[i] == 'W') gpsInfo.longitude = -gpsInfo.longitude; // correct latitute for E/W
+
+		next(',');				                        // next field: position fix status
+		gpsInfo.positionFixStatus = atoi(&nmeaBuffer[i]); // position fix status  0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
+	
+		next(',');	                        			// next field: satellites used
+		gpsInfo.numberOfSatellites = atoi(&nmeaBuffer[i]); // get number of satellites used in GPS solution
+
+		next(',');				                        // next field: HDOP (horizontal dilution of precision)
+    decimal();                                // get HDOP
+    gpsInfo.hdop = (256 * whole + frac);      // get HDOP (horizontal dilution of precisiom) (in meters * 256)
+
+		next(',');				                        // next field: altitude
+    decimal();
+    gpsInfo.altitude = (16 * whole + frac / 16);  // get altitude (in meters * 16)
+	
+    next(',');				// next field: altitude units, always 'M'
+    next(',');				// next field: geoid seperation
+    next(',');				// next field: seperation units
+    next(',');				// next field: DGPS age
+    next(',');				// next field: DGPS station ID
+    next('*');				// next field: checksum
+
+//    printf("Lat: %f Long: %f\n", gpsInfo.latitude/3.6e6, gpsInfo.longitude/3.6e6);
+		return 1;
+	}
+  return 0;
+}
diff --git a/esp/lib/GPS/gps.h b/esp/lib/GPS/gps.h
index fab4b3b..301303c 100644
--- a/esp/lib/GPS/gps.h
+++ b/esp/lib/GPS/gps.h
@@ -6,19 +6,33 @@
 #define NMEA_LENGTH   80
 #define NMEA_FILTERS  2
 
+struct __attribute__((packed)) GPSinfo {
+  uint32_t  timeFix;              // UTC time [hhmmss]  ... TODO convert to unix timestamp
+	int32_t   latitude;             // in seconds * 10e6
+	int32_t   longitude;            // in seconds * 10e6
+	uint16_t  altitude;             // in meters * 16
+	uint16_t  hdop;                 // in meters * 256
+	uint8_t   numberOfSatellites;
+	uint8_t   positionFixStatus;
+}; 
+
 class GPS {
   public:
     GPS(Stream*);
-    bool process();
-    uint8_t get(uint8_t*); // zkopiruje obsah nactene NMEA vety a vrati jeji delku
+    bool    process();
+    uint8_t get(uint8_t*); // mode: 1 .. zkopiruje obsah nactene NMEA vety a vrati jeji delku, 2 .. vrati GPSinfo
     void    config(uint8_t);
   private:
   	Stream* serial;
     uint8_t mode = 0;
-    uint8_t nmea_ready = 0;
-    uint8_t nmea_index = 0;
-    char    nmea_buffer[NMEA_LENGTH];
-    char    nmea_filter[NMEA_FILTERS][4] = {"GGA", "RMC"};
+    uint8_t nmeaReady = 0;
+    uint8_t nmeaIndex = 0;
+    char    nmeaBuffer[NMEA_LENGTH];
+    char    nmeaFilter[NMEA_FILTERS][4] = {"GGA", "RMC"};
+    GPSinfo gpsInfo;
+
+    int32_t atoi(char*);
+    bool    parse();
 };
 
 #endif
\ No newline at end of file
diff --git a/esp/lib/IMU/IMU.cpp b/esp/lib/IMU/IMU.cpp
new file mode 100644
index 0000000..783e8dc
--- /dev/null
+++ b/esp/lib/IMU/IMU.cpp
@@ -0,0 +1,322 @@
+#include "IMU.h"
+
+I2C Wire2(0);
+
+void IMU::imuTask(void *pvParameters) {
+	static_cast<IMU*>(pvParameters)->update();
+}
+
+void IMU::init() {
+	if (getId() == 0x05) {
+    configSensors();
+		xTaskCreate(&IMU::imuTask, "Task IMU", 2048, (void*)this, 1, NULL);
+  }
+}
+
+void IMU::update() {
+	uint32_t lastTime ;
+	for(;;) {
+		uint8_t x = readData();
+		if (x) {
+			uint32_t t = micros();
+			float dt = (t - lastTime) * 1.0e-6;
+			MadgwickAHRSupdate(g, a, m, dt);
+			if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
+				Quaternion(q);
+				xSemaphoreGive(mutex);
+			}
+			lastTime = t;
+		} else {
+			vTaskDelay(1 / portTICK_PERIOD_MS); // 1 ms
+		}
+	}
+}
+
+void IMU::eulerAngles(float &roll, float &pitch, float &yaw) {
+	if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
+		float qt[4];
+		qt[0] = q[0];
+		qt[1] = q[1];
+		qt[2] = q[2];
+		qt[3] = q[3];
+		xSemaphoreGive(mutex);
+		EulerAngles(qt, roll, pitch, yaw);
+	}
+}
+
+void IMU::enableAccGyro(uint8_t enableFlags) {
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl7, enableFlags);
+	delay(1);
+}
+
+void IMU::configSensors() {
+	enableAccGyro(QMI8658_DISABLE_ALL);  
+	configAcc(Qmi8658AccRange_8g, Qmi8658AccOdr_250Hz);
+  configGyro(Qmi8658GyrRange_512dps, Qmi8658GyrOdr_250Hz);
+  configMag(AK09918_CONTINUOUS_100HZ);
+//	configAcc(Qmi8658AccRange_8g, Qmi8658AccOdr_500Hz); // Obcas hazi chybne hodnoty, pri 250 Hz uz ne
+//  configGyro(Qmi8658GyrRange_512dps, Qmi8658GyrOdr_500Hz);
+	enableAccGyro(0x80 | QMI8658_ACCGYR_ENABLE);
+}
+
+uint8_t IMU::getId() {
+ 	uint8_t chipId = readRegister(QMI8658_ADDR, Qmi8658Register_WhoAmI);
+  if (chipId != 0x05)	{
+    Serial.printf("Error: Qmi8658Register_WhoAmI = 0x%x\r\n", chipId);
+  } else {
+    Serial.printf("OK: Qmi8658Register_WhoAmI = 0x%x\r\n", chipId);
+//    qmi8658_on_demand_cali();
+
+    writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl1, 0x60); // Address autoincrement, data big endian
+    writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl7, 0x00); // Disable accelerometer and gyro
+//    writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl8, 0xc0);
+  }
+	return chipId;
+}
+
+void IMU::qmi8658_on_demand_cali(void) {
+	int16_t gyroGain[3];
+
+	Serial.print("qmi8658_on_demand_cali start\r\n");
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Reset, 0xb0);
+	delay(10);	
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_On_Demand_Cali);
+	delay(2200);	// delay 2000ms above
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_NOP);
+	delay(100);	
+  uint8_t err = readRegister(QMI8658_ADDR, 70);
+  readBytes(QMI8658_ADDR, Qmi8658Register_uuid, gyroGain, 6);
+  Serial.printf("Gyro gain: %3d %5d, %5d, %5d\n\r", err, gyroGain[0], gyroGain[1], gyroGain[2]);
+	Serial.print("qmi8658_on_demand_cali done\r\n");
+}
+
+void IMU::autoCalibrate() {
+	#define SAMPLES	50
+  Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
+  delay(1000);
+
+  accBias[0] = 0;
+  accBias[1] = 0;
+  accBias[2] = 0;
+	for(int i = 0; i < SAMPLES; i++) {
+		readAccRaw();
+		accBias[0] += accRaw[0];
+		accBias[1] += accRaw[1];
+		accBias[2] += accRaw[2];
+		delay(10);
+	}
+	
+	accBias[0] *= accScale / SAMPLES;
+	accBias[1] *= accScale / SAMPLES;
+	accBias[2] *= accScale / SAMPLES;
+	accBias[2] -= 1000.0f; // - => + Opraveno
+	
+  gyroBias[0] = 0;
+  gyroBias[1] = 0;
+  gyroBias[2] = 0;
+	for(int i = 0; i < SAMPLES; i++) {
+		readGyroRaw();
+		gyroBias[0] += gyroRaw[0];
+		gyroBias[1] += gyroRaw[1];
+		gyroBias[2] += gyroRaw[2];
+		delay(2);
+	}
+	
+	gyroBias[0] *= gyroScale / SAMPLES;
+	gyroBias[1] *= gyroScale / SAMPLES;
+	gyroBias[2] *= gyroScale / SAMPLES;
+
+  Serial.printf("accBias:  %6.2f, %6.2f, %6.2f\n\r", accBias[0], accBias[1], accBias[2]);
+  Serial.printf("gyroBias: %6.2f, %6.2f, %6.2f\n\r", gyroBias[0], gyroBias[1], gyroBias[2]);
+}
+
+// x = -y, y = x ... otoceni os o 90 st.
+void IMU::readAccRaw() {
+	readBytes(QMI8658_ADDR, Qmi8658Register_Ax_L, accRaw, 6);
+  int16_t x = -accRaw[1];
+  accRaw[1] = accRaw[0];
+  accRaw[0] = x;
+}
+
+void IMU::readGyroRaw() {
+	readBytes(QMI8658_ADDR, Qmi8658Register_Gx_L, gyroRaw, 6);
+  int16_t x = -gyroRaw[1];
+  gyroRaw[1] = gyroRaw[0];
+  gyroRaw[0] = x;
+}
+
+void IMU::readMagRaw() {
+	readBytes(AK09918_ADDR, AK09918_HXL, magRaw, 6);
+  readRegister(AK09918_ADDR, AK09918_ST2);
+  int16_t x = -magRaw[1];
+  magRaw[1] = magRaw[0];
+  magRaw[0] = x;
+}
+
+void IMU::readAcc() {
+  readAccRaw();
+  // Now we'll calculate the accleration value into actual g's
+  // This depends on scale being set
+  a[0] = (float)accRaw[0] * accScale - accBias[0];
+  a[1] = (float)accRaw[1] * accScale - accBias[1];
+  a[2] = (float)accRaw[2] * accScale - accBias[2];
+}
+
+void IMU::readGyro() {
+  readGyroRaw();
+  // Calculate the gyro value into actual degrees per second
+  // This depends on scale being set
+  g[0] = (float)gyroRaw[0] * gyroScale - gyroBias[0];
+  g[1] = (float)gyroRaw[1] * gyroScale - gyroBias[1];
+  g[2] = (float)gyroRaw[2] * gyroScale - gyroBias[2];
+}
+
+void IMU::readMag() {
+  uint8_t x = readRegister(AK09918_ADDR, AK09918_ST1);
+#ifndef CALIBRATION
+	m[0] = 0.0f; m[1] = 0.0f; m[2] = 0.0f;
+#endif
+  if (x & AK09918_DRDY_BIT) {
+    readMagRaw();
+    float x = (float)magRaw[0] * magScale - magBias[0];
+    float y = (float)magRaw[1] * magScale - magBias[1];
+    float z = (float)magRaw[2] * magScale - magBias[2];
+    m[0] = x * magCorect[0][0] + y * magCorect[0][1] + z * magCorect[0][2];
+    m[1] = x * magCorect[1][0] + y * magCorect[1][1] + z * magCorect[1][2];
+    m[2] = x * magCorect[2][0] + y * magCorect[2][1] + z * magCorect[2][2];
+
+// Dle literatury m = 47 uT pro CR, ale nameril jsem cca 54 uT
+		mg = sqrtf(m[0]*m[0] + m[1]*m[1] + m[2]*m[2]);
+// zatim vyrazeno
+//		if (mg > 60.0f || mg < 40.0f) {
+//				m[0] = 0.0f; m[1] = 0.0f; m[2] = 0.0f;
+//		}
+
+  }
+}
+
+uint8_t IMU::readData() {
+	uint8_t result = 0;
+	if (Wire2.lock()) {
+		uint8_t status = readRegister(QMI8658_ADDR, Qmi8658Register_Status0);
+		if (status & 0x03 == 0x03) {
+			readAcc();
+			readGyro();
+			readMag();
+			result = 1;
+		}
+		Wire2.unlock();
+	}
+	return result;
+}
+
+void IMU::configAcc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr) {
+	unsigned char ctl_dada;
+
+	switch(range) {
+		case Qmi8658AccRange_2g:
+			accScale = 1000.0f / (1<<14);
+			break;
+		case Qmi8658AccRange_4g:
+			accScale = 1000.0f / (1<<13);
+			break;
+		case Qmi8658AccRange_8g:
+			accScale = -1000.0f / (1<<12); // ??? z neznameho duvodu je nutne otocit osy
+			break;
+		case Qmi8658AccRange_16g:
+			accScale = 1000.0f / (1<<11);
+			break;
+		default:
+			range = Qmi8658AccRange_8g;
+			accScale = 1000.0f / (1<<12);
+	}
+  ctl_dada = (uint8_t)range | (uint8_t)odr;
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl2, ctl_dada);
+
+	ctl_dada = readRegister(QMI8658_ADDR, Qmi8658Register_Ctrl5);
+	ctl_dada &= 0xf0;
+	ctl_dada &= ~0x01; // Disable low pass filter
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl5, ctl_dada);
+}
+
+void IMU::configGyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr) {
+	uint8_t ctl_dada;
+
+	// Store the scale factor for use when processing raw data
+	switch (range) {
+		case Qmi8658GyrRange_16dps:
+			gyroScale = 1.0f/2048;
+			break;
+		case Qmi8658GyrRange_32dps:
+			gyroScale = 1.0f/1024;
+			break;
+		case Qmi8658GyrRange_64dps:
+			gyroScale = 1.0f/512;
+			break;
+		case Qmi8658GyrRange_128dps:
+			gyroScale = 1.0f/256;
+			break;
+		case Qmi8658GyrRange_256dps:
+			gyroScale = 1.0f/128;
+			break;
+		case Qmi8658GyrRange_512dps:
+			gyroScale = 1.0f/64;
+			break;
+		case Qmi8658GyrRange_1024dps:
+			gyroScale = 1.0f/32;
+			break;
+		case Qmi8658GyrRange_2048dps:
+			gyroScale = 1.0f/16;
+			break;
+		default:
+			range = Qmi8658GyrRange_512dps;
+			gyroScale = 1.0f/64;
+			break;
+	}
+
+	ctl_dada = (uint8_t)range | (uint8_t)odr;
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl3, ctl_dada);
+
+	ctl_dada = readRegister(QMI8658_ADDR, Qmi8658Register_Ctrl5);
+	ctl_dada &= 0x0f;
+	ctl_dada &= ~0x10; // Disable low pass filter
+	writeRegister(QMI8658_ADDR, Qmi8658Register_Ctrl5, ctl_dada);
+}
+
+void IMU::configMag(enum AK09918_mode_type_t odr) {
+	writeRegister(AK09918_ADDR, AK09918_CNTL3, AK09918_SRST_BIT);
+	writeRegister(AK09918_ADDR, AK09918_CNTL2, odr);
+  magScale = 0.15f;
+}
+
+void IMU::writeRegister(uint8_t addr, uint8_t reg, uint8_t value) {
+  Wire2.beginTransmission(addr);
+  Wire2.write(reg);
+  Wire2.write(value);
+  Wire2.endTransmission();
+}
+
+uint8_t IMU::readRegister(uint8_t addr, uint8_t reg) {
+	Wire2.beginTransmission(addr);
+	Wire2.write(reg);
+	Wire2.endTransmission();
+	Wire2.requestFrom(addr, (uint8_t)1);
+	uint8_t ret = Wire2.read();
+	Wire2.endTransmission();
+
+	return ret;
+}
+
+void IMU::readBytes(uint8_t addr, uint8_t reg, void* data, uint8_t count) {
+  Wire2.beginTransmission(addr);
+  Wire2.write(reg);
+  Wire2.endTransmission();
+  Wire2.requestFrom(addr, count);
+	for (int i = 0; i < count; i++) {
+  	((uint8_t*)data)[i] = Wire2.read();
+  }
+  Wire2.endTransmission();
+}
+
+
+
diff --git a/esp/lib/IMU/IMU.h b/esp/lib/IMU/IMU.h
new file mode 100644
index 0000000..70a9cd2
--- /dev/null
+++ b/esp/lib/IMU/IMU.h
@@ -0,0 +1,70 @@
+#ifndef IMU_H
+#define IMU_H
+
+#include "config.h"
+#include "IMU_9DOF.h"
+#include <Arduino.h>
+#include "i2c.h"
+#include "quaternionFilters.h"
+
+#define CALIBRATION
+
+class IMU {
+  public:
+    void init();
+    void update();  // cteni a vypocet imu pro task
+    static void imuTask(void* pvParameters);
+    void eulerAngles(float &roll, float &pitch, float &yaw);
+
+    uint8_t getId();
+    void qmi8658_on_demand_cali();
+    void autoCalibrate();
+
+    void readAccRaw();
+    void readGyroRaw();
+    void readMagRaw();
+    void readAcc();
+    void readGyro();
+    void readMag();
+    uint8_t readData();
+
+    void configAcc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr);
+    void configGyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr);
+    void configMag(enum AK09918_mode_type_t odr);
+    void configSensors();
+    void enableAccGyro(unsigned char enableFlags);
+
+    int16_t accRaw[3];    // Stores the 16-bit signed accelerometer sensor output 
+    int16_t gyroRaw[3];   // Stores the 16-bit signed gyro sensor output
+    int16_t magRaw[3];    // Stores the 16-bit signed magnetometer sensor output
+    // Scale resolutions per LSB for the sensors
+    float accScale, gyroScale, magScale;
+    // Variables to hold latest sensor data values
+    float a[3], g[3], m[3];
+    float mg; // hodnota intenzity magnetick0ho pole
+    float q[4];
+
+// Pro kalibraci - vypnout korekce !!!    
+#ifdef CALIBRATION
+    // Bias corrections for gyro, accelerometer, and magnetometer
+    float accBias[3] = {0, 0, 0};
+    float gyroBias[3]  = {0, 0, 0};
+    float magBias[3]   = {0, 0, 0};
+    float magCorect[3][3]  = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 
+#else
+    float accBias[3] = ACC_BIAS;
+    float gyroBias[3] = GYRO_BIAS;
+    float magBias[3]  = MAG_BIAS;
+    float magCorect[3][3]  = MAG_CORECT; 
+#endif
+
+  private:
+    void    writeRegister(uint8_t addr, uint8_t reg, uint8_t value);
+    uint8_t readRegister(uint8_t addr, uint8_t reg);
+    void    readBytes(uint8_t addr, uint8_t reg, void* data, uint8_t count);
+
+    xSemaphoreHandle mutex = xSemaphoreCreateMutex();
+
+};
+
+#endif
diff --git a/esp/lib/IMU/IMU_9DOF.h b/esp/lib/IMU/IMU_9DOF.h
new file mode 100644
index 0000000..cd09344
--- /dev/null
+++ b/esp/lib/IMU/IMU_9DOF.h
@@ -0,0 +1,352 @@
+#ifndef IMU_9DOF_H
+#define IMU_9DOF_H
+
+//#define QMI8658_USE_SPI
+//#define QMI8658_SYNC_SAMPLE_MODE
+//#define QMI8658_SOFT_SELFTEST
+//#define QMI8658_USE_CALI
+#define QMI8658_USE_FIFO
+//#define QMI8658_USE_AMD
+//#define QMI8658_USE_PEDOMETER
+
+#define QMI8658_SLAVE_ADDR_L			0x6a
+#define QMI8658_SLAVE_ADDR_H			0x6b
+
+#define QMI8658_DISABLE_ALL				(0x0)
+#define QMI8658_ACC_ENABLE				(0x1)
+#define QMI8658_GYR_ENABLE				(0x2)
+#define QMI8658_ACCGYR_ENABLE			(QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE)
+
+#define QMI8658_STATUS1_CMD_DONE		(0x01)
+#define QMI8658_STATUS1_WAKEUP_EVENT	(0x04)
+
+#define QMI8658_CTRL8_DATAVALID_EN		0x40		// bit6:1 int1, 0 int2
+#define QMI8658_CTRL8_PEDOMETER_EN		0x10
+#define QMI8658_CTRL8_SIGMOTION_EN		0x08
+#define QMI8658_CTRL8_NOMOTION_EN		0x04
+#define QMI8658_CTRL8_ANYMOTION_EN		0x02
+#define QMI8658_CTRL8_TAP_EN			0x01
+
+#define QMI8658_INT1_ENABLE				0x08
+#define QMI8658_INT2_ENABLE				0x10
+
+#define QMI8658_DRDY_DISABLE			0x20	// ctrl7
+
+#define QMI8658_FIFO_MAP_INT1			0x04	// ctrl1
+#define QMI8658_FIFO_MAP_INT2			~0x04	// ctrl1
+
+///<Configuration Registers>
+#define QMI8658_ADDR 0X6B  //device address
+
+#define WHO_AM_I 0X00 //Device identifier
+#define CTRL1 0x02    //Serial Interface and Sensor Enable
+#define CTRL2 0x03    //Accelerometer Settings
+#define CTRL3 0x04    //Gyroscope Settings
+#define CTRL4 0X05    //Magnetometer Settings
+#define CTRL5 0X06    //Sensor Data Processing Settings
+#define CTRL7 0x08    //Enable Sensors and Configure Data Reads
+#define CTRL8 0X09    //Reserved – Special Settings
+
+///<Sensor Data Output Registers>
+#define AccX_L 0x35
+#define AccX_H 0x36
+#define AccY_L 0x37
+#define AccY_H 0x38
+#define AccZ_L 0x39
+#define AccZ_H 0x3A
+#define TEMP_L 0x33
+
+#define GyrX_L 0x3B
+#define GyrX_H 0x3C
+#define GyrY_L 0x3D
+#define GyrY_H 0x3E
+#define GyrZ_L 0x3F
+#define GyrZ_H 0x40
+
+enum Qmi8658Register {
+	Qmi8658Register_WhoAmI = 0,
+	Qmi8658Register_Revision,
+	Qmi8658Register_Ctrl1,
+	Qmi8658Register_Ctrl2,
+	Qmi8658Register_Ctrl3,
+	Qmi8658Register_Ctrl4,
+	Qmi8658Register_Ctrl5,
+	Qmi8658Register_Ctrl6,
+	Qmi8658Register_Ctrl7,
+	Qmi8658Register_Ctrl8,
+	Qmi8658Register_Ctrl9,
+	Qmi8658Register_Cal1_L = 11,
+	Qmi8658Register_Cal1_H,
+	Qmi8658Register_Cal2_L,
+	Qmi8658Register_Cal2_H,
+	Qmi8658Register_Cal3_L,
+	Qmi8658Register_Cal3_H,
+	Qmi8658Register_Cal4_L,
+	Qmi8658Register_Cal4_H,
+	Qmi8658Register_FifoWmkTh = 19,
+	Qmi8658Register_FifoCtrl = 20,
+	Qmi8658Register_FifoCount = 21,
+	Qmi8658Register_FifoStatus = 22,
+	Qmi8658Register_FifoData = 23,
+	Qmi8658Register_StatusI2CM = 44,
+	Qmi8658Register_StatusInt = 45,
+	Qmi8658Register_Status0,
+	Qmi8658Register_Status1,
+	Qmi8658Register_Timestamp_L = 48,
+	Qmi8658Register_Timestamp_M,
+	Qmi8658Register_Timestamp_H,
+	Qmi8658Register_Tempearture_L = 51,
+	Qmi8658Register_Tempearture_H,
+	Qmi8658Register_Ax_L = 53,
+	Qmi8658Register_Ax_H,
+	Qmi8658Register_Ay_L,
+	Qmi8658Register_Ay_H,
+	Qmi8658Register_Az_L,
+	Qmi8658Register_Az_H,
+	Qmi8658Register_Gx_L = 59,
+	Qmi8658Register_Gx_H,
+	Qmi8658Register_Gy_L,
+	Qmi8658Register_Gy_H,
+	Qmi8658Register_Gz_L,
+	Qmi8658Register_Gz_H,
+	Qmi8658Register_Mx_L = 65,
+	Qmi8658Register_Mx_H,
+	Qmi8658Register_My_L,
+	Qmi8658Register_My_H,
+	Qmi8658Register_Mz_L,
+	Qmi8658Register_Mz_H,
+	Qmi8658Register_firmware_id = 73,
+	Qmi8658Register_uuid = 81,
+
+	Qmi8658Register_Pedo_L = 90,
+	Qmi8658Register_Pedo_M = 91,
+	Qmi8658Register_Pedo_H = 92,
+
+	Qmi8658Register_Reset = 96
+};
+
+enum qmi8658_Ois_Register {
+	qmi8658_OIS_Reg_Ctrl1 = 0x02,
+	qmi8658_OIS_Reg_Ctrl2,
+	qmi8658_OIS_Reg_Ctrl3,
+	qmi8658_OIS_Reg_Ctrl5	= 0x06,
+	qmi8658_OIS_Reg_Ctrl7   = 0x08,
+	qmi8658_OIS_Reg_StatusInt = 0x2D,
+	qmi8658_OIS_Reg_Status0   = 0x2E,
+	qmi8658_OIS_Reg_Ax_L  = 0x33,
+	qmi8658_OIS_Reg_Ax_H,
+	qmi8658_OIS_Reg_Ay_L,
+	qmi8658_OIS_Reg_Ay_H,
+	qmi8658_OIS_Reg_Az_L,
+	qmi8658_OIS_Reg_Az_H,
+
+	qmi8658_OIS_Reg_Gx_L  = 0x3B,
+	qmi8658_OIS_Reg_Gx_H,
+	qmi8658_OIS_Reg_Gy_L,
+	qmi8658_OIS_Reg_Gy_H,
+	qmi8658_OIS_Reg_Gz_L,
+	qmi8658_OIS_Reg_Gz_H,
+};
+
+enum qmi8658_Ctrl9Command {
+	qmi8658_Ctrl9_Cmd_NOP					= 0X00,
+	qmi8658_Ctrl9_Cmd_GyroBias				= 0X01,
+	qmi8658_Ctrl9_Cmd_Rqst_Sdi_Mod			= 0X03,
+	qmi8658_Ctrl9_Cmd_Rst_Fifo				= 0X04,
+	qmi8658_Ctrl9_Cmd_Req_Fifo				= 0X05,
+	qmi8658_Ctrl9_Cmd_I2CM_Write			= 0X06,
+	qmi8658_Ctrl9_Cmd_WoM_Setting			= 0x08,
+	qmi8658_Ctrl9_Cmd_AccelHostDeltaOffset	= 0x09,
+	qmi8658_Ctrl9_Cmd_GyroHostDeltaOffset	= 0x0A,
+	qmi8658_Ctrl9_Cmd_EnableExtReset		= 0x0B,
+	qmi8658_Ctrl9_Cmd_EnableTap				= 0x0C,
+	qmi8658_Ctrl9_Cmd_EnablePedometer		= 0x0D,
+	qmi8658_Ctrl9_Cmd_Motion				= 0x0E,
+	qmi8658_Ctrl9_Cmd_CopyUsid				= 0x10,
+	qmi8658_Ctrl9_Cmd_SetRpu				= 0x11,
+	qmi8658_Ctrl9_Cmd_On_Demand_Cali		= 0xA2,
+	qmi8658_Ctrl9_Cmd_Dbg_WoM_Data_Enable	= 0xF8
+};
+
+
+enum qmi8658_LpfConfig {
+	Qmi8658Lpf_Disable,
+	Qmi8658Lpf_Enable
+};
+
+enum qmi8658_HpfConfig {
+	Qmi8658Hpf_Disable,
+	Qmi8658Hpf_Enable
+};
+
+enum qmi8658_StConfig {
+	Qmi8658St_Disable,
+	Qmi8658St_Enable
+};
+
+enum qmi8658_LpfMode {
+	A_LSP_MODE_0 = 0x00<<1,
+	A_LSP_MODE_1 = 0x01<<1,
+	A_LSP_MODE_2 = 0x02<<1,
+	A_LSP_MODE_3 = 0x03<<1,
+
+	G_LSP_MODE_0 = 0x00<<5,
+	G_LSP_MODE_1 = 0x01<<5,
+	G_LSP_MODE_2 = 0x02<<5,
+	G_LSP_MODE_3 = 0x03<<5
+};
+
+enum qmi8658_AccRange {
+	Qmi8658AccRange_2g = 0x00 << 4,
+	Qmi8658AccRange_4g = 0x01 << 4,
+	Qmi8658AccRange_8g = 0x02 << 4,
+	Qmi8658AccRange_16g = 0x03 << 4
+};
+
+
+enum qmi8658_AccOdr {
+	Qmi8658AccOdr_8000Hz = 0x00,
+	Qmi8658AccOdr_4000Hz = 0x01,
+	Qmi8658AccOdr_2000Hz = 0x02,
+	Qmi8658AccOdr_1000Hz = 0x03,
+	Qmi8658AccOdr_500Hz = 0x04,
+	Qmi8658AccOdr_250Hz = 0x05,
+	Qmi8658AccOdr_125Hz = 0x06,
+	Qmi8658AccOdr_62_5Hz = 0x07,
+	Qmi8658AccOdr_31_25Hz = 0x08,
+	Qmi8658AccOdr_LowPower_128Hz = 0x0c,
+	Qmi8658AccOdr_LowPower_21Hz = 0x0d,
+	Qmi8658AccOdr_LowPower_11Hz = 0x0e,
+	Qmi8658AccOdr_LowPower_3Hz = 0x0f
+};
+
+enum qmi8658_GyrRange {
+	Qmi8658GyrRange_16dps = 0 << 4,
+	Qmi8658GyrRange_32dps = 1 << 4,
+	Qmi8658GyrRange_64dps = 2 << 4,
+	Qmi8658GyrRange_128dps = 3 << 4,
+	Qmi8658GyrRange_256dps = 4 << 4,
+	Qmi8658GyrRange_512dps = 5 << 4,
+	Qmi8658GyrRange_1024dps = 6 << 4,
+	Qmi8658GyrRange_2048dps = 7 << 4
+};
+
+/*!
+ * \brief Gyroscope output rate configuration.
+ */
+enum qmi8658_GyrOdr {
+	Qmi8658GyrOdr_8000Hz = 0x00,
+	Qmi8658GyrOdr_4000Hz = 0x01,
+	Qmi8658GyrOdr_2000Hz = 0x02,
+	Qmi8658GyrOdr_1000Hz = 0x03,
+	Qmi8658GyrOdr_500Hz	= 0x04,
+	Qmi8658GyrOdr_250Hz	= 0x05,
+	Qmi8658GyrOdr_125Hz	= 0x06,
+	Qmi8658GyrOdr_62_5Hz 	= 0x07,
+	Qmi8658GyrOdr_31_25Hz	= 0x08
+};
+
+enum qmi8658_AccUnit {
+	Qmi8658AccUnit_g,
+	Qmi8658AccUnit_ms2
+};
+
+enum qmi8658_GyrUnit {
+	Qmi8658GyrUnit_dps,
+	Qmi8658GyrUnit_rads
+};
+
+enum qmi8658_FifoMode {
+	qmi8658_Fifo_Bypass = 0,
+	qmi8658_Fifo_Fifo = 1,
+	qmi8658_Fifo_Stream = 2,
+	qmi8658_Fifo_StreamToFifo = 3
+};
+
+
+enum qmi8658_FifoWmkLevel {
+	qmi8658_Fifo_WmkEmpty = 		(0 << 4),
+	qmi8658_Fifo_WmkOneQuarter =	(1 << 4),
+	qmi8658_Fifo_WmkHalf =			(2 << 4),
+	qmi8658_Fifo_WmkThreeQuarters =	(3 << 4)
+};
+
+enum qmi8658_FifoSize {
+	qmi8658_Fifo_16 = (0 << 2),
+	qmi8658_Fifo_32 = (1 << 2),
+	qmi8658_Fifo_64 = (2 << 2),
+	qmi8658_Fifo_128 = (3 << 2)
+};
+
+enum qmi8658_Interrupt {
+	qmi8658_Int_none,
+	qmi8658_Int1,
+	qmi8658_Int2,
+
+	qmi8658_Int_total
+};
+
+enum qmi8658_InterruptState {
+	Qmi8658State_high = (1 << 7),
+	Qmi8658State_low  = (0 << 7)
+};
+
+/***************************************************************
+    AK09918 I2C Register
+ ***************************************************************/
+#define AK09918_ADDR        0x0c    // I2C address (Can't be changed)
+#define AK09918_WIA1        0x00    // Company ID
+#define AK09918_WIA2        0x01    // Device ID
+#define AK09918_RSV1        0x02    // Reserved 1
+#define AK09918_RSV2        0x03    // Reserved 2
+#define AK09918_ST1         0x10    // DataStatus 1
+#define AK09918_HXL         0x11    // X-axis data 
+#define AK09918_HXH         0x12
+#define AK09918_HYL         0x13    // Y-axis data
+#define AK09918_HYH         0x14
+#define AK09918_HZL         0x15    // Z-axis data
+#define AK09918_HZH         0x16
+#define AK09918_TMPS        0x17    // Dummy
+#define AK09918_ST2         0x18    // Datastatus 2
+#define AK09918_CNTL1       0x30    // Dummy
+#define AK09918_CNTL2       0x31    // Control settings
+#define AK09918_CNTL3       0x32    // Control settings
+
+#define AK09918_SRST_BIT    0x01    // Soft Reset
+#define AK09918_HOFL_BIT    0x08    // Sensor Over Flow
+#define AK09918_DOR_BIT     0x02    // Data Over Run
+#define AK09918_DRDY_BIT    0x01    // Data Ready
+
+// #define AK09918_MEASURE_PERIOD 9    // Must not be changed
+// AK09918 has following seven operation modes:
+// (1) Power-down mode: AK09918 doesn't measure
+// (2) Single measurement mode: measure when you call any getData() function
+// (3) Continuous measurement mode 1: 10Hz, measure 10 times per second,
+// (4) Continuous measurement mode 2: 20Hz, measure 20 times per second,
+// (5) Continuous measurement mode 3: 50Hz, measure 50 times per second,
+// (6) Continuous measurement mode 4: 100Hz, measure 100 times per second,
+// (7) Self-test mode
+enum AK09918_mode_type_t {
+    AK09918_POWER_DOWN = 0x00,
+    AK09918_NORMAL = 0x01,
+    AK09918_CONTINUOUS_10HZ = 0x02,
+    AK09918_CONTINUOUS_20HZ = 0x04,
+    AK09918_CONTINUOUS_50HZ = 0x06,
+    AK09918_CONTINUOUS_100HZ = 0x08,
+    AK09918_SELF_TEST = 0x10, // ignored by switchMode() and initialize(), call selfTest() to use this mode
+};
+
+enum AK09918_err_type_t {
+    AK09918_ERR_OK = 0,                 // ok
+    AK09918_ERR_DOR = 1,                // data skipped
+    AK09918_ERR_NOT_RDY = 2,            // not ready
+    AK09918_ERR_TIMEOUT = 3,            // read/write timeout
+    AK09918_ERR_SELFTEST_FAILED = 4,    // self test failed
+    AK09918_ERR_OVERFLOW = 5,           // sensor overflow, means |x|+|y|+|z| >= 4912uT
+    AK09918_ERR_WRITE_FAILED = 6,       // fail to write
+    AK09918_ERR_READ_FAILED = 7,        // fail to read
+};
+
+
+
+#endif
\ No newline at end of file
diff --git a/esp/lib/IMU/i2c.cpp b/esp/lib/IMU/i2c.cpp
new file mode 100644
index 0000000..2974493
--- /dev/null
+++ b/esp/lib/IMU/i2c.cpp
@@ -0,0 +1,9 @@
+#include "I2C.h"
+
+bool I2C::lock() {
+  return xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE;
+} 
+
+void I2C::unlock() {
+	xSemaphoreGive(mutex);
+}
diff --git a/esp/lib/IMU/i2c.h b/esp/lib/IMU/i2c.h
new file mode 100644
index 0000000..a251677
--- /dev/null
+++ b/esp/lib/IMU/i2c.h
@@ -0,0 +1,18 @@
+#ifndef I2C_H
+#define I2C_H
+
+#include <Arduino.h>
+#include <Wire.h>
+
+class I2C : public TwoWire {
+  public:
+    using TwoWire::TwoWire; // prebira constructory z predka
+    bool lock();
+    void unlock();
+  private:
+    xSemaphoreHandle mutex = xSemaphoreCreateMutex();
+};
+
+extern I2C Wire2;
+
+#endif
diff --git a/esp/lib/IMU/quaternionFilters.cpp b/esp/lib/IMU/quaternionFilters.cpp
new file mode 100644
index 0000000..e066222
--- /dev/null
+++ b/esp/lib/IMU/quaternionFilters.cpp
@@ -0,0 +1,200 @@
+#include "quaternionFilters.h"
+
+const float GyroMeasError = 3.0f; // Error 3 degrees/s
+const float beta = sqrt(3.0f / 4.0f) * GyroMeasError * DEG_TO_RAD;
+
+float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
+
+void Quaternion(float q[4]) {
+  q[0] = q0;
+  q[1] = q1;
+  q[2] = q2;
+  q[3] = q3;
+}
+
+void EulerAngles(float q[4], float &roll, float &pitch, float &yaw) {
+  yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+  pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+  roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+  yaw   *= RAD_TO_DEG;
+  pitch *= RAD_TO_DEG;
+  roll  *= RAD_TO_DEG;
+}
+
+void MadgwickAHRSupdate(float g[3], float a[3], float m[3], float dT) {
+  MadgwickAHRSupdate(g[0], g[1], g[2], a[0], a[1], a[2], m[0], m[1], m[2], dT);
+}
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dT) {
+
+  gx *= DEG_TO_RAD;
+  gy *= DEG_TO_RAD;
+  gz *= DEG_TO_RAD;
+
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float hx, hy;
+  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _8bx, _8bz,  _2q0, _2q1, _2q2,
+          _2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+  if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dT);
+    return;
+  }
+
+  // Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+    // Normalise accelerometer measurement
+    recipNorm = 1.0f / (float) sqrtf(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Normalise magnetometer measurement
+    recipNorm = 1.0f / (float) sqrtf(mx * mx + my * my + mz * mz);
+    mx *= recipNorm;
+    my *= recipNorm;
+    mz *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    _2q0mx = 2.0f * q0 * mx;
+    _2q0my = 2.0f * q0 * my;
+    _2q0mz = 2.0f * q0 * mz;
+    _2q1mx = 2.0f * q1 * mx;
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+    q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;
+
+    // Reference direction of Earth's magnetic field
+    hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+    hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+    _2bx = (float) sqrtf(hx * hx + hy * hy);
+    _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+    _4bx = 2.0f * _2bx;
+    _4bz = 2.0f * _2bz;
+    _8bz = 2.0f * _4bz;
+    _8bx = 2.0f * _2bx;
+
+    // Gradient decent algorithm corrective step
+    s0 = -_2q2 * (2 * (q1q3 - q0q2) - ax) + _2q1 * (2 * (q0q1 + q2q3) - ay) -_4bz * q2 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (-_4bx * q3 + _4bz * q1) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + _4bx * q2 * (_4bx * (q0q2 + q1q3) + _4bz * (0.5f - q1q1 - q2q2) - mz);
+    s1 = _2q3 * (2 * (q1q3 - q0q2) - ax) + _2q0 * (2 * (q0q1 + q2q3) - ay) -4 * q1 * (2 * (0.5f - q1q1 - q2q2) - az) + _4bz * q3 * (_4bx * (0.5f - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (_4bx * q2 + _4bz * q0) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + (_4bx * q3 - _8bz * q1) * (_4bx * (q0q2 + q1q3) + _4bz * (0.5f - q1q1 - q2q2) - mz);
+    s2 = -_2q0*(2*(q1q3 - q0q2) - ax)    +     _2q3*(2*(q0q1 + q2q3) - ay)   +   (-4*q2)*(2*(0.5f - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5f - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5f - q1q1 - q2q2) - mz);
+    s3 = _2q1*(2*(q1q3 - q0q2) - ax) +   _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5f - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5f - q1q1 - q2q2) - mz);
+    recipNorm = 1.0f / (float) sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * dT;
+  q1 += qDot2 * dT;
+  q2 += qDot3 * dT;
+  q3 += qDot4 * dT;
+
+  // Normalise quaternion
+  recipNorm = 1.0f / (float) sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dT) {
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+  // Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+    // Normalise accelerometer measurement
+    recipNorm = 1.0f / (float) sqrtf(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    _4q0 = 4.0f * q0;
+    _4q1 = 4.0f * q1;
+    _4q2 = 4.0f * q2;
+    _8q1 = 8.0f * q1;
+    _8q2 = 8.0f * q2;
+    q0q0 = q0 * q0;
+    q1q1 = q1 * q1;
+    q2q2 = q2 * q2;
+    q3q3 = q3 * q3;
+
+    // Gradient decent algorithm corrective step
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+    recipNorm = 1.0f / (float) sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * dT;
+  q1 += qDot2 * dT;
+  q2 += qDot3 * dT;
+  q3 += qDot4 * dT;
+
+  // Normalise quaternion
+  recipNorm = 1.0f / (float) sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+//  recipNorm = 1.0f;
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+}
\ No newline at end of file
diff --git a/esp/lib/IMU/quaternionFilters.h b/esp/lib/IMU/quaternionFilters.h
new file mode 100644
index 0000000..f41b7b6
--- /dev/null
+++ b/esp/lib/IMU/quaternionFilters.h
@@ -0,0 +1,12 @@
+#ifndef QUATERNIONFILTERS_H_
+#define QUATERNIONFILTERS_H_
+
+#include <Arduino.h>
+
+void Quaternion(float q[4]);
+void EulerAngles(float q[4], float &roll, float &pitch, float &yaw);
+void MadgwickAHRSupdate(float g[3], float a[3], float m[3], float dT);
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dT);
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dT);
+
+#endif // _QUATERNIONFILTERS_H_
diff --git a/esp/lib/NeoPixel/ws2812.cpp b/esp/lib/NeoPixel/ws2812.cpp
new file mode 100644
index 0000000..b488861
--- /dev/null
+++ b/esp/lib/NeoPixel/ws2812.cpp
@@ -0,0 +1,63 @@
+#include "ws2812.h"
+
+Neopixel::Neopixel() {
+  init();
+}
+
+esp_err_t Neopixel::init(void) {
+  rmt_config_t config;
+  config.rmt_mode = RMT_MODE_TX;
+  config.channel = LED_RMT_TX_CHANNEL;
+  config.gpio_num = LED_RMT_TX_GPIO;
+  config.mem_block_num = 3;
+  config.tx_config.loop_en = false;
+  config.tx_config.carrier_en = false;
+  config.tx_config.idle_output_en = true;
+  config.tx_config.idle_level = (rmt_idle_level_t)0;
+  config.clk_div = 2;
+
+  ESP_RETURN_ON_ERROR(rmt_config(&config), TAG, "Failed to configure RMT");
+  ESP_RETURN_ON_ERROR(rmt_driver_install(config.channel, 0, 0), TAG, "Failed to install RMT driver");
+
+  return ESP_OK;
+}
+
+esp_err_t Neopixel::write() {
+  setup_rmt_data_buffer(leds);
+  ESP_RETURN_ON_ERROR(rmt_write_items(LED_RMT_TX_CHANNEL, led_data_buffer, LED_BUFFER_ITEMS+1, false), TAG, "Failed to write items");
+//  Cekani na ukonceni zapisu, funguje i kdyz je nepouzito 118 -> 16 us
+//  ESP_RETURN_ON_ERROR(rmt_wait_tx_done(LED_RMT_TX_CHANNEL, portMAX_DELAY), TAG, "Failed to wait for RMT transmission to finish");
+
+  return ESP_OK;
+}
+
+void Neopixel::setup_rmt_data_buffer(uint32_t* leds) {
+  for (uint32_t led = 0; led < NUM_LEDS; led++) {
+    uint32_t bits_to_send = leds[led];
+    uint32_t mask = 1 << (BITS_PER_LED_CMD - 1);
+
+    for (uint32_t bit = 0; bit < BITS_PER_LED_CMD; bit++) {
+      uint32_t bit_is_set = bits_to_send & mask;
+      led_data_buffer[(led * BITS_PER_LED_CMD) + bit] = bit_is_set ? (rmt_item32_t){{{T1H, 1, T1L, 0}}} : (rmt_item32_t){{{T0H, 1, T0L, 0}}};
+      mask >>= 1;
+    }
+  }
+  led_data_buffer[LED_BUFFER_ITEMS] = (rmt_item32_t){{{TE, 0, 0, 0}}};
+}
+
+void Neopixel::clear() {
+  for (uint32_t i = 0; i < NUM_LEDS; i++) {
+    leds[i] = 0;
+  }
+  write();
+}
+
+void Neopixel::led(uint8_t index, uint32_t brg) {
+  if (index < NUM_LEDS) {
+    uint32_t rgb = ((brg & 0x00FF0000) >> 16) | ((brg & 0x0000FFFF) << 8);
+    if (leds[index] != rgb) {
+      leds[index] = rgb;
+      write();
+    }  
+  }
+}
\ No newline at end of file
diff --git a/esp/lib/NeoPixel/ws2812.h b/esp/lib/NeoPixel/ws2812.h
new file mode 100644
index 0000000..d080954
--- /dev/null
+++ b/esp/lib/NeoPixel/ws2812.h
@@ -0,0 +1,91 @@
+#ifndef WS2812_H
+#define WS2812_H
+
+#include <stdint.h>
+#include "sdkconfig.h"
+#include "esp_err.h"
+#include "driver/rmt.h"
+#include "esp_check.h"
+
+/* ===============================
+WS2811: (2.5us bit time, 400Kbps)
+    T0H: 0.5us <-- 0 bit
+    T0L: 2.0us
+    T1H: 1.2us <-- 1 bit
+    T1L: 1.3us
+    RES: 50us
+
+WS2812: (1.25us bit time, 800Kbps)
+    T0H: 0.35us <-- 0 bit
+    T0L: 0.8us
+    T1H: 0.7us <-- 1 bit
+    T1L: 0.6us
+    RES: 50us
+
+WS2812b: (1.25us bit time, 800Kbps)
+    T0H: 0.4us <-- 0 bit
+    T0L: 0.85us
+    T1H: 0.8us <-- 1 bit
+    T1L: 0.45us
+    RES: 50us
+
+To calculate values for these configuration times, 
+multiply the desired time in micro seconds (μs or us in data sheets) 
+by the default clock rate in MHz and divide by 2. E.g., for 280ns (0.28μs) 
+on an ESP32 with an 80MHz clock: 0.28 * 80 / 2 = 11.2 => 11 (rounded when needed)
+=================================== */
+
+#define NUM_LEDS	2
+
+// Configure these based on your project needs using menuconfig ********
+#define LED_RMT_TX_CHANNEL	(rmt_channel_t)0
+#define LED_RMT_TX_GPIO		(gpio_num_t)4
+// ****************************************************
+
+#define BITS_PER_LED_CMD	24
+#define LED_BUFFER_ITEMS	(NUM_LEDS * BITS_PER_LED_CMD)
+
+// These values are determined by measuring pulse timing with logic analyzer and adjusting to match datasheet. 
+#define T0H	20  // 0 bit high time WS2811
+#define T0L	80  // low time for either bit
+#define T1H	48  // 1 bit high time
+#define T1L	52
+#define TE  2000
+
+enum COLOR { // BRG
+  RED   = 0x00003F,
+  GREEN = 0x003F00,
+  BLUE  = 0x3F0000
+};
+
+class Neopixel {
+  public:
+    Neopixel();
+    void  led(uint8_t index, uint32_t rgb);
+    void  clear();
+
+  private:
+    // This structure is used for indicating what the colors of each LED should be set to.
+    // There is a 32bit value for each LED. Only the lower 3 bytes are used and they hold the
+    // Red (byte 2), Green (byte 1), and Blue (byte 0) values to be set.
+    uint32_t leds[NUM_LEDS];
+
+    // Tag for log messages
+    static constexpr const char *TAG = "NeoPixel WS2812 Driver";
+
+    // This is the buffer which the hw peripheral will access while pulsing the output pin
+    rmt_item32_t led_data_buffer[LED_BUFFER_ITEMS+1];
+
+    // Setup the hardware peripheral. Only call this once.
+    esp_err_t init(void);
+
+    // Update the LEDs to the new state. Call as needed.
+    // This function will block the current task until the RMT peripheral is finished sending 
+    // the entire sequence.
+    esp_err_t write();
+
+    void setup_rmt_data_buffer(uint32_t* leds);
+};
+
+#endif
+
diff --git a/esp/lib/Servo/servo.cpp b/esp/lib/Servo/servo.cpp
new file mode 100644
index 0000000..645ace9
--- /dev/null
+++ b/esp/lib/Servo/servo.cpp
@@ -0,0 +1,25 @@
+#include "servo.h"
+
+Servo::Servo(uint8_t channel) {
+  this->channel = channel;
+  ledcSetup(channel, PWM_FREQ, PWM_RESOLUTION);    // nastaveni PWM vystupu
+//  write(1500);
+}
+
+void Servo::attach(uint8_t pin) {
+  this->pin = pin;
+  ledcAttachPin(pin, channel);  
+}
+
+void Servo::detach() {
+  ledcDetachPin(pin);
+}
+
+void Servo::write(int pos) {
+  pos = 65536 * pos / 20000;
+  ledcWrite(channel, pos);    // nastaveni vystupni hodnoty na vystup
+}
+
+Servo::~Servo() {
+  detach();
+}
diff --git a/esp/lib/Servo/servo.h b/esp/lib/Servo/servo.h
new file mode 100644
index 0000000..6f2ff53
--- /dev/null
+++ b/esp/lib/Servo/servo.h
@@ -0,0 +1,22 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include <Arduino.h>
+
+#define PWM_CHANNEL      1
+#define PWM_FREQ        50
+#define PWM_RESOLUTION  16
+
+class Servo {
+  public:
+    Servo(uint8_t channel = PWM_CHANNEL);
+    void attach(uint8_t pin);
+    void detach();
+    void write(int pos);
+    ~Servo();
+  private:
+    uint8_t channel;
+    uint8_t pin;
+};
+
+#endif
\ No newline at end of file
diff --git a/esp/matty-v4.txt b/esp/matty-v8.txt
similarity index 69%
rename from esp/matty-v4.txt
rename to esp/matty-v8.txt
index 08c0557..67c1dda 100644
--- a/esp/matty-v4.txt
+++ b/esp/matty-v8.txt
@@ -7,7 +7,7 @@ SYNC  0x55
 ESC   0x56 (volitelné escapování znaků SYNC a ESC)
 CRC = 256 - (LENGTH + DATA0 + DATA1 + ... DATAn)
 
-Komunikační protokol Matty v2:
+Komunikační protokol Matty v8:
 
 PC -> ESP
 
@@ -21,38 +21,51 @@ counter command   data
           T       uint16 period (ms)    uint16 timeout (ms)       ... nastavení periody posílání dat z robota a timeout pro příjem povelů
           P       uint8 mode                                      ... nastavení režimu gps (0 - vypnuto, 1 - surová data, 2 - ...)
           V                                                       ... version
-  
+          C       uint16_t                                        ... analogové servo na pinu GPIO4 (šířka pulzu 1000 ... 2000 us)
+          X       uint16_t index, uint16_t position               ... digitální ST servo na sběrnici (adresa 4 ... ), pozice (0..4095)
+          D       uint8_t index, uint8_t r, uint8_t g, uint8_t b  ... neopixel led (WS2811) na pinu GPIO4 
+            
 ESP -> PC
 
 potvrzení přijatého příkazu DATA:  
-
-counter potvrzení
- uint8    A/N
+  counter     uint8 
+  ack/nack    A/N
   
 stavová zpráva (odesíláná pravidelně dle nastavené periody) DATA:
-
-counter message  status  mode  voltage(mV)  current(mA)  speed(mm/s) angle(0,01°) encoder0(mm)  encoder1(mm)  encoder2(mm)  encoder3(mm)
- uint8    I       uint8  uint8  uint16        uint16       int16         int16      uint16        uint16        uint16        uint16
-
-status (příznakové bity): 
-  EMERGENCY_STOP  = 0x01
-  VOLTAGE_LOW     = 0x02
-  BUMPER_FRONT    = 0x04
-  BUMPER_BACK     = 0x08
-  ERROR_ENCODER   = 0x10
-  ERROR_POWER     = 0x20
-  RUNNING         = 0x80
+  counter             uint8 
+  message               I
+  status              uint8
+  mode                uint8
+  voltage(mV)         uint16
+  current(mA)         uint16
+  speed(mm/s)         int16
+  joint angle(0,01°)  int16
+  encoder0(mm)        uint16
+  encoder1(mm)        uint16
+  encoder2(mm)        uint16
+  encoder3(mm)        uint16
+  roll(0,01°)         int16_t
+  pitch(0,01°)        int16_t
+  yaw(0,01°)          int16_t
+
+  status (příznakové bity): 
+    EMERGENCY_STOP  = 0x01
+    VOLTAGE_LOW     = 0x02
+    BUMPER_FRONT    = 0x04
+    BUMPER_BACK     = 0x08
+    ERROR_ENCODER   = 0x10
+    ERROR_POWER     = 0x20
+    RUNNING         = 0x80
 
 gps zpráva (odesílána ihned po přijetí z gps):
-
-counter message  data 
- uint8    P      nmea ($  ... CRLF)
-
+  counter   uint8 
+  message   P
+  data      nmea ($  ... CRLF)
 
 version zpráva
-
-counter message  data
- uint8    V      uint8 (serial number), uint8 (version)
+  counter   uint8
+  message   uint8
+  data      uint8 (serial number), uint8 (version)
 
 =================================================================  
 bitova negace pro escape

From c67131a61fdd6b29986336f28835a98479cbcef3 Mon Sep 17 00:00:00 2001
From: Martin Locker <martin.locker@seznam.cz>
Date: Fri, 8 Aug 2025 13:10:33 +0200
Subject: [PATCH 2/7] Update wrong files

---
 esp/Robot/src/robot.cpp | 30 ++++++++++++++
 esp/Robot/src/robot.h   | 15 +++++++
 esp/common/config.h     | 88 ++++++++++++++++++++++++++++++++++++-----
 3 files changed, 124 insertions(+), 9 deletions(-)

diff --git a/esp/Robot/src/robot.cpp b/esp/Robot/src/robot.cpp
index 3e512ca..959dc30 100644
--- a/esp/Robot/src/robot.cpp
+++ b/esp/Robot/src/robot.cpp
@@ -4,6 +4,13 @@ uint8_t idServo[SERVOS] = {1, 2, 3, 4};
 
 AS5600 as5600(ZERO_JOINT);  // zero joint position
 Power power;
+IMU   imu;
+#ifdef SERVO_PIN
+  Servo servo;
+#endif
+#ifdef LED_PIN
+  Neopixel neopixel;
+#endif  
 
 Robot::Robot(Stream* stSerial) : Sts(stSerial) {
 }
@@ -13,6 +20,7 @@ void Robot::init() {
   power.init();
   pinMode(BUMPER_FRONT_PIN, INPUT);
   pinMode(BUMPER_BACK_PIN,  INPUT);
+  imu.init();
   stop();
   process();
   reset();
@@ -164,6 +172,7 @@ bool Robot::process() {
     updateEncoder(p);
     updateOdometry(t - timeScan);
     updateSystem();
+    updateImu();
     timeScan = t;
     return 1;
   }
@@ -181,6 +190,10 @@ void Robot::updatePower() {
   voltage = power.getBusVoltage_V() * 1000.0f;
 }
 
+void Robot::updateImu() {
+  imu.eulerAngles(roll, pitch, yaw);
+}
+
 void Robot::setTime(uint16_t period, uint16_t t) { // nastavi periodu pro cteni odomerie a odesilani dat, timeout pro automaticke zastaveni, pokud neprijde novy povel G
   scanPeriod = period;
   robotTimeout = t;
@@ -190,3 +203,20 @@ void Robot::setLimits(uint16_t maxSpeed, uint16_t maxAngleSpeed) { // nastavi ma
   if (maxSpeed <= SPEED_MAX) v_max = maxSpeed;
   if (maxAngleSpeed <= STEER_MAX) a_max = maxAngleSpeed;
 }
+
+#ifdef SERVO_PIN
+void Robot::setServo(uint16_t position) {
+  if (position > 0) {
+    servo.attach(SERVO_PIN);
+    servo.write(position);
+  } else {
+    servo.detach();
+  }
+}
+#endif
+
+#ifdef LED_PIN
+void Robot::led(uint8_t index, uint32_t rgb) {
+  neopixel.led(index, rgb);
+}
+#endif
diff --git a/esp/Robot/src/robot.h b/esp/Robot/src/robot.h
index 9676bd2..7f51940 100644
--- a/esp/Robot/src/robot.h
+++ b/esp/Robot/src/robot.h
@@ -6,6 +6,13 @@
 #include "as5600.h"
 #include "power.h"
 #include "sts.h"
+#include "imu.h"  
+#ifdef SERVO_PIN
+  #include "servo.h"  
+#endif
+#ifdef LED_PIN
+  #include "ws2812.h"  
+#endif
 
 #define STOP            0
 #define REMOTE_CONTROL  1
@@ -33,6 +40,12 @@ class Robot : public Sts {
     void  setTime(uint16_t p, uint16_t t); // nastavi periodu pro odomerii a odesilani dat, timeout pro automaticke zastaveni, pokud neprijde novy povel G
     void  setLimits(uint16_t maxSpeed, uint16_t maxAngleSpeed); // nastavi maximalni rychlosti
     void  updateSystem();
+    #ifdef SERVO_PIN
+      void  setServo(uint16_t position);
+    #endif
+    #ifdef LED_PIN
+      void  led(uint8_t index, uint32_t rgb);
+    #endif  
 
     float     x, y, a;  // aktualni pozice robota [mm, mm, rad]
     uint8_t   status;
@@ -42,6 +55,7 @@ class Robot : public Sts {
     float     joint;            // uhel natoceni kloubu ve stupnich
     float     actualSpeed;      // rychlost pocitana z odometrie v mm/s
     int32_t   encoder[SERVOS];
+    float     roll, pitch, yaw;
     uint32_t  scanPeriod = SCAN_PERIOD;
   private:
     float speed;  // pozadovana rychlost
@@ -56,6 +70,7 @@ class Robot : public Sts {
     void  updateEncoder(uint16_t p[SERVOS]);
     void  updateOdometry(uint32_t t);
     void  updatePower();
+    void  updateImu();
     void  control();  // vypocet kinematiky a rizeni pohonu
 };
 
diff --git a/esp/common/config.h b/esp/common/config.h
index 4faa7eb..a66b729 100644
--- a/esp/common/config.h
+++ b/esp/common/config.h
@@ -10,7 +10,7 @@
 //    ROBOT
 // ========================================================================
 
-#define ROBOT_FW_VERSION  6 // please increase with every change of Robot project (or common library)
+#define ROBOT_FW_VERSION  8 // please increase with every change of Robot project (or common library)
 
 #define ROBOT_TIMEOUT   250 // timeout do automatickeho zastaveni, pokud neprijde povel G
 #define CONTROL_PERIOD   20 // perioda rizeni  20 ms (50 Hz)
@@ -22,27 +22,85 @@
 #ifndef MATTY
   #error "Not defined MATTY platform"
 #else
-  #if (MATTY == 01)
+
+  #if (MATTY == 00) // testovaci board ESP32
+    #define ZERO_JOINT 180.00f  // nulova poloha kloubu
+    #define L              320  // rozvor
+    #define A              315  // rozchod
+    #define D              135  // prumer kol
+
+    // IMU kalibrace
+    #define ACC_BIAS    {-0.60, -58.37, -13.21}
+    #define GYRO_BIAS   {-2.15, 0.25, -0.16}
+    #define MAG_BIAS    {3.99, 9.87, 6.42}
+    #define MAG_CORECT  {{0.999, 0.008, 0.002}, {0.008, 1.027, 0.009}, {0.002, 0.009, 0.974}}
+  #endif
+
+  #if (MATTY == 01) // Matty M01 - modry
     #define ZERO_JOINT 178.59f  // nulova poloha kloubu Matty M01
     #define L              320  // rozvor
     #define A              311  // rozchod
     #define D              135  // prumer kol
 
+    // IMU kalibrace
+    #define ACC_BIAS    {115.55, -48.88,   8.52}
+    #define GYRO_BIAS   {-2.23,   1.45,  -0.30}
+    #define MAG_BIAS    {22.27, -67.19, 1.94}
+    #define MAG_CORECT  {{1.050, -0.006, -0.022}, {-0.006, 1.016, 0.019}, {-0.022, 0.019, 0.939}}
+
     #define USE_ESP_NOW
   #endif
   
-  #if (MATTY == 02)
-    #define ZERO_JOINT -178.15f  // nulova poloha kloubu Matty M02 - Martin Dlouhy
+  #if (MATTY == 02) // Matty M02 - zeleny
+    #define ZERO_JOINT -178.15f // nulova poloha kloubu Matty M02 - Martin Dlouhy
     #define L              320  // rozvor
     #define A              315  // rozchod
     #define D              135  // prumer kol
+
+    // IMU kalibrace
+    #define ACC_BIAS    {0, 0, 0}
+    #define GYRO_BIAS   {0, 0, 0}
+    #define MAG_BIAS    {0, 0, 0}
+    #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
   #endif
 
-  #if (MATTY == 03)
+  #if (MATTY == 03) // Matty M03 - cerveny
     #define ZERO_JOINT   0.00f  // nulova poloha kloubu Matty M03 - Martin Dlouhy
     #define L              320  // rozvor
     #define A              315  // rozchod
     #define D              135  // prumer kol
+
+    // IMU kalibrace
+    #define ACC_BIAS    {0, 0, 0}
+    #define GYRO_BIAS   {0, 0, 0}
+    #define MAG_BIAS    {0, 0, 0}
+    #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
+  #endif
+
+  #if (MATTY == 04) // Matty M04 - ruzovy
+    #define ZERO_JOINT   0.00f  // nulova poloha kloubu Matty M04 - Martin Dlouhy
+    #define L              320  // rozvor
+    #define A              315  // rozchod
+    #define D              135  // prumer kol
+
+    // IMU kalibrace
+    #define ACC_BIAS    {0, 0, 0}
+    #define GYRO_BIAS   {0, 0, 0}
+    #define MAG_BIAS    {0, 0, 0}
+    #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
+  #endif
+
+  #if (MATTY == 05) // Matty M05 - oranzovy
+    #define ZERO_JOINT   0.00f  // nulova poloha kloubu Matty M05 - Martin Dlouhy
+    #define L              320  // rozvor
+    #define A              315  // rozchod
+    #define D              135  // prumer kol
+
+    // IMU kalibrace
+    #define ACC_BIAS    {0, 0, 0}
+    #define GYRO_BIAS   {0, 0, 0}
+    #define MAG_BIAS    {0, 0, 0}
+    #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
   #endif
 #endif
 
@@ -56,10 +114,10 @@
 #define LINE        (1 / STEP)            // steps/mm
 
 // ========================================================================
-//    SERVO
+//    SERVO ST
 // ========================================================================
 
-// the uart used to control servos.
+// the uart used to control servos
 #define SerialServo     Serial1  /* UART1 */
 #define S_RXD           18
 #define S_TXD           19
@@ -78,8 +136,20 @@
 // ========================================================================
 
 #define SerialGPS     Serial2  /* UART2 */
-#define GPS_RXD       16       // zelena / GPS16 / B_C2
-#define GPS_TXD       27       // modra  / GPS17 / B_C1 
+#define GPS_RXD       16       // zelena / GPIO16 / B_C2
+#define GPS_TXD       27       // modra  / GPIO17 / B_C1 
 #define GPS_BAUDRATE  9600L 
 
+// ========================================================================
+//    SERVO
+// ========================================================================
+
+//#define SERVO_PIN     4        // servo GPIO4
+
+// ========================================================================
+//    NEOPIXEL LED
+// ========================================================================
+
+#define LED_PIN     4        // servo GPIO4
+
 #endif

From edabc69b554909df08db252e2fbf49bc12910818 Mon Sep 17 00:00:00 2001
From: Martin Dlouhy <md@robotika.cz>
Date: Fri, 8 Aug 2025 13:35:58 +0200
Subject: [PATCH 3/7] FIXUP linux imports

---
 esp/Robot/src/robot.h | 2 +-
 esp/lib/IMU/i2c.cpp   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/esp/Robot/src/robot.h b/esp/Robot/src/robot.h
index 7f51940..f59eee0 100644
--- a/esp/Robot/src/robot.h
+++ b/esp/Robot/src/robot.h
@@ -6,7 +6,7 @@
 #include "as5600.h"
 #include "power.h"
 #include "sts.h"
-#include "imu.h"  
+#include "IMU.h"
 #ifdef SERVO_PIN
   #include "servo.h"  
 #endif
diff --git a/esp/lib/IMU/i2c.cpp b/esp/lib/IMU/i2c.cpp
index 2974493..f303d98 100644
--- a/esp/lib/IMU/i2c.cpp
+++ b/esp/lib/IMU/i2c.cpp
@@ -1,4 +1,4 @@
-#include "I2C.h"
+#include "i2c.h"
 
 bool I2C::lock() {
   return xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE;

From 7ddb4ccef75d0a5640fe83153aa2d30b1a5b61e1 Mon Sep 17 00:00:00 2001
From: Martin Locker <martin.locker@seznam.cz>
Date: Fri, 8 Aug 2025 20:24:28 +0200
Subject: [PATCH 4/7] Add fw for imu calibration

---
 esp/Imu/.gitignore              |  5 +++
 esp/Imu/.travis.yml             | 67 +++++++++++++++++++++++++++++++++
 esp/Imu/.vscode/extensions.json | 10 +++++
 esp/Imu/README.md               | 22 +++++++++++
 esp/Imu/data/imu.dat            |  4 ++
 esp/Imu/include/README          | 39 +++++++++++++++++++
 esp/Imu/platformio.ini          | 18 +++++++++
 esp/Imu/src/main.cpp            | 47 +++++++++++++++++++++++
 esp/Imu/test/README             | 11 ++++++
 esp/lib/IMU/IMU.cpp             |  6 +--
 esp/lib/IMU/IMU.h               |  4 +-
 11 files changed, 227 insertions(+), 6 deletions(-)
 create mode 100644 esp/Imu/.gitignore
 create mode 100644 esp/Imu/.travis.yml
 create mode 100644 esp/Imu/.vscode/extensions.json
 create mode 100644 esp/Imu/README.md
 create mode 100644 esp/Imu/data/imu.dat
 create mode 100644 esp/Imu/include/README
 create mode 100644 esp/Imu/platformio.ini
 create mode 100644 esp/Imu/src/main.cpp
 create mode 100644 esp/Imu/test/README

diff --git a/esp/Imu/.gitignore b/esp/Imu/.gitignore
new file mode 100644
index 0000000..89cc49c
--- /dev/null
+++ b/esp/Imu/.gitignore
@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
diff --git a/esp/Imu/.travis.yml b/esp/Imu/.travis.yml
new file mode 100644
index 0000000..7c486f1
--- /dev/null
+++ b/esp/Imu/.travis.yml
@@ -0,0 +1,67 @@
+# Continuous Integration (CI) is the practice, in software
+# engineering, of merging all developer working copies with a shared mainline
+# several times a day < https://docs.platformio.org/page/ci/index.html >
+#
+# Documentation:
+#
+# * Travis CI Embedded Builds with PlatformIO
+#   < https://docs.travis-ci.com/user/integration/platformio/ >
+#
+# * PlatformIO integration with Travis CI
+#   < https://docs.platformio.org/page/ci/travis.html >
+#
+# * User Guide for `platformio ci` command
+#   < https://docs.platformio.org/page/userguide/cmd_ci.html >
+#
+#
+# Please choose one of the following templates (proposed below) and uncomment
+# it (remove "# " before each line) or use own configuration according to the
+# Travis CI documentation (see above).
+#
+
+
+#
+# Template #1: General project. Test it using existing `platformio.ini`.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio run
+
+
+#
+# Template #2: The project is intended to be used as a library with examples.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# env:
+#     - PLATFORMIO_CI_SRC=path/to/test/file.c
+#     - PLATFORMIO_CI_SRC=examples/file.ino
+#     - PLATFORMIO_CI_SRC=path/to/test/directory
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N
diff --git a/esp/Imu/.vscode/extensions.json b/esp/Imu/.vscode/extensions.json
new file mode 100644
index 0000000..080e70d
--- /dev/null
+++ b/esp/Imu/.vscode/extensions.json
@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}
diff --git a/esp/Imu/README.md b/esp/Imu/README.md
new file mode 100644
index 0000000..e3e5d6b
--- /dev/null
+++ b/esp/Imu/README.md
@@ -0,0 +1,22 @@
+Postup kalibrace imu esp32
+
+1. Zkompilovat a nahrát testovací fw do esp32 (#define CALIBRATE)
+2. Přípojit k terminálu (115200 bd)
+3. Postavit na vodorovnou podložku a nepohybovat deskou (robotem)
+4. Resetovat esp (druhé tlačítko od usb konektoru)
+5. Cca po 3 sekundách se objeví kalibrační hodnoty akcelerometru a gyra
+6. Zapsat do config.h (pro daného robota)
+   např.:
+    #define ACC_BIAS    {-0.60, -58.37, -13.21}
+    #define GYRO_BIAS   {-2.15, 0.25, -0.16}
+7. Spustit aplikaci MotionCal
+    https://learn.adafruit.com/adafruit-sensorlab-magnetometer-calibration/magnetic-calibration-with-motioncal
+8. Připojit COMx a Clear
+9. Otáčet robotem tak, aby se pokryla celá koule (viz. popis výše)
+10. Zapsat do config.h
+    např.:
+    #define MAG_BIAS    {3.99, 9.87, 6.42}
+    #define MAG_CORECT  {{0.999, 0.008, 0.002}, {0.008, 1.027, 0.009}, {0.002, 0.009, 0.974}}
+11. Zkompilovat a nahrát testovací fw do esp32 bez kalibrace(// #define CALIBRATE)
+12. Připojit k terminálu a otestovat
+     
\ No newline at end of file
diff --git a/esp/Imu/data/imu.dat b/esp/Imu/data/imu.dat
new file mode 100644
index 0000000..32f1d85
--- /dev/null
+++ b/esp/Imu/data/imu.dat
@@ -0,0 +1,4 @@
+-0.60, -58.37, -13.21
+-2.15, 0.25, -0.16
+3.99, 9.87, 6.42
+0.999, 0.008, 0.002, 0.008, 1.027, 0.009, 0.002, 0.009, 0.974
diff --git a/esp/Imu/include/README b/esp/Imu/include/README
new file mode 100644
index 0000000..194dcd4
--- /dev/null
+++ b/esp/Imu/include/README
@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
diff --git a/esp/Imu/platformio.ini b/esp/Imu/platformio.ini
new file mode 100644
index 0000000..b13a919
--- /dev/null
+++ b/esp/Imu/platformio.ini
@@ -0,0 +1,18 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32doit-devkit-v1]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+lib_extra_dirs = ../lib
+build_flags = -I../common
+; upload_port = COM8
+monitor_speed = 115200
diff --git a/esp/Imu/src/main.cpp b/esp/Imu/src/main.cpp
new file mode 100644
index 0000000..c114bcf
--- /dev/null
+++ b/esp/Imu/src/main.cpp
@@ -0,0 +1,47 @@
+#include <Wire.h>
+#include "IMU.h"
+
+#define S_SCL   33
+#define S_SDA   32
+
+#define CALIBRATION
+
+IMU imu;
+
+void setup() { 
+  Wire2.begin(S_SDA, S_SCL, 400000);
+  Serial.begin(115200);
+  delay(1000);
+
+  #ifdef CALIBRATION
+    imu.init(1);
+    imu.autoCalibrate();
+  #else
+    imu.init();
+  #endif  
+  Serial.println("/-------------------------------------------------------------/");
+}
+
+#define PERIOD  100
+uint32_t timeStamp;
+
+float roll, pitch, yaw;
+
+uint32_t lastTime = 0;
+
+void loop() {
+  if (millis() - timeStamp >= PERIOD) {
+    imu.eulerAngles(roll, pitch, yaw);
+    timeStamp = millis();
+#ifndef CALIBRATION
+    Serial.printf("Roll: %6.2f Pitch: %6.2f Yaw: %6.2f Mag: %6.2f\n\r", roll, pitch, yaw, imu.mg);
+#else
+//  Data for magnetometer calibration by MotionCal
+//  "Raw:0,0,0,0,0,0,mx,my,mz\n\r" mx, my, mz 0.1uT
+    uint8_t x = imu.readData();
+    if (x) {
+      Serial.printf("Raw:0,0,0,0,0,0,%d,%d,%d\n\r", (int)(imu.m[0]*10), (int)(imu.m[1]*10), (int)(imu.m[2]*10));
+    }
+#endif
+  }
+}
diff --git a/esp/Imu/test/README b/esp/Imu/test/README
new file mode 100644
index 0000000..df5066e
--- /dev/null
+++ b/esp/Imu/test/README
@@ -0,0 +1,11 @@
+
+This directory is intended for PIO Unit Testing and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PIO Unit Testing:
+- https://docs.platformio.org/page/plus/unit-testing.html
diff --git a/esp/lib/IMU/IMU.cpp b/esp/lib/IMU/IMU.cpp
index 783e8dc..0fe595a 100644
--- a/esp/lib/IMU/IMU.cpp
+++ b/esp/lib/IMU/IMU.cpp
@@ -6,10 +6,10 @@ void IMU::imuTask(void *pvParameters) {
 	static_cast<IMU*>(pvParameters)->update();
 }
 
-void IMU::init() {
+void IMU::init(bool noTask) {
 	if (getId() == 0x05) {
     configSensors();
-		xTaskCreate(&IMU::imuTask, "Task IMU", 2048, (void*)this, 1, NULL);
+		if (!noTask) xTaskCreate(&IMU::imuTask, "Task IMU", 2048, (void*)this, 1, NULL);
   }
 }
 
@@ -91,7 +91,7 @@ void IMU::qmi8658_on_demand_cali(void) {
 }
 
 void IMU::autoCalibrate() {
-	#define SAMPLES	50
+	#define SAMPLES	500
   Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
   delay(1000);
 
diff --git a/esp/lib/IMU/IMU.h b/esp/lib/IMU/IMU.h
index 70a9cd2..16c0976 100644
--- a/esp/lib/IMU/IMU.h
+++ b/esp/lib/IMU/IMU.h
@@ -7,11 +7,9 @@
 #include "i2c.h"
 #include "quaternionFilters.h"
 
-#define CALIBRATION
-
 class IMU {
   public:
-    void init();
+    void init(bool noTask = 0);
     void update();  // cteni a vypocet imu pro task
     static void imuTask(void* pvParameters);
     void eulerAngles(float &roll, float &pitch, float &yaw);

From d6317895cdc67f538f54c138f2d574f60505a064 Mon Sep 17 00:00:00 2001
From: Martin Dlouhy <md@robotika.cz>
Date: Sun, 10 Aug 2025 13:33:31 +0200
Subject: [PATCH 5/7] esp/common/config.h - first attempt to calibrate M02

---
 esp/common/config.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/esp/common/config.h b/esp/common/config.h
index a66b729..51cfff7 100644
--- a/esp/common/config.h
+++ b/esp/common/config.h
@@ -58,9 +58,9 @@
     #define D              135  // prumer kol
 
     // IMU kalibrace
-    #define ACC_BIAS    {0, 0, 0}
-    #define GYRO_BIAS   {0, 0, 0}
-    #define MAG_BIAS    {0, 0, 0}
+    #define ACC_BIAS    {87.54, 5.79, 21.60}
+    #define GYRO_BIAS   {-2.40, 0.77, -0.43}
+    #define MAG_BIAS    {-1.10, 32.20, -18.15}
     #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
   #endif
 

From 5a8b1448693ac2ba167ecbb90f1e4cb0c3847c13 Mon Sep 17 00:00:00 2001
From: Martin Dlouhy <md@robotika.cz>
Date: Sun, 10 Aug 2025 21:21:44 +0200
Subject: [PATCH 6/7] Matty M03 IMU calibration

---
 esp/common/config.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/esp/common/config.h b/esp/common/config.h
index 51cfff7..f85c3f4 100644
--- a/esp/common/config.h
+++ b/esp/common/config.h
@@ -71,9 +71,9 @@
     #define D              135  // prumer kol
 
     // IMU kalibrace
-    #define ACC_BIAS    {0, 0, 0}
-    #define GYRO_BIAS   {0, 0, 0}
-    #define MAG_BIAS    {0, 0, 0}
+    #define ACC_BIAS    {26.22, -57.87, -13.25}
+    #define GYRO_BIAS   {-2.57, 2.78, -1.17}
+    #define MAG_BIAS    {-21.3, 12.7, -21.7}
     #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
   #endif
 

From 895f32684dc49188d267e91402f0075d837fb90c Mon Sep 17 00:00:00 2001
From: Martin Locker <martin.locker@seznam.cz>
Date: Wed, 3 Sep 2025 20:29:03 +0200
Subject: [PATCH 7/7] Update neopixel and add M04, M05 configuration

---
 esp/Robot/src/main.cpp      | 13 +------------
 esp/Robot/src/robot.cpp     |  2 ++
 esp/common/config.h         | 20 ++++++++++----------
 esp/lib/NeoPixel/ws2812.cpp |  2 +-
 esp/lib/NeoPixel/ws2812.h   | 15 ++++++++++++---
 5 files changed, 26 insertions(+), 26 deletions(-)

diff --git a/esp/Robot/src/main.cpp b/esp/Robot/src/main.cpp
index 017ac00..e1397a2 100644
--- a/esp/Robot/src/main.cpp
+++ b/esp/Robot/src/main.cpp
@@ -62,7 +62,6 @@ void receiveCommand() {
         case 'L': robot.setLimits(comm.rxData.speed, comm.rxData.steer);
                   break;
         case 'T': robot.setTime(comm.rxData.period, comm.rxData.timeout);
-                  robot.led(0, COLOR::GREEN);
                   break;
         case 'P': gps.config(comm.rxData.mode);
                   break;
@@ -121,15 +120,6 @@ void loop() {
 
   if (robot.process()) { // periodicke odesilani dat
     comm.send(robot.status, robot.mode, robot.voltage, robot.current, robot.actualSpeed, robot.joint, robot.encoder, robot.roll, robot.pitch, robot.yaw);
-    static uint8_t lastStatus;
-    if ((lastStatus ^ robot.status) & ROBOT_STATUS::EMERGENCY_STOP) {
-      if (robot.status & ROBOT_STATUS::EMERGENCY_STOP) {
-        robot.led(1, COLOR::RED);
-      } else {
-        robot.led(1, COLOR::GREEN);
-      }
-    }
-    lastStatus = robot.status;
   }
 
   if (gps.process()) {
@@ -138,6 +128,5 @@ void loop() {
     if (length != 0) {
       comm.send('P', buffer, length);
     }
-  }
-  
+  }  
 }
diff --git a/esp/Robot/src/robot.cpp b/esp/Robot/src/robot.cpp
index 959dc30..f614003 100644
--- a/esp/Robot/src/robot.cpp
+++ b/esp/Robot/src/robot.cpp
@@ -24,6 +24,8 @@ void Robot::init() {
   stop();
   process();
   reset();
+  neopixel.init();
+  led(0, GREEN);
 }
 
 void Robot::reset() {
diff --git a/esp/common/config.h b/esp/common/config.h
index f85c3f4..d2217eb 100644
--- a/esp/common/config.h
+++ b/esp/common/config.h
@@ -78,29 +78,29 @@
   #endif
 
   #if (MATTY == 04) // Matty M04 - ruzovy
-    #define ZERO_JOINT   0.00f  // nulova poloha kloubu Matty M04 - Martin Dlouhy
+    #define ZERO_JOINT  172.18f  // nulova poloha kloubu Matty M04 - Martin Dlouhy
     #define L              320  // rozvor
     #define A              315  // rozchod
     #define D              135  // prumer kol
 
     // IMU kalibrace
-    #define ACC_BIAS    {0, 0, 0}
-    #define GYRO_BIAS   {0, 0, 0}
-    #define MAG_BIAS    {0, 0, 0}
-    #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
+    #define ACC_BIAS    {63.89, -12.78,  46.65}
+    #define GYRO_BIAS   {0.56,   1.10,  -0.76}
+    #define MAG_BIAS    {-35.06, 0.41, -57.53}
+    #define MAG_CORECT  {{1.021, -0.005, -0.023}, {-0.005, 1.040, 0.031}, {-0.023, 0.031, 0.943}}
   #endif
 
   #if (MATTY == 05) // Matty M05 - oranzovy
-    #define ZERO_JOINT   0.00f  // nulova poloha kloubu Matty M05 - Martin Dlouhy
+    #define ZERO_JOINT  157.68f  // nulova poloha kloubu Matty M05 - Martin Dlouhy
     #define L              320  // rozvor
     #define A              315  // rozchod
     #define D              135  // prumer kol
 
     // IMU kalibrace
-    #define ACC_BIAS    {0, 0, 0}
-    #define GYRO_BIAS   {0, 0, 0}
-    #define MAG_BIAS    {0, 0, 0}
-    #define MAG_CORECT  {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
+    #define ACC_BIAS    {12.37, -39.15, -15.27}
+    #define GYRO_BIAS   {-3.03,   1.62,  -0.25}
+    #define MAG_BIAS    {-20.56, 1.94, -45.34}
+    #define MAG_CORECT  {{1.052, 0.003, -0.017}, {0.003, 1.018, 0.013}, {-0.017, 0.013, 0.933}}
   #endif
 #endif
 
diff --git a/esp/lib/NeoPixel/ws2812.cpp b/esp/lib/NeoPixel/ws2812.cpp
index b488861..2f12c22 100644
--- a/esp/lib/NeoPixel/ws2812.cpp
+++ b/esp/lib/NeoPixel/ws2812.cpp
@@ -1,7 +1,7 @@
 #include "ws2812.h"
 
 Neopixel::Neopixel() {
-  init();
+//  init(); // nekdy funguje, ale ..., nutno inicializovat az pozdeji
 }
 
 esp_err_t Neopixel::init(void) {
diff --git a/esp/lib/NeoPixel/ws2812.h b/esp/lib/NeoPixel/ws2812.h
index d080954..e4a2272 100644
--- a/esp/lib/NeoPixel/ws2812.h
+++ b/esp/lib/NeoPixel/ws2812.h
@@ -45,12 +45,21 @@ on an ESP32 with an 80MHz clock: 0.28 * 80 / 2 = 11.2 => 11 (rounded when needed
 #define BITS_PER_LED_CMD	24
 #define LED_BUFFER_ITEMS	(NUM_LEDS * BITS_PER_LED_CMD)
 
+/*
 // These values are determined by measuring pulse timing with logic analyzer and adjusting to match datasheet. 
 #define T0H	20  // 0 bit high time WS2811
 #define T0L	80  // low time for either bit
 #define T1H	48  // 1 bit high time
 #define T1L	52
 #define TE  2000
+*/
+
+// These values are determined by measuring pulse timing with logic analyzer and adjusting to match datasheet. 
+#define T0H	14  // 0 bit high time WS2812
+#define T0L	32  // low time for either bit
+#define T1H	28  // 1 bit high time
+#define T1L	24
+#define TE  2000
 
 enum COLOR { // BRG
   RED   = 0x00003F,
@@ -64,6 +73,9 @@ class Neopixel {
     void  led(uint8_t index, uint32_t rgb);
     void  clear();
 
+    // Setup the hardware peripheral. Only call this once.
+    esp_err_t init(void);
+
   private:
     // This structure is used for indicating what the colors of each LED should be set to.
     // There is a 32bit value for each LED. Only the lower 3 bytes are used and they hold the
@@ -76,9 +88,6 @@ class Neopixel {
     // This is the buffer which the hw peripheral will access while pulsing the output pin
     rmt_item32_t led_data_buffer[LED_BUFFER_ITEMS+1];
 
-    // Setup the hardware peripheral. Only call this once.
-    esp_err_t init(void);
-
     // Update the LEDs to the new state. Call as needed.
     // This function will block the current task until the RMT peripheral is finished sending 
     // the entire sequence.