so i am new to the field and am doing a drone using esp32 and gy-85. after much work i got the accelerometer working (i think ) and gyroscope but not at the same time (also i think ) . i want to combine their data to get 1 roll 1 pitch and 1 yaw to control the propellers .
i think because i got a lot of values .the accelerometer is ADXL345 and the gyroscope is ITG3200 . if there is problems in the codes below please tell me , but the main issue is how to combine them . i know that yaw isnt my code, i forgot about it and now noticed it , but i guess if i figured roll and pitch yaw would be the same.
thanks
accelerometer code:
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>
// Create an instance of the ADXL345 class
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified();
// Calibration offsets
float x_offset = 0.0;
float y_offset = 0.0;
float z_offset = 0.0;
// Filter coefficients
float alpha = 0.9; // Adjust alpha as needed
float roll_filtered = 0.0;
float pitch_filtered = 0.0;
float z_acc_filtered = 0.0;
void setup() {
Serial.begin(115200);
if (!accel.begin()) {
Serial.println("Failed to initialize ADXL345 sensor!");
while (1);
}
// Calibrate the accelerometer
calibrateAccelerometer();
}
void loop() {
sensors_event_t event;
accel.getEvent(&event);
// Apply offsets
float x_corrected = event.acceleration.x - x_offset;
float y_corrected = event.acceleration.y - y_offset;
float z_corrected = event.acceleration.z - z_offset;
// Calculate Roll and Pitch using updated formulas
float roll = atan2(y_corrected, sqrt(pow(x_corrected, 2) + pow(z_corrected, 2))) * 180 / PI;
float pitch = atan2(-x_corrected, sqrt(pow(y_corrected, 2) + pow(z_corrected, 2))) * 180 / PI;
// Apply low-pass filter
roll_filtered = alpha * roll_filtered + (1 - alpha) * roll;
pitch_filtered = alpha * pitch_filtered + (1 - alpha) * pitch;
z_acc_filtered = alpha * z_acc_filtered + (1 - alpha) * z_corrected;
// Serial output for debugging
Serial.print("Raw Roll: ");
Serial.print(roll);
Serial.print(" Raw Pitch: ");
Serial.print(pitch);
Serial.print(" Raw Z-Acc: ");
Serial.println(z_corrected);
Serial.print("Filtered Roll: ");
Serial.print(roll_filtered);
Serial.print(" Filtered Pitch: ");
Serial.print(pitch_filtered);
Serial.print(" Filtered Z-Acc: ");
Serial.println(z_acc_filtered);
// Control motors with filtered values
controlMotors(roll_filtered, pitch_filtered, z_acc_filtered);
delay(50);
}
void calibrateAccelerometer() {
int numReadings = 100;
float x_sum = 0.0;
float y_sum = 0.0;
float z_sum = 0.0;
for (int i = 0; i < numReadings; i++) {
sensors_event_t event;
accel.getEvent(&event);
x_sum += event.acceleration.x;
y_sum += event.acceleration.y;
z_sum += event.acceleration.z;
delay(50);
}
// Calculate average values
x_offset = x_sum / numReadings;
y_offset = y_sum / numReadings;
z_offset = z_sum / numReadings + 9.81; // Adjust Z-axis for gravity
}
void controlMotors(float roll, float pitch, float z_acc) {
// Placeholder for motor control logic using PID control
// Adjust motor speeds based on roll, pitch, and altitude control (z_acc)
// For example:
// motor1_speed = base_speed + (adjustment based on roll/pitch);
// motor2_speed = base_speed - (adjustment based on roll/pitch);
// Serial output for debugging
Serial.print("Roll: ");
Serial.print(roll);
Serial.print(" Pitch: ");
Serial.print(pitch);
Serial.print(" Z-Acc: ");
Serial.println(z_acc);
}
gyroscope code
#include <Wire.h>
#include "ITG3200.h"
ITG3200 gyro;
void setup() {
Serial.begin(9600);
gyro.init();
gyro.zeroCalibrate(200, 10); //sample 200 times to calibrate and it will take 200*10ms
}
void loop() {
// Serial.print("Temperature = ");
// Serial.print(gyro.getTemperature());
//Serial.println(" C");
/*int16_t x, y, z;
gyro.getXYZ(&x, &y, &z);
Serial.print("values of X , Y , Z: ");
Serial.print(x);
Serial.print(" , ");
Serial.print(y);
Serial.print(" , ");
Serial.println(z);*/
float ax, ay, az;
gyro.getAngularVelocity(&ax, &ay, &az);
Serial.print("Angular Velocity of X , Y , Z: ");
Serial.print(ax);
Serial.print(" , ");
Serial.print(ay);
Serial.print(" , ");
Serial.print(az);
Serial.println(" degrees per second");
Serial.println("*************");
delay(1000);
}
this part is what chatgpt did but the values i got werent correct . for example at first when the module is stable the values are good but if i move it i get values that arent correct + when the put back on the table the values dont back to almost 0
//MOST PROBABLY WRONG
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>
#include "ITG3200.h"
// Accelerometer setup
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified();
float x_offset = 0.0;
float y_offset = 0.0;
float z_offset = 0.0;
float alpha = 0.9; // Filter coefficient
float roll_filtered = 0.0;
float pitch_filtered = 0.0;
// Gyroscope setup
ITG3200 gyro;
float gyro_x = 0.0;
float gyro_y = 0.0;
float gyro_z = 0.0;
void setup() {
Serial.begin(115200);
// Initialize accelerometer
if (!accel.begin()) {
Serial.println("Failed to initialize ADXL345 sensor!");
while (1);
}
calibrateAccelerometer();
// Initialize gyroscope
gyro.init();
gyro.zeroCalibrate(200, 10); // Calibrate gyroscope
}
void loop() {
// Read accelerometer data
sensors_event_t event;
accel.getEvent(&event);
float x_corrected = event.acceleration.x - x_offset;
float y_corrected = event.acceleration.y - y_offset;
float z_corrected = event.acceleration.z - z_offset;
// Calculate roll and pitch from accelerometer data
float roll_acc = atan2(y_corrected, sqrt(pow(x_corrected, 2) + pow(z_corrected, 2))) * 180 / PI;
float pitch_acc = atan2(-x_corrected, sqrt(pow(y_corrected, 2) + pow(z_corrected, 2))) * 180 / PI;
// Read gyroscope data
gyro.getAngularVelocity(&gyro_x, &gyro_y, &gyro_z);
// Integrate gyroscope data for drift compensation
static float roll_gyro = 0.0;
static float pitch_gyro = 0.0;
roll_gyro += gyro_x * 0.01; // Assuming 10 ms loop delay
pitch_gyro += gyro_y * 0.01; // Assuming 10 ms loop delay
// Complementary filter to combine accelerometer and gyroscope data
roll_filtered = alpha * (roll_filtered + gyro_x * 0.01) + (1 - alpha) * roll_acc;
pitch_filtered = alpha * (pitch_filtered + gyro_y * 0.01) + (1 - alpha) * pitch_acc;
// Serial output for debugging
/*Serial.print("Raw Roll (Accel): ");
Serial.print(roll_acc);
Serial.print(" Raw Pitch (Accel): ");
Serial.print(pitch_acc);
Serial.print(" Raw Roll (Gyro): ");
Serial.print(roll_gyro);
Serial.print(" Raw Pitch (Gyro): ");
Serial.print(pitch_gyro);*/
Serial.print(" Filtered Roll: ");
Serial.print(roll_filtered);
Serial.print(" Filtered Pitch: ");
Serial.println(pitch_filtered);
// Control motors with filtered values
//controlMotors(roll_filtered, pitch_filtered);
delay(1000); // Adjust loop delay as necessary
}
void calibrateAccelerometer() {
int numReadings = 100;
float x_sum = 0.0;
float y_sum = 0.0;
float z_sum = 0.0;
for (int i = 0; i < numReadings; i++) {
sensors_event_t event;
accel.getEvent(&event);
x_sum += event.acceleration.x;
y_sum += event.acceleration.y;
z_sum += event.acceleration.z;
delay(50);
}
x_offset = x_sum / numReadings;
y_offset = y_sum / numReadings;
z_offset = z_sum / numReadings + 9.81; // Adjust Z-axis for gravity
}
/*void controlMotors(float roll, float pitch) {
// Placeholder for motor control logic using PID control
// Adjust motor speeds based on roll and pitch
// Example:
// motor1_speed = base_speed + (adjustment based on roll/pitch);
// motor2_speed = base_speed - (adjustment based on roll/pitch);
Serial.print("Roll: ");
Serial.print(roll);
Serial.print(" Pitch: ");
Serial.println(pitch);
}*/
1