Bonjour,
J'utilise un accéléromètre gyroscope MPU6050 pour un projet de fusée à base d'Arduino. J'ai réussit à partir de ce capteur à obtenir la position angulaire de celle ci sur les 3 axes X, Y et Z à l'aide des nombreux programmes trouvables sur internet. Cependant, je n'arrive pas, à partir de ces trois angles, à déterminer l'inclinaison de la fusée par rapport au sol, ou à l'axe Z ( en effet, j'ai besoin d'ouvrir un parachute au moment ou la fusée attint une inclinaison précise). J'ai donc pensé à "fusionner" les valeurs des angles X et Y , mais je ne sais absolument pas comment m'y prendre et je n'ait trouver aucun code pour cela.
Auriez vous des idées de programme ou de formules mathématiques pour résoudre ce problème ?
Merci d'avance !
Je n'y aurait jamais pensé mais avec quelques modifications son programme marche !
Merci quand même pour cette réponse rapide ( et j'espère que ce topic pourra servir à d'autres )
Salut!
Peut-être peux-tu poster ici le programme et une copie de la réponse du chat qui pète, au cas ou le lien viendrai à expirer pour les futurs demandeurs?
Bine sur : voici le programme de chat gpt que j'ai modifié et qui marche
//----------------------------------------------------------------------------------------------------------------------
#include <Wire.h>
//----------------------------------------------------------------------------------------------------------------------
#define YAW 0
#define PITCH 1
#define ROLL 2
#define X 0 // X axis
#define Y 1 // Y axis
#define Z 2 // Z axis
#define MPU_ADDRESS 0x68 // I2C address of the MPU-6050
#define FREQ 250 // Sampling frequency
#define SSF_GYRO 65.5 // Sensitivity Scale Factor of the gyro from datasheet
//----------------------------------------------------------------------------------------------------------------------
// The RAW values got from gyro (in °/sec) in that order: X, Y, Z
int gyro_raw[3] = {0, 0, 0};
// Average gyro offsets of each axis in that order: X, Y, Z
long gyro_offset[3] = {0, 0, 0};
// Calculated angles from gyro's values in that order: X, Y, Z
float gyro_angle[3] = {0, 0, 0};
// The RAW values got from accelerometer (in m/sec²) in that order: X, Y, Z
int acc_raw[3] = {0 , 0 , 0};
// Calculated angles from accelerometer's values in that order: X, Y, Z
float acc_angle[3] = {0, 0, 0};
// Total 3D acceleration vector in m/s²
long acc_total_vector;
float inclinaison;
/**
* Real measures on 3 axis calculated from gyro AND accelerometer in that order : Yaw, Pitch, Roll
* - Left wing up implies a positive roll
* - Nose up implies a positive pitch
* - Nose right implies a positive yaw
*/
float measures[3] = {0, 0, 0};
// MPU's temperature
int temperature;
// Init flag set to TRUE after first loop
boolean initialized;
unsigned int period; // Sampling period
unsigned long loop_timer;
//----------------------------------------------------------------------------------------------------------------------
void setup() {
// Serial.begin(57600); Only for debug
Wire.begin();
TWBR = 12; // Set the I2C clock speed to 400kHz.
setupMpu6050Registers();
calibrateMpu6050();
loop_timer = micros();
period = (1000000 / FREQ) ; // Sampling period in µs
}
void loop() {
readSensor();
calculateAngles();
inclinaison = sqrt(pow(measures[ROLL] , 2) + pow(measures[PITCH], 2));
while (micros() - loop_timer < period);
loop_timer = micros();
}
/**
* Configure gyro and accelerometer precision as following:
* - accelerometer: ±8g
* - gyro: ±500°/s
*
* @see https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
* @return void
*/
void setupMpu6050Registers() {
// Configure power management
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x6B); // Request the PWR_MGMT_1 register
Wire.write(0x00); // Apply the desired configuration to the register
Wire.endTransmission(); // End the transmission
// Configure the gyro's sensitivity
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1B); // Request the GYRO_CONFIG register
Wire.write(0x08); // Apply the desired configuration to the register : ±500°/s
Wire.endTransmission(); // End the transmission
// Configure the acceleromter's sensitivity
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1C); // Request the ACCEL_CONFIG register
Wire.write(0x10); // Apply the desired configuration to the register : ±8g
Wire.endTransmission(); // End the transmission
Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
Wire.write(0x1A); // Request the CONFIG register
Wire.write(0x03); // Set Digital Low Pass Filter about ~43Hz
Wire.endTransmission(); // End the transmission
}
/**
* Calibrate MPU6050: take 2000 samples to calculate average offsets.
* During this step, the quadcopter needs to be static and on a horizontal surface.
*
* This function also sends low throttle signal to each ESC to init and prevent them beeping annoyingly.
*
* This function might take ~2sec for 2000 samples.
*
* @return void
*/
void calibrateMpu6050()
{
int max_samples = 2000;
for (int i = 0; i < max_samples; i++) {
readSensor();
gyro_offset[X] += gyro_raw[X];
gyro_offset[Y] += gyro_raw[Y];
gyro_offset[Z] += gyro_raw[Z];
// Just wait a bit before next loop
delay(3);
}
// Calculate average offsets
gyro_offset[X] /= max_samples;
gyro_offset[Y] /= max_samples;
gyro_offset[Z] /= max_samples;
}
/**
* Request raw values from MPU6050.
*
* @return void
*/
void readSensor() {
Wire.beginTransmission(MPU_ADDRESS); // Start communicating with the MPU-6050
Wire.write(0x3B); // Send the requested starting register
Wire.endTransmission(); // End the transmission
Wire.requestFrom(MPU_ADDRESS, 14); // Request 14 bytes from the MPU-6050
// Wait until all the bytes are received
while (Wire.available() < 14);
acc_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[X] variable
acc_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Y] variable
acc_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Z] variable
temperature = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the temperature variable
gyro_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[X] variable
gyro_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Y] variable
gyro_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Z] variable
}
/**
* Calculate real angles from gyro and accelerometer's values
*/
void calculateAngles()
{
calculateGyroAngles();
calculateAccelerometerAngles();
if (initialized) {
// Correct the drift of the gyro with the accelerometer
gyro_angle[X] = gyro_angle[X] * 0.9996 + acc_angle[X] * 0.0004;
gyro_angle[Y] = gyro_angle[Y] * 0.9996 + acc_angle[Y] * 0.0004;
} else {
// At very first start, init gyro angles with accelerometer angles
gyro_angle[X] = acc_angle[X];
gyro_angle[Y] = acc_angle[Y];
initialized = true;
}
// To dampen the pitch and roll angles a complementary filter is used
measures[ROLL] = measures[ROLL] * 0.9 + gyro_angle[X] * 0.1;
measures[PITCH] = measures[PITCH] * 0.9 + gyro_angle[Y] * 0.1;
measures[YAW] = -gyro_raw[Z] / SSF_GYRO; // Store the angular motion for this axis
}
/**
* Calculate pitch & roll angles using only the gyro.
*/
void calculateGyroAngles()
{
// Subtract offsets
gyro_raw[X] -= gyro_offset[X];
gyro_raw[Y] -= gyro_offset[Y];
gyro_raw[Z] -= gyro_offset[Z];
// Angle calculation using integration
gyro_angle[X] += (gyro_raw[X] / (FREQ * SSF_GYRO));
gyro_angle[Y] += (-gyro_raw[Y] / (FREQ * SSF_GYRO)); // Change sign to match the accelerometer's one
// Transfer roll to pitch if IMU has yawed
gyro_angle[Y] += gyro_angle[X] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
gyro_angle[X] -= gyro_angle[Y] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
}
/**
* Calculate pitch & roll angles using only the accelerometer.
*/
void calculateAccelerometerAngles()
{
// Calculate total 3D acceleration vector : √(X² + Y² + Z²)
acc_total_vector = sqrt(pow(acc_raw[X], 2) + pow(acc_raw[Y], 2) + pow(acc_raw[Z], 2));
// To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
if (abs(acc_raw[X]) < acc_total_vector) {
acc_angle[X] = asin((float)acc_raw[Y] / acc_total_vector) * (180 / PI); // asin gives angle in radian. Convert to degree multiplying by 180/pi
}
if (abs(acc_raw[Y]) < acc_total_vector) {
acc_angle[Y] = asin((float)acc_raw[X] / acc_total_vector) * (180 / PI);
}
}
la ligne permettant de déterminer l'inclinaison (qui utilise le théorème de pythagore ) est la suivante:
)