Как Настроить 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, чтобы иметь возможность использовать их и использовать их для поиска скорости, но в настоящее время я получил только индекс ошибки связи, и знаю, что это значит, но это было с пятницы, я нахожусь на нем и не могу найти, где главная проблема…

Смотрите про коптеры:  Скачать Приложения для Дронов на Андроид

Вот моя 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();

}

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