Как Настроить Mpu6500 На Работу И Прочитать Акселерометр?

Arduino leonardo подключился к mpu6500 с помощью программы go для получения данных – go

В настоящее время я работаю над датчиком MPU6500, который подключен к моему Arduino Leonardo.

Но я не очень разбираюсь в Arduino и GO, так что код не очень хорош, и я извиняюсь заранее.

Я не прошу кого-то сделать всю мою коррекцию, но если у кого-то есть подсказки, в чем заключаются мои проблемы или советы, это действительно может помочь

Моей целью было получить необработанные данные (ускорение, гироскоп, магнито) и обработать их. Поэтому, проведя много исследований и попробовав, я мог бы написать этот код (я знаю, что это большой беспорядок, и мне жаль):

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9250.h"    
#define    MPU9250_ADDRESS            0x68
MPU9250 accelgyro;
I2Cdev   I2C_M;


uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t   mx, my, mz;

float heading;
float tiltheading;

float Axyz[3];
float Gxyz[3];
float Mxyz[3];

#define sample_num_mdate  5000

volatile float mx_sample[3];
volatile float my_sample[3];
volatile float mz_sample[3];

static float mx_centre = 0;
static float my_centre = 0;
static float mz_centre = 0;

volatile int mx_max = 0;
volatile int my_max = 0;
volatile int mz_max = 0;

volatile int mx_min = 0;
volatile int my_min = 0;
volatile int mz_min = 0;



void setup()
{

    Wire.begin();

    Serial.begin(115200);
    accelgyro.initialize();
    delay(3000);
    // Mxyz_init_calibrated ();

}

void loop()
{

    getAccel_Data();
    getGyro_Data();
    getCompassDate_calibrated();
    getHeading();              
    getTiltHeading();

    //Serial.println("Acceleration(g) of X,Y,Z:");
    Serial.print(Axyz[0]);
    Serial.print("/");
    Serial.print(Axyz[1]);
    Serial.print("/");
    Serial.print(Axyz[2]);
    Serial.print("/");
    //Serial.println("Gyro(degress/s) of X,Y,Z:");
    Serial.print(Gxyz[0]);
    Serial.print("/");
    Serial.print(Gxyz[1]);
    Serial.print("/");
    Serial.print(Gxyz[2]);
    Serial.print("/");
    //Serial.println("Compass Valeur of X,Y,Z:");
    Serial.print(Mxyz[0]);
    Serial.print("/");
    Serial.print(Mxyz[1]);
    Serial.print("/");
    Serial.print(Mxyz[2]);
    Serial.print("/");

    delay(2000);

}


void getHeading(void)
{
    heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
    if (heading < 0) heading  = 360;
}

void getTiltHeading(void)
{
    float pitch = asin(-Axyz[0]);
    float roll = asin(Axyz[1] / cos(pitch));

    float xh = Mxyz[0] * cos(pitch)   Mxyz[2] * sin(pitch);
    float yh = Mxyz[0] * sin(roll) * sin(pitch)   Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
    float zh = -Mxyz[0] * cos(roll) * sin(pitch)   Mxyz[1] * sin(roll)   Mxyz[2] * cos(roll) * cos(pitch);
    tiltheading = 180 * atan2(yh, xh) / PI;
    if (yh < 0)    tiltheading  = 360;
}



void Mxyz_init_calibrated ()
{

   /* Serial.println(F("Avant d'utiliser 9DOF, nous avons besoin de calibrer la boussole, cela prend environ 2 minutes."));
    Serial.print("  ");
    Serial.println(F("Au cours du calibrage, vous devez faire pivoter et tourner le capteur en permanence pendant 2 minutes."));
    Serial.print("  ");
    Serial.println(F("envoyer l'information ready."));*/
    while (!Serial.find("ready"));
   /* Serial.println("  ");
    Serial.println("ready");
    Serial.println("demmarrage de l'echantillonnage......");
    Serial.println("attendez s'il vous plait ......");*/

    get_calibration_Data ();

    /*Serial.println("     ");
    Serial.println("paramètre de calibrage du compas");
    Serial.print(mx_centre);
    Serial.print("     ");
    Serial.print(my_centre);
    Serial.print("     ");
    Serial.println(mz_centre);
    Serial.println("    ");*/
}


void get_calibration_Data ()
{
    for (int i = 0; i < sample_num_mdate; i  )
    {
        get_one_sample_date_mxyz();


        // vous pouvez activer la visualisation de l'echantillonnage 
        // en desactivant les commentaires ci-dessous 

        /*
        Serial.print(mx_sample[2]);
        Serial.print(" ");
        Serial.print(my_sample[2]);                           
        Serial.print(" ");
        Serial.println(mz_sample[2]);
        */





        //trouver de la valeur max
        if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
        if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; 
        if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];

        if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
        if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; 
        if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];

    }

    mx_max = mx_sample[1];
    my_max = my_sample[1];
    mz_max = mz_sample[1];

    mx_min = mx_sample[0];
    my_min = my_sample[0];
    mz_min = mz_sample[0];



    mx_centre = (mx_max   mx_min) / 2;
    my_centre = (my_max   my_min) / 2;
    mz_centre = (mz_max   mz_min) / 2;

}






void get_one_sample_date_mxyz()
{
    getCompass_Data();
    mx_sample[2] = Mxyz[0];
    my_sample[2] = Mxyz[1];
    mz_sample[2] = Mxyz[2];
}


void getAccel_Data(void)
{
    uint8_t Buf[14];
    I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
    ax=-(Buf[0]<<8 | Buf[1]);
    ay=-(Buf[2]<<8 | Buf[3]);
    az=Buf[4]<<8 | Buf[5];
    //accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384;
}

void getGyro_Data(void)
{
    uint8_t Buf[14];
    I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
    gx=-(Buf[8]<<8 | Buf[9]);
    gy=-(Buf[10]<<8 | Buf[11]);
    gz=Buf[12]<<8 | Buf[13];
    //accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Gxyz[0] = (double) gx * 250 / 32768;
    Gxyz[1] = (double) gy * 250 / 32768;
    Gxyz[2] = (double) gz * 250 / 32768;
}

void getCompass_Data(void)
{


    I2C_M.writeByte(MPU9250_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
    delay(10);
    I2C_M.readBytes(MPU9250_MAG_ADDRESS, MPU9250_MAG_XOUT_L, 6, buffer_m);

    mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
    my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
    mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;

    Mxyz[0] = (double) mx * 1200 / 4096;
    Mxyz[1] = (double) my * 1200 / 4096;
    Mxyz[2] = (double) mz * 1200 / 4096;
}

void getCompassDate_calibrated ()
{
    getCompass_Data();
    Mxyz[0] = Mxyz[0] - mx_centre;
    Mxyz[1] = Mxyz[1] - my_centre;
    Mxyz[2] = Mxyz[2] - mz_centre;
}

// This function read Nbytes bytes from I2C device at address Address. 
// Put read bytes starting at register Register in the Data array. 
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission();

  // Read Nbytes
  Wire.requestFrom(Address, Nbytes); 
  uint8_t index=0;
  while (Wire.available())
    Data[index  ]=Wire.read();
}



// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}

Поэтому, когда я запускаю код, я получаю хорошие данные для Accelaration и Gyro, но я не могу получить Magneto, если я сначала не запустил этот код на Arduino, который я нашел в Интернете

#include <Wire.h>
#include <TimerOne.h>

#define    MPU9250_ADDRESS            0x68
#define    MAG_ADDRESS                0x0C

#define    GYRO_FULL_SCALE_250_DPS    0x00  
#define    GYRO_FULL_SCALE_500_DPS    0x08
#define    GYRO_FULL_SCALE_1000_DPS   0x10
#define    GYRO_FULL_SCALE_2000_DPS   0x18

#define    ACC_FULL_SCALE_2_G        0x00  
#define    ACC_FULL_SCALE_4_G        0x08
#define    ACC_FULL_SCALE_8_G        0x10
#define    ACC_FULL_SCALE_16_G       0x18



// This function read Nbytes bytes from I2C device at address Address. 
// Put read bytes starting at register Register in the Data array. 
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission();

  // Read Nbytes
  Wire.requestFrom(Address, Nbytes); 
  uint8_t index=0;
  while (Wire.available())
    Data[index  ]=Wire.read();
}


// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}



// Initial time
long int ti;
volatile bool intFlag=false;


// Calcul the speed
int16_t Speed(int16_t Axyz, int16_t vi, int16_t t){
  int16_t vf;
  vf = vi   (Axyz*t);
  return vf;  
}

int16_t Position(int16_t Vxyz, int16_t pi, int16_t t){
  int16_t pf;
  pf = pi   (Vxyz*t);
  return pf;  
}

// Initializations
void setup()
{
  // Arduino initializations
  Wire.begin();
  Serial.begin(115200);

  // Set accelerometers low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS,29,0x06);
  // Set gyroscope low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS,26,0x06);


  // Configure gyroscope range
  I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_1000_DPS);
  // Configure accelerometers range
  I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_4_G);
  // Set by pass mode for the magnetometers
  I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);

  // Request continuous magnetometer measurements in 16 bits
  I2CwriteByte(MAG_ADDRESS,0x0A,0x16);

  pinMode(13, OUTPUT);
  Timer1.initialize(10000);         // initialize timer1, and set a 1/2 second period
  Timer1.attachInterrupt(callback);  // attaches callback() as a timer overflow interrupt


  // Store initial value
  ti=millis();
}





// Counter
long int cpt=0;

void callback()
{ 
  intFlag=true;
  digitalWrite(13, digitalRead(13) ^ 1);
}

// Main loop, read and display data
void loop()
{
  while (!intFlag);
  intFlag=false;

  // Display time
  Serial.print (millis()-ti,DEC);
  Serial.print ("t");


  // _______________
  // ::: Counter :::

  // Display data counter
    Serial.print (cpt  ,DEC);
    Serial.print ("t");



  // ____________________________________
  // :::  accelerometer and gyroscope ::: 

  // Read accelerometer and gyroscope
  uint8_t Buf[14];
  I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);

  // Create 16 bits values from 8 bits data

  // Accelerometer
  int16_t ax=-(Buf[0]<<8 | Buf[1]);
  int16_t ay=-(Buf[2]<<8 | Buf[3]);
  int16_t az=Buf[4]<<8 | Buf[5];

  // Speed
  int16_t vx= 0; 
  vx = Speed(ax,0,millis()-ti);
  int16_t vy= 0;
  vy = Speed(ay,0,millis()-ti);
  int16_t vz= 0;
  vz = Speed(az,0,millis()-ti);

  //Position
  int16_t px= 0; 
  px = Position(vx,0,millis()-ti);
  int16_t py= 0;
  py = Position(vy,0,millis()-ti);
  int16_t pz= 0;
  pz = Position(vz,0,millis()-ti);

  // Gyroscope
  int16_t gx=-(Buf[8]<<8 | Buf[9]);
  int16_t gy=-(Buf[10]<<8 | Buf[11]);
  int16_t gz=Buf[12]<<8 | Buf[13];

    // Display values

  // Accelerometer
  //Serial.print ("ax : ");
  Serial.print (ax,DEC); 
  Serial.print (" ");
  //Serial.print ("ay : ");
  Serial.print (ay,DEC);
  Serial.print (" ");
  //Serial.print ("az : ");
  Serial.print (az,DEC);  
  Serial.print (" ");


  // Speed
 // Serial.print ("vx : ");
  Serial.print (vx,DEC); 
  Serial.print (" ");
  //Serial.print ("vy : ");
  Serial.print (vy,DEC);
  Serial.print (" ");
 // Serial.print ("vz : ");
  Serial.print (vz,DEC);  
  Serial.print (" ");

  // Postion
  //Serial.print ("px : ");
  Serial.print (px,DEC); 
  Serial.print (" ");
 // Serial.print ("py : ");
  Serial.print (py,DEC);
  Serial.print (" ");
 // Serial.print ("pz : ");
  Serial.print (pz,DEC);  
  Serial.print (" ");

  /*

  // Gyroscope
  Serial.print ("gx : ");
  Serial.print (gx,DEC); 
  Serial.print ("t");
  Serial.print ("gy : ");
  Serial.print (gy,DEC);
  Serial.print ("t");
  Serial.print ("gz : ");
  Serial.print (gz,DEC);  
  Serial.print ("t");
*/

  // _____________________
  // :::  Magnetometer ::: 


  // Read register Status 1 and wait for the DRDY: Data Ready

  uint8_t ST1;
  do
  {
    I2Cread(MAG_ADDRESS,0x02,1,&ST1);
  }
  while (!(ST1&0x01));

  // Read magnetometer data  
  uint8_t Mag[7];  
  I2Cread(MAG_ADDRESS,0x03,7,Mag);


  // Create 16 bits values from 8 bits data

  // Magnetometer
  int16_t mx=-(Mag[3]<<8 | Mag[2]);
  int16_t my=-(Mag[1]<<8 | Mag[0]);
  int16_t mz=-(Mag[5]<<8 | Mag[4]);

  /*
  // Magnetometer
  Serial.print ("mx : ");
  Serial.print (mx 200,DEC); 
  Serial.print ("t");
  Serial.print ("my : ");
  Serial.print (my-70,DEC);
  Serial.print ("t");
  Serial.print ("mz : ");
  Serial.print (mz-700,DEC);  
  Serial.print ("t");
*/


  // End of line
  Serial.println("");
  delay(1000); 

}

Обычно, когда он работает, я использую Go, чтобы получить данные буфера, такие как большая строка, и использую функцию strings.split, чтобы иметь возможность использовать их и использовать их для поиска скорости, но в настоящее время я получил только индекс ошибки связи, и знаю, что это значит, но это было с пятницы, я нахожусь на нем и не могу найти, где главная проблема…

Смотрите про коптеры:  Как выбрать моторы для квадрокоптера или гоночного дрона? | RCDetails Blog

Вот моя GO программа:

package main

import (
    "fmt"
    "log"
    "os"
    "strings"

    "github.com/tarm/serial"
)

func check(e error) {
    if e != nil {
        panic(e)
    }
}

func checkError(message string, err error) {
    if err != nil {
        log.Fatal(message, err)
    }
}


}

func main() {

    c := &serial.Config{Name: "COM5", Baud: 115200}
    s, err := serial.OpenPort(c)
    if err != nil {
        log.Fatal(err)
    }
    buf := make([]byte, 128)
    for {
        _, err := s.Read(buf)
        if err != nil {
            log.Fatal(err)
        }

        if err != nil {
            log.Fatal(err)
        }
        str := strings.Split(string(buf), "/")
        ax,ay,az,gx,gy,gz,mx,my,mz := str[0] //, str[1], str[2], str[3], str[4], str[5], str[6], str[7], str[8]
            log.Printf(ax)
            log.Printf(ay)

            log.Printf(az)
            log.Printf(gx)
            log.Printf(gy)
            log.Printf(gz)
            log.Printf(mx)
            log.Printf(my)
            log.Printf(mz)


    }
}

Заранее спасибо

Как настроить mpu6500 на работу и прочитать акселерометр?

#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
//Some of those boards have a pull-down resistor at AD0 (address = 0x68), others have a pull-up resistor (address = 0x69).
int16_t AcX,AcY,AcZ;

float time;

void setup() {
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
//or one, I am not so sure
delay(100);
Wire.write(0x68);
Wire.write(0);
delay(100);
Wire.endTransmission(true);
Serial.begin(115200);
time = micros();
}

void loop() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,6,true); // request a total of 6 registers

AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

Serial.print(“AcX = “); Serial.print(AcX);
Serial.print(” | AcY = “); Serial.print(AcY);
Serial.print(” | AcZ = “); Serial.println(AcZ);

//delay(333);
float temp = time – micros();
Serial.println(temp);
time = micros();

}

Оцените статью
Радиокоптер.ру
Добавить комментарий

Adblock
detector