Coordonnée Magnétomètre en Z

International Français
1

Bonsoir le forum !

Je suis encore sur mon drone Arduino, et j'ai enfin racheté un MPU9250 qui fonctionne bien.

J'ai donc la lecture des 9 axes.

Avec un peu de recherche, j'ai réussi à avoir l'inclinaison en x, y, et z et en degrés donc c'est parfait.
Néanmois, la valeur en z n'est vraiment pas stable et toutes les coordonnées vers les 200° sont comme "sautées". Par contre, quand c'est entre 270 et 180 (et pas entre 180 et 270), c'est plutôt précis.

Voici mon code (le + important)

 xmag = IMU.getMagX_uT();
  ymag = IMU.getMagY_uT();
  

  if (gx >= -3.0 && gx <= 3.0 && gy >= -3.0 && gy <= 3.0) {
    if (xmag > 0) {
      function = atan(ymag/xmag);
      }
    if (xmag == 0 && ymag > 0) {
      function = PI/2;
      }
    if (xmag == 0 && ymag < 0) {
      function = -PI/2;
      }
    if (xmag < 0 && ymag >= 0) {
      function = atan(ymag/xmag) + PI;
      }
    if (xmag < 0 && ymag < 0) {
      function = atan(ymag/xmag) - PI;
      }
    } 

  gz = 90 - function * (180/PI);
  
  if (gz < 0.0) {
    gz = gz + 360.0;
  }

gx, gy et gz c'est l'inclinaison du drone en x, en y, et en z

J'ai quand même trouvé un super code assez compliqué qui calcule la moyenne des valeurs pour en sortie donner une valeur précise

Voici le lien du post: Get stable values from magnetometer (MPU9250) - Programming Questions - Arduino Forum

et le code donné:

#include "MPU9250.h"

MPU9250 *IMU;
int status;

const  double xBias = 1624.766973;
const  double yBias = -977.863214;
const  double zBias = -674.986618;

const  double xScaleX = 0.012480;
const  double xScaleY = 0.000097;
const  double xScaleZ = 0.000032;

const  double yScaleX = 0.000097;
const  double yScaleY = 0.012680;
const  double yScaleZ = -0.000110;


const unsigned ACLineFrequency = 50;
const unsigned samplesPerACCycle = 2;
const unsigned sampleCount = ACLineFrequency * samplesPerACCycle;
const unsigned  sampleIntervalMicroseconds = 1000000UL / sampleCount;

 double *samplesX;
 double *samplesY;

 double sampleTotalX = 0;
 double sampleTotalY = 0;

 double sampleAverageX = 0;
 double sampleAverageY = 0;

unsigned sampleIndexX = 0;
unsigned sampleIndexY = 0;

unsigned  LastsampleTime;

 double getNewValue(char dimension)
{
  IMU->readSensor();

   double x, y, z;

  x = IMU->getMagX_uT() * (100000.0 / 1100.0) + xBias;
  y = IMU->getMagY_uT() * (100000.0 / 1100.0) + yBias;
  z = IMU->getMagZ_uT() * (100000.0 / 1100.0) + zBias;

   double calX, calY, calZ;

  calX = x * xScaleX + y * xScaleY + z * xScaleZ;
  calY = x * yScaleX + y * yScaleY + z * yScaleZ;

  if (dimension == 'x')
    return calX;  
 else if (dimension == 'y')
    return calY;
  else
    return calZ;
}


void Addsample()
{
  sampleTotalX -= samplesX[sampleIndexX];
  samplesX[sampleIndexX] = getNewValue('x');
  sampleTotalX += samplesX[sampleIndexX];

  sampleTotalY -= samplesY[sampleIndexY];
  samplesY[sampleIndexY] = getNewValue('y');
  sampleTotalY += samplesY[sampleIndexY];

  sampleIndexX = (sampleIndexX + 1) % sampleCount;
  sampleIndexY = (sampleIndexY + 1) % sampleCount;
}



void setup() {
  IMU = new MPU9250(Wire, 0x68);
  Serial.begin(115200);

  while (!Serial) {}
  status = IMU->begin();
  if (status < 0) {
     //...
  }

  samplesX = new  double[sampleCount]();
  samplesY = new  double[sampleCount]();

  for (unsigned i = 0; i < sampleCount; i++)
  {
    Addsample();
    delayMicroseconds(sampleIntervalMicroseconds); 
  }
  LastsampleTime = micros();

  sampleAverageX = sampleTotalX / sampleCount;
}

void loop() {

  unsigned  currentMicros = micros();
  if (currentMicros - LastsampleTime >= sampleIntervalMicroseconds)
  {
    LastsampleTime += sampleIntervalMicroseconds;
    Addsample();

    sampleAverageX = sampleTotalX / sampleCount;
    sampleAverageY = sampleTotalY / sampleCount;


  }

   double heading = atan2(sampleAverageY, sampleAverageX) * 180.0 / PI; 
  if (heading < 0.0) heading = heading + 360.0; 


  Serial.println(heading, 6);


}

On sent que les valeurs sont "lissées", mais il semble que l'intervalle des angles est [-pi/2; pi/2] et non [-pi; pi]

J'ai donc voulu injecter ce dernier code dans mon code "final" du drone, mais il y a clairement un problème de mémoire, et je ne peux plus rien écrire dans le Serial (je suis sûr que ça ne fonctionne pas)

Donc je voudrais garder ma première solution, mais c'est pas très précis ...

Sinon, ça vaut le coup de ne pas mettre de PID sur l'axe z ? (J'aurais perdu pas mal de temps, mais c'est pour la bonne cause :smiley: )

Merci pour la lecture du pavé, très bonne soirée à vous.

2

J'imagine que si je n'asservis pas l'axe des z, le drone va pivoter tout seul

3

Bon c'est réglé, j'ai trouvé la bibliothèque "MPU9250_asukiaaa"