Arduino Leonardo подключился к MPU6500 с помощью программы 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)
}
}
Заранее спасибо