jremington / mpu-9250-ahrs Goto Github PK

View Code? Open in Web Editor NEW

46.0 4.0 4.0 143 KB

Arduino Mahony AHRS, includes magnetometer and accelerometer calibration code

C++ 50.87% C 48.04% Python 1.10%

navigation imu compass ahrs

mpu-9250-ahrs's People

Contributors

Stargazers

Watchers

Forkers

fengzhang123 geekkernel drf5n crnicholson

mpu-9250-ahrs's Issues

Compile error min() required

Hi, I wanted to use your roll offset code. I started with calibration but I get a compilation error on stm32f411.

On the stm32duino forum I got this answer:


_min() required to compare two value of the same type. which is not the case here.

So simply cast them to fix the issue here:
https://github.com/jremington/MPU-9250-AHRS/blob/master/libs/I2Cdev.cpp#L276_

Error code:


C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp: In static member function 'static int8_t I2Cdev::readBytes(uint8_t, uint8_t, uint8_t, uint8_t*, uint16_t)':
C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:276:75: error: no matching function for call to 'min(uint8_t&, int)'
  276 |             for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
      |                                                                           ^
In file included from c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\algorithm:62,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring_constants.h:26,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring.h:34,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/Arduino.h:36,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.h:74,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:44:
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algo.h:3450:5: note: candidate: 'template<class _Tp, class _Compare> constexpr _Tp std::min(std::initializer_list<_Tp>, _Compare)'
 3450 |     min(initializer_list<_Tp> __l, _Compare __comp)
      |     ^~~
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algo.h:3450:5: note:   template argument deduction/substitution failed:
C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:276:75: note:   mismatched types 'std::initializer_list<_Tp>' and 'unsigned char'
  276 |             for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
      |                                                                           ^
In file included from c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\algorithm:62,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring_constants.h:26,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring.h:34,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/Arduino.h:36,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.h:74,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:44:
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algo.h:3444:5: note: candidate: 'template<class _Tp> constexpr _Tp std::min(std::initializer_list<_Tp>)'
 3444 |     min(initializer_list<_Tp> __l)
      |     ^~~
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algo.h:3444:5: note:   template argument deduction/substitution failed:
C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:276:75: note:   mismatched types 'std::initializer_list<_Tp>' and 'unsigned char'
  276 |             for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
      |                                                                           ^
In file included from c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\algorithm:61,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring_constants.h:26,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring.h:34,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/Arduino.h:36,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.h:74,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:44:
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algobase.h:246:5: note: candidate: 'template<class _Tp, class _Compare> constexpr const _Tp& std::min(const _Tp&, const _Tp&, _Compare)'
  246 |     min(const _Tp& __a, const _Tp& __b, _Compare __comp)
      |     ^~~
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algobase.h:246:5: note:   template argument deduction/substitution failed:
C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:276:75: note:   deduced conflicting types for parameter 'const _Tp' ('unsigned char' and 'int')
  276 |             for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
      |                                                                           ^
In file included from c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\algorithm:61,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring_constants.h:26,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/wiring.h:34,
                 from C:\Users\backu\AppData\Local\Arduino15\packages\STM32\hardware\stm32\1.9.0\cores\arduino/Arduino.h:36,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.h:74,
                 from C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:44:
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algobase.h:198:5: note: candidate: 'template<class _Tp> constexpr const _Tp& std::min(const _Tp&, const _Tp&)'
  198 |     min(const _Tp& __a, const _Tp& __b)
      |     ^~~
c:\users\backu\appdata\local\arduino15\packages\stm32\tools\xpack-arm-none-eabi-gcc\9.2.1-1.1\arm-none-eabi\include\c++\9.2.1\bits\stl_algobase.h:198:5: note:   template argument deduction/substitution failed:
C:\Users\backu\OneDrive\Dokumenty\Arduino\libraries\I2Cdev\I2Cdev.cpp:276:75: note:   deduced conflicting types for parameter 'const _Tp' ('unsigned char' and 'int')
  276 |             for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
      |                                                                           ^
exit status 1

How can I add a delay instead of using the millis() function?

EDIT: I am closing this issue because I am posting on the Arduino Forum because it seems more lively there. I don't want to cross-post and waste people's time. This issue has NOT been solved.

Sorry if the title was not clear, but I want to replace the following lines in MPU9250_Mahony.ino with a delay function that will eventually become a sleep function to allow the Arduino to go into deep sleep. I also need to use a delay function because I want to move servos, but not all of them at the same time, so as to avoid a brownout.

now_ms = millis(); //time to print?
if (now_ms - last_ms >= print_ms) {
last_ms = now_ms;

So from that to this:

delay(1000).

I have tried to just put the delay function there, but the values get all skewed, jumping back and forth. I understand that this phenomenon probably relates to how the sensor polls and how it calculates yaw, pitch, and roll based on the time between the readings. I understand that there is already a variable (deltat) that tracks the time between readings, but it doesn't seem to help in this operation.

Is this even feasible, or am I undermining the working principle of this sketch? I apologize in advance for my relatively low understanding of how this works. I also apologize that this might not be the ideal place to ask questions like this, I just couldn't find anywhere else.

If anyone can guide me in the right direction, that would be greatly appreciated.

Thanks!

Magneto output to Arduino Code

Hi everybody,

I am trying to use AHRS code. I run the calibration code and got the output and separated the values as mag and acc.

I run the magneto software and Hnorm as 10000 for acc and 100 for mag.

attached files are the output of the magneto software.

Which values should I enter to Arduino Code.

Thanks

Madgwick Quaternion Update gravity Compensate y and z direction wrong output

Hello jremington

I am currently developing a Gravity Compensate code using your explanation in the Arduino forum.
Thanks for it. It was a great discussion ever. I was clarified a lot of things from it

This my problem.
When I am getting gx, gy, and gz from bellow code

g[0] = 2 * (q[2] * q[0] - q[1] * q[3]);
g[1] = 2 * (q[1] * q[2] + q[3] * q[0]);
g[2] = q[1] * q[1] - q[2] * q[2] - q[3] * q[3] + q[0] * q[0];

When I rotating the MPU sensor gy and gz values change But gx value not change
I calibrate gyro and mag also

For example

a mean acceleration
g mean gravity

Finally, I want to do (a - g)

Initial Position   &          ax         ay           az          gx          gy          gz
No movement  &
Z up                           0         0            +1          0           0           -1
X down                        -1         0             0          1           0            0
Y down                         0        -1             0          0           1            0
Z down                         0         0            -1          0           0            1

// above values are correct exactly I want to get above values.
But I rotate the sensor that values change like below
 
But when I rotate clockwise gy and gz change
Y down & 
Rotate clockwise              0           -1           0           0             0          1

Z down &
Rotate clockwise              0            0          -1           0             1           0

I am using this for getting quaternion

MadgwickQuaternionUpdate(accel[0], accel[1], accel[2], gyro[0]*PI/180.0f, -gyro[1]*PI/180.0f, -gyro[2]*PI/180.0f, mag[1], -mag[0], -mag[2], deltat);

void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
{
  // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, _2bx, _2bz;
  float s1, s2, s3, s4;
  float qDot1, qDot2, qDot3, qDot4;

  // Auxiliary variables to avoid repeated arithmetic
  float _2q1mx;
  float _2q1my;
  float _2q1mz;
  float _2q2mx;
  float _4bx;
  float _4bz;
  float _2q1 = 2.0f * q1;
  float _2q2 = 2.0f * q2;
  float _2q3 = 2.0f * q3;
  float _2q4 = 2.0f * q4;
  float _2q1q3 = 2.0f * q1 * q3;
  float _2q3q4 = 2.0f * q3 * q4;
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;

  // Normalise accelerometer measurement
  norm = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;
  ax *= norm;
  ay *= norm;
  az *= norm;

  // Normalise magnetometer measurement
  norm = sqrt(mx * mx + my * my + mz * mz);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f / norm;
  mx *= norm;
  my *= norm;
  mz *= norm;
  
  // Reference direction of Earth's magnetic field
  _2q1mx = 2.0f * q1 * mx;
  _2q1my = 2.0f * q1 * my;
  _2q1mz = 2.0f * q1 * mz;
  _2q2mx = 2.0f * q2 * mx;
  hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 +
       _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
  hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
  _2bx = sqrt(hx * hx + hy * hy);
  _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
  _4bx = 2.0f * _2bx;
  _4bz = 2.0f * _2bz;

  // Gradient decent algorithm corrective step
  s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
  norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
  norm = 1.0f / norm;
  s1 *= norm;
  s2 *= norm;
  s3 *= norm;
  s4 *= norm;

  // Compute rate of change of quaternion
  qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
  qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
  qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
  qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

  // Integrate to yield quaternion
  q1 += qDot1 * deltatx;
  q2 += qDot2 * deltatx;
  q3 += qDot3 * deltatx;
  q4 += qDot4 * deltatx;
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

I try to resolve this by changing variable values and sing but it does not work for me.
Can you help me t do that?
Thank you

Is there a license?

Hey all,

We can't legally use this code until there is a license. Could a license be added, such as MIT?

Thanks,
Charlie

Doc Folder File "Instructions" refers to non-existent file MPU9250_AHRS.ino

Instructions in Doc folder state: "Copy the output of Magneto into either MPU9520_tiltcomp or MPU9250_AHRS". However, file MPU9250_AHRS does not exist in this repository. MPU9250_Mahony.ino exists, however it does not accept full Magneto output, instead takes input from MPU9250_cal.ino output. Is MPU9250_AHRS" file supposed to exist?

Wrong accelerometer data during Calibration

Hello jremington,
I'm having an issue with the data from calibration: it's working well for Magnetometer but Accelerometer seems "stuck".
Here is an extract from the calibration:

"Initializing I2C devices...
Testing device connections...
MPU9250 OK
Gyro bias collection ... KEEP SENSOR STILL
Done. Gyro offsets (raw) -240.2, 610.5, -557.3
Collecting 300 points for scaling, 3/second
TURN SENSOR VERY SLOWLY AND CAREFULLY IN 3D
32767 -32768 -32768 -15 21 62
32767 -32768 -32768 -17 21 61
32767 -32768 -32768 -15 20 60
32767 -32768 -32768 -16 21 63
32767 -32768 -32768 -14 17 62
32767 -32768 -32768 -10 15 62
32767 -32768 -32768 -6 13 62
32767 -32768 -32768 -6 12 61
32767 -32768 -32768 -5 13 64
32767 -32768 -32768 -2 11 62
32767 -32768 -32768 0 10 61
...
32767 -32768 -32768 21 72 -42
32767 -32768 -32768 22 63 -45
Done. rms Acc = 56849.78, rms Mag = 74.30"

Do you have any idea to solve this issue?

My setup: Arduino nano, SDA and SCL on A4 A5, 5V and grnd.

Many thanks for the libraries btw :)

Regards
Mixou

Mahony2 code doesn't compile

`Arduino: 1.8.19 (Windows 10), Board: "Arduino Pro or Pro Mini, ATmega328P (3.3V, 8 MHz)"

...\Desktop\MPU-9250-AHRS-master\Mahony2\Mahony2.ino: In function 'void MahonyQuaternionUpdate(float, float, float, float, float, float, float, float, float, float)':

Mahony2:272:1: error: 'qa' was not declared in this scope

qa = q1;

...\Desktop\MPU-9250-AHRS-master\Mahony2\Mahony2.ino:272:1: note: suggested alternative: 'pa'

qa = q1;

Mahony2:273:1: error: 'qb' was not declared in this scope

qb = q2;

...\Desktop\MPU-9250-AHRS-master\Mahony2\Mahony2.ino:273:1: note: suggested alternative: 'pb'

qb = q2;

Mahony2:274:1: error: 'qc' was not declared in this scope

qc = q3;

C:\Users\Gamze TEKIN\Desktop\MPU-9250-AHRS-master\Mahony2\Mahony2.ino:274:1: note: suggested alternative: 'pc'

qc = q3;

exit status 1

'qa' was not declared in this scope

This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.
`

Hello everybody,

When I try to upload Mahony2 code, i got the above error.

jremington / mpu-9250-ahrs Goto Github PK

mpu-9250-ahrs's People

Contributors

Stargazers

Watchers

Forkers

mpu-9250-ahrs's Issues

Compile error min() required

How can I add a delay instead of using the millis() function?

Magneto output to Arduino Code

Madgwick Quaternion Update gravity Compensate y and z direction wrong output

Is there a license?

Doc Folder File "Instructions" refers to non-existent file MPU9250_AHRS.ino

Wrong accelerometer data during Calibration

Mahony2 code doesn't compile

Recommend Projects

React

Vue.js

Typescript

TensorFlow

Django

Laravel

D3

Recommend Topics

javascript

web

server

Machine learning

Visualization

Game

Recommend Org

Facebook

Microsoft

Google

Alibaba

D3

Tencent