gyro acce — ESP32 simulation

An interactive ESP32 circuit simulation you can run free in your browser on Velxio, by josephkinuthia541.

Components

Sketch code

#include <Wire.h>
#define IMU_ADDR 0x6A

float roll, pitch, yaw = 0;
float gyroRoll = 0, gyroPitch = 0, gyroYaw = 0;
unsigned long lastTime;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  
  // Wake up LSM6DS3 - Accel 416Hz 8G
  Wire.beginTransmission(IMU_ADDR);
  Wire.write(0x10);
  Wire.write(0x60);
  Wire.endTransmission();
  
  // Wake up LSM6DS3 - Gyro 416Hz 500dps
  Wire.beginTransmission(IMU_ADDR);
  Wire.write(0x11);
  Wire.write(0x60);
  Wire.endTransmission();

  Serial.println("LSM6DS at 0x6A - Full 3-Axis Drone IMU Ready");
  lastTime = millis();
  delay(500);
}

void loop() {
  // Read 12 bytes from register 0x22
  Wire.beginTransmission(IMU_ADDR);
  Wire.write(0x22);
  Wire.endTransmission(false);
  Wire.requestFrom(IMU_ADDR, 12, true);

  int16_t ax = Wire.read() | Wire.read() << 8;
  int16_t ay = Wire.read() | Wire.read() << 8;
  int16_t az = Wire.read() | Wire.read() << 8;
  int16_t gx = Wire.read() | Wire.read() << 8;
  int16_t gy = Wire.read() | Wire.read() << 8;
  int16_t gz = Wire.read() | Wire.read() << 8;

  float dt = (millis() - lastTime) / 1000.0;
  lastTime = millis();

  // Convert raw to real units
  float accX = ax / 4098.0; // 8G scale
  float accY = ay / 4098.0;
  float accZ = az / 4098.0;
  float gyroX = gx / 17.5; // 500dps scale
  float gyroY = gy / 17.5;
  float gyroZ = gz / 17.5;

  // --- ROLL & PITCH from accelerometer (gravity) ---
  float accRoll = atan2(accY, accZ) * 180 / PI;
  float accPitch = atan2(-accX, sqrt(accY*accY + accZ*accZ)) * 180 / PI;

  // --- ROLL, PITCH, YAW from gyroscope (integration) ---
  gyroRoll += gyroX * dt;
  gyroPitch += gyroY * dt;
  gyroYaw += gyroZ * dt;
  yaw = gyroYaw; // Yaw only from gyro, no accelerometer

  // --- Complementary Filter (96% gyro + 4% accel) ---
  roll = 0.96 * gyroRoll + 0.04 * accRoll;
  pitch = 0.96 * gyroPitch + 0.04 * accPitch;

  // --- Output for drone ---
  Serial.print("ROLL(X): "); Serial.print(roll);
  Serial.print(" | PITCH(Y): "); Serial.print(pitch);
  Serial.print(" | YAW(Z): "); Serial.println(yaw);
  
  delay(20);
}

More projects by josephkinuthia541