Есть готовыи код подключения MPU 9250, нужно внести изменения на подключение и соответственно считывания значении с трех модулеи либо через SPI либо с помощью библиотеки #include <SoftwareWire.h> SoftwareWire myWire1(4, 5);//4 sda and 5 scl SoftwareWire myWire2(6, 7);//6 sda and 7 scl SoftwareWire myWire2(8, 9);//6 sda and 7 scl Вот код Код (C++): #include <Wire.h> #define MPU9250_ADDRESS 0x68 #define MAG_ADDRESS 0xC #define AK8963_ADDRESS 0xC #define AK8963_ST1 0x02 #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 #define Pi 3,1415926 #define WHO_AM_I_MPU9250 0x75 enum Mscale { MFS_14BITS = 0, // 0.6 mG per LSB MFS_16BITS // 0.15 mG per LSB }; uint8_t Mmode = 0x06; uint8_t Mscale = MFS_16BITS; float aRes, gRes, mRes; float X ; float Y ; float Z ; void getMres() { switch (Mscale) { // Possible magnetometer scales (and their register bit settings) are: // 14 bit resolution (0) and 16 bit resolution (1) case MFS_14BITS: mRes = 10.*4912./8190.; // Proper scale to return milliGauss break; case MFS_16BITS: mRes = 10.*4912./32760.0; // Proper scale to return milliGauss break; } } // 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(false); // 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(); } float xv, yv, zv; //calibrated_values[3] is the global array where the calibrated data will be placed //calibrated_values[3]: [0]=Xc, [1]=Yc, [2]=Zc float calibrated_values[3]; //transformation(float uncalibrated_values[3]) is the function of the magnetometer data correction //uncalibrated_values[3] is the array of the non calibrated magnetometer data //uncalibrated_values[3]: [0]=Xnc, [1]=Ync, [2]=Znc void transformation(float uncalibrated_values[3]) { //calibration_matrix[3][3] is the transformation matrix //replace M11, M12,..,M33 with your transformation matrix data double calibration_matrix[3][3] = { {2195.878755, -32.093680, 6.147725},// Матрица пока не верная но уже работает! {-32.093680, 2372.162687, -7.654780}, {6.147725, -7.654780, 2407.630918} }; //bias[3] is the bias //replace Bx, By, Bz with your bias data double bias[3] = { 0.189463, -0.176225, 0.127606 }; //calculation for (int i=0; i<3; ++i) uncalibrated_values[i] = uncalibrated_values[i] - bias[i]; float result[3] = {0, 0, 0}; for (int i=0; i<3; ++i) for (int j=0; j<3; ++j) result[i] += calibration_matrix[i][j] * uncalibrated_values[j]; for (int i=0; i<3; ++i) calibrated_values[i] = result[i]; } //vector_length_stabilasation() - is the function of the magnetometer vector length stabilasation (stabilisation of the sphere radius) float scaler; boolean scaler_flag = false; float normal_vector_length; void vector_length_stabilasation(){ //calculate the normal vector length if (scaler_flag == false) { getHeading(); normal_vector_length = sqrt(calibrated_values[0]*calibrated_values[0] + calibrated_values[1]*calibrated_values[1] + calibrated_values[2]*calibrated_values[2]); scaler_flag = true; } //calculate the current scaler scaler = normal_vector_length/sqrt(calibrated_values[0]*calibrated_values[0] + calibrated_values[1]*calibrated_values[1] + calibrated_values[2]*calibrated_values[2]); //apply the current scaler to the calibrated coordinates (global array calibrated_values) calibrated_values[0] = calibrated_values[0]*scaler; calibrated_values[1] = calibrated_values[1]*scaler; calibrated_values[2] = calibrated_values[2]*scaler; } // Initializations void setup() { // Arduino initializations Wire.begin(); Serial.begin(115200); // Configure gyroscope range I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS); // Configure accelerometers range I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_16_G); // Set by pass mode for the magnetometers I2CwriteByte(MPU9250_ADDRESS,0x37,0x02); // Request first magnetometer single measurement I2CwriteByte(MAG_ADDRESS,0x0A,Mscale << 4 | Mmode );//Mscale << 4 | Mmode } long int cpt=0; // Main loop, read and display data void loop() { // _______________ // ::: 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]; // 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 ("Значения акселерометра"); //Serial.print ("\t"); //Serial.print (ax,DEC); //Serial.print ("\t"); //Serial.print (ay,DEC); //Serial.print ("\t"); //Serial.print (az,DEC); //Serial.print ("\t"); //Gyroscope //Serial.print ("Значения гироскопа"); //Serial.print ("\t"); //Serial.print (gx,DEC); //Serial.print ("\t"); //Serial.print (gy,DEC); //Serial.print ("\t"); //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)); if(readByte(MAG_ADDRESS, 0x02)& 0x01); { // Read magnetometer data uint8_t Mag[7]; //readBytes(AK8963_ADDRESS, 0x03, 7, &Mag[0]); I2Cread(MAG_ADDRESS,0x03,7,&Mag[0]); uint8_t c = Mag[6]; if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data // 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]); X = mx; Y = my; Z = mz; //float values_from_magnetometer[3]; // getHeading(); // values_from_magnetometer[0] = xv; //values_from_magnetometer[1] = yv; //values_from_magnetometer[2] = zv; //transformation(values_from_magnetometer); //vector_length_stabilasation(); //Serial.flush(); //Serial.print(calibrated_values[0]); //Serial.print(","); // Serial.print(calibrated_values[1]); //Serial.print(","); //Serial.print(calibrated_values[2]); // Serial.println(); // delay(100); // Magnetometer //Serial.print ("Значения магнитометра"); //Serial.print ("\t"); Serial.print (X ,DEC); Serial.print ("\t"); Serial.println (Y ,DEC); Serial.println ("\t"); Serial.print (Z ,DEC); Serial.print ("\t"); Serial.print ( Mscale << 4 | Mmode ,DEC); Serial.print ("\t"); // Serial.print (mx,DEC); //Serial.print ("\t"); //Serial.println (my ,DEC); //Serial.println ("\t"); //Serial.print (mz,DEC); //Serial.print ("\t"); } } // End of line //Serial.println(""); delay(100); } uint8_t readByte(uint8_t address, uint8_t subAddress) { uint8_t data; // `data` will store the register data Wire.beginTransmission(address); // Initialize the Tx buffer Wire.write(subAddress); // Put slave register address in Tx buffer Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address data = Wire.read(); // Fill Rx buffer with result return data; // Return data read from slave register } void getHeading() { xv = X; yv = Y; zv = Z; }
Ок! Я согласен на 2 тыщ, три момента две кнопки ещё прикрутить, одна для передачи данных в порт, другая при длительном нажатии 3 сек пишет в порт слово, и третье это реализация разницы между предыдущим и текущим значением, на примере одного значения, я потом разнести для всех значений.
