[Arduino] Problème angles gyroscope par ExpCarthage - page 1

Bonjour à tous,
je suis toujours sur mon projet de quadri-rotor maison et je viens vers vous suite à un petit problème concernant le calcul des angles sortant du gyroscope MPU-6050.

Tout fonctionne "bien", les angles dérivent pratiquement pas, en revanche, lorsque j'incline le gyroscope de 90°, ce dernier m'indique un angle d'environ 45°...

Je ne sais pas si ma technique pour convertir en angle est correcte donc si un oeil aguerri pourrai m'éclairer sur une éventuelle erreur.
J'avais testé de multiplier par 2 les valeurs AngleX, AngleY et AngleZ mais je pense que cette technique ne doit pas forcément être la bonne...

Je vous mets le code que j'utilise :

#include <util/twi.h>

/***************************************************
 **  Configuration                                **
 ***************************************************/

#define CPPM_NUMBER_CHANNELS    4

#define PWM_NUMBER_MOTORS 4

#define IMU_ALPHA 0.97

/***************************************************/

volatile unsigned long timer2_overflow_count = 0;

unsigned int cppm[CPPM_NUMBER_CHANNELS];
unsigned int motor[PWM_NUMBER_MOTORS];

long GyroX_offset = 0;
long GyroY_offset = 0;
long GyroZ_offset = 0;
float GyroX_angle = 0;
float GyroY_angle = 0;
float GyroZ_angle = 0;
float AngleX;
float AngleY;
float AngleZ;

long t_last = 0;

void setup() {
  Serial.begin(115200);

  Serial.print("Initialisation CPPM...           ");
  RX_init();
  Serial.println("OK");
  
  Serial.print("Initialisation PWM...            ");
  Output_init();
  Serial.println("OK");
  
  Serial.print("Initialisation IMU...            ");
  IMU_init();
  Serial.println("OK");
  
  Serial.print("Calibrage gyroscope en cours...  ");
  IMU_calibrate();
  t_last = millis_t2();
  Serial.print("OK     ");

  TIMSK2 = (1 << TOIE2);
}

void loop() {
  IMU_getAngles();

  Serial.print("X : "); Serial.print(AngleX);
  Serial.print(" | Y : "); Serial.print(AngleY);
  Serial.print(" | Z : "); Serial.println(AngleZ);

  unsigned int throttle = map(cppm[0], 1170, 3220, 1000, 2000);

  for(unsigned char i = 0; i < PWM_NUMBER_MOTORS; i++) {
    motor[i] = throttle;
  }
  
  Output_writeMotors();
}

/***************************************************
 **  Timer2                                       **
 ***************************************************/

long micros_t2() {
  return (timer2_overflow_count * 1024) + ((((TCNT2 + 1) * 64) * 1000L) / (F_CPU / 1000L));
}

long millis_t2() {
  return micros_t2() / 1000;
}

SIGNAL(TIMER2_OVF_vect) {
  timer2_overflow_count++;
}

/***************************************************
 **  RX                                           **
 ***************************************************/

bool startPulse = false;
unsigned char currentChannel = 0;
unsigned long widthPulse = 0;

void RX_init(void) {
  DDRB &= ~(1 << PORTB0);
  TCCR1A = 0;
  TCCR1B = (1 << ICES1) | (1 << CS11);
  TIMSK1 = (1 << ICIE1);
}

ISR(TIMER1_CAPT_vect) {
  TCNT1 = 0;

  if(TCCR1B & (1 << ICES1)) {
    TCCR1B &= ~(1 << ICES1);
  }
  else {
    widthPulse = ICR1;
    TCCR1B |= (1 << ICES1);

    if(widthPulse > 20000) {
      startPulse = true;
    }
    else {
      if(startPulse) {
        cppm[currentChannel] = widthPulse;
        currentChannel++;

        if(currentChannel == CPPM_NUMBER_CHANNELS) {
          currentChannel = 0;
          startPulse = false;
        }
      }
    }
  }
}

/***************************************************
 **  Output                                       **
 ***************************************************/

void Output_init(void) {
  DDRB |= (1 << PORTB3);
  DDRD |= (1 << PORTD6) | (1 << PORTD5) | (1 << PORTD3);
  TCCR0A = (1 << COM0A1) | (1 << COM0B1) | (1 << WGM00);
  TCCR0B = (1 << CS01) | (1 << CS00);
  TCCR2A = (1 << COM2A1) | (1 << COM2B1) | (1 << WGM20);
  TCCR2B = (1 << CS22);

  Output_writeAllMotors(1000);
  _delay_ms(300);
}

void Output_writeAllMotors(unsigned int mc) {
  for(unsigned char i = 0; i < PWM_NUMBER_MOTORS; i++) {
    motor[i] = mc;
  }
  Output_writeMotors();
}

void Output_writeMotors(void) {
  OCR0A = motor[0] >> 3;
  OCR0B = motor[1] >> 3;
  OCR2A = motor[2] >> 3;
  OCR2B = motor[3] >> 3;
}

/***************************************************
 **  IMU                                          **
 ***************************************************/

void IMU_init(void) {
  (*(volatile unsigned char *)((unsigned int) &(TWSR))) &= ~((1 << TWPS1) | (1 << TWPS0));
  TWBR = ((F_CPU / 100000L) - 16) / 2;

  IMU_write(0x68, 0x6B, 0b10000000);
  _delay_ms(100);
  IMU_write(0x68, 0x6B, 0b00000000);
  IMU_write(0x68, 0x1B, 0b00000000);  // GYRO_CONFIG  - FS_SEL : 0
  IMU_write(0x68, 0x1C, 0b00000000);  // ACCEL_CONFIG - AFS_SEL : 0
}

void IMU_calibrate() {
  for(unsigned int i = 0; i < 1000; i++) {
    GyroX_offset += IMU_read(0x68, 0x43) << 8 | IMU_read(0x68, 0x44);
    GyroY_offset += IMU_read(0x68, 0x45) << 8 | IMU_read(0x68, 0x46);
    GyroZ_offset += IMU_read(0x68, 0x47) << 8 | IMU_read(0x68, 0x48);
  }
  GyroX_offset /= 1000;
  GyroY_offset /= 1000;
  GyroZ_offset /= 1000;
}

void IMU_getAngles(void) {
  long t_now = millis_t2();
  float dt = (t_now - t_last) / 1000.00;
  
  int AccelX = (IMU_read(0x68, 0x3B) << 8 | IMU_read(0x68, 0x3C));
  int AccelY = (IMU_read(0x68, 0x3D) << 8 | IMU_read(0x68, 0x3E));
  int AccelZ = (IMU_read(0x68, 0x3F) << 8 | IMU_read(0x68, 0x40));
  int GyroX  = ((IMU_read(0x68, 0x43) << 8 | IMU_read(0x68, 0x44)) - GyroX_offset) / 131;
  int GyroY  = ((IMU_read(0x68, 0x45) << 8 | IMU_read(0x68, 0x46)) - GyroY_offset) / 131;
  int GyroZ  = ((IMU_read(0x68, 0x47) << 8 | IMU_read(0x68, 0x48)) - GyroZ_offset) / 131;

  float AccelX_angle = atan(AccelY / sqrt(pow(AccelX, 2) + pow(AccelZ, 2))) * RAD_TO_DEG;
  float AccelY_angle = atan(AccelX / sqrt(pow(AccelY, 2) + pow(AccelZ, 2))) * RAD_TO_DEG;
  float AccelZ_angle = atan(AccelZ / sqrt(pow(AccelY, 2) + pow(AccelY, 2))) * RAD_TO_DEG;

  GyroX_angle = GyroX * dt + GyroX_angle;
  GyroY_angle = GyroY * dt + GyroY_angle;
  GyroZ_angle = GyroZ * dt + GyroZ_angle;

  // Filtre complémentaire
  AngleX = IMU_ALPHA * GyroX_angle + (1.00 - IMU_ALPHA) * AccelX_angle;
  AngleY = IMU_ALPHA * GyroY_angle + (1.00 - IMU_ALPHA) * AccelY_angle;
  AngleZ = IMU_ALPHA * GyroZ_angle + (1.00 - IMU_ALPHA) * AccelZ_angle;
  
  t_last = t_now;
}

unsigned char IMU_read(unsigned char device, unsigned char address) {
  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWDR = (device << 1) + TW_WRITE;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWDR = address;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWDR = (device << 1) + TW_READ;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
  while(!(TWCR & (1 << TWSTO)));

  return TWDR;
}

void IMU_write(unsigned char device, unsigned char address, unsigned char data) {
  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWDR = (device << 1) + TW_WRITE;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWDR = address;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWDR = data;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while(!(TWCR & (1 << TWINT)));
  TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
  while(!(TWCR & (1 << TWSTO)));
}

D'avance merci

-
Edité par ExpCarthage 22 juillet 2016 à 17:03:25

[Arduino] Problème angles gyroscope

[Arduino] Problème angles gyroscope

Supprimer

Modérer

Signaler le message aux modérateurs