HMC5883L and QMC5883L (3 axis compass)

EDC · Joined: 26 Mar 2014 Posts: 1136

Hello. This code is for modules named GY-271 or GY-273
GY-271 have simple level translators where GY-273 is "simples/cheap made" or dedicated for 3V3

First I wrote code for HMC5883L and...code dont work for me Very Happy

After some investigation I read that those modules uses two chip manufacturers. So module can be equpped with QM.. or HM..

QM with I2C scanner respond at &H1A where HM should respond at &H3C (I dont have this chip)

So I wrote code for QM "from scratch" with datasheet and it finally work Very Happy

I attach both codes and datasheets for them. WIth this basic readings you can calculate more complex data.

albertsm · Joined: 09 Apr 2004 Posts: 6198 Location: Holland

you are very productive Very Happy

Thanks for sharing your work. Indeed a good idea to run the i2c scanner. That would be my advise too when i2c slave is not working as expected.
A big problem with lot of chips : there are many different versions which look similar. But we have EDC , so no problem Very Happy

micro_anne · Joined: 28 Nov 2014 Posts: 6

Hi,

congratulations on your awesome project.

I do have some questions for you and others who may be reading.

I tried in the past to get the MPU9250 9DOF working under bascom. I managed to get the I2C communication working, also reading the registers, writing new settings to the registry, and read the raw data was not a big problem.
What I couldn't do in Bascom was calculate the quaternions based on these raw data.

Calculating valid euler angles should be possible though I didn't find any successful attempt under bascom.

Would you, or anybody else, know how would that be done ?

Second question related to your shared work: Is there a big difference between using accelerometers and gyros to get the Yaw, Pitch, Roll angles and just using a 3 axis compass to accomplish the same thing ? Can I use this module to navigate without the assistance of any other inertial nav. plaforms ?

thanks again for sharing knowledge

EDC · Joined: 26 Mar 2014 Posts: 1136

MPU means that some of the calculations is made by MPU chip and they are ready to read for chip who want the data.
I dont remember exactly what MPU I use one day 6050 or so.
But more important is that any library/API can be translated to the Bascom.

To be honest and precise: You can drive any device that AVR can drive/use and write library for it by reading device datasheet.
But when one know second language because like learning then translate some of the done work/code is easy.

My point is that : "If you can find any working library for the chip then it can be easy translated"

For HMC i translate Arduino lib , for QMC I work on it from scratch with the datasheet (but with some basic knowledge and maybe nomenclature(naming of variables are same for code compatibility)).

Sensor give you data about X, Y and Z axis, but gyroMPU give you ready to use data about acceleration in the 3D directions and AVR dont need to calculate them. This is the difference between Sensor and the gyroMPU.

autoguider · Joined: 24 Sep 2007 Posts: 82 Location: Aachen

Dear All,
I was digging into the datasheets for the MCU9250 and got lost in the complexity of this device.
https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-9250-Register-Map.pdf
https://3cfeqx1hf82y3xcoull08ihx-wpengine.netdna-ssl.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf

I also found a library for Arduino written in C++ https://github.com/asukiaaa/MPU9250_asukiaaa
Cross referencing to the datasheets is a bit difficult to me.
I even could not find the I²c address for the device.
In the block diagram the magnetometer is not shown. So it could be that it has its own I²c address.
Later in the description it says about an auxiliary I²c bus which may contact a magnetometer.
So I am a bit confused.

My intention is to use a GY-91 10DOF device and get the tilt compensated compass heading and the tilt angle from it.
As I did my programmiong with bascom I would like to stay with it and not move to Arduino C.

If you have a recommendation for another suitable device please let me know.

best regards
Christian

hgrueneis · Joined: 04 Apr 2009 Posts: 906 Location: A-4786 Brunnenthal

Christian,

a tilt compensated Compass will not work right at 90 degrees tilt.
There are easier ways to determine the direction especially with a lot of iron around, if you have other means like 10-rot pots with calibration possibilities, evaluated by adc sense readings. Optical reading of an offset bar strip will also let you know the count forward and reverse. especially if you power your 'TOWER' rotation with servos. This would also enable you to control the positions of your telescopes to less than a second, which could be very useful in finding distant objects. A compass with Iron around can easily be off by 20 degrees!
I would not go with a compass, but just my thinking.

Regards
Hubert

Per Svensson · Joined: 03 Oct 2004 Posts: 238 Location: Gothenburg, Sweden

Regarding Inertial sensors.

I am now in the process of retiring myself after 30 years of electronic design. Smile

I write this as I for a very long time have been part of a design team where the target has been "The worlds best micro-mechanical Gyroscope". I am not saying this to brag, but to make you trust me when I say that Inertial sensors like Gyroscopes, Accelerometers and Magnetometers are inherently difficult to use for high accuracy angular measurements.
This is because they all have lots of limitations, but also that they are limited in the way they can reach physical measures.
Examples:
- Accelerometers are fine for calculating Roll and Pitch angles but are useless for horizontal angles as gravity is pointing down to the center of earth. So there is no direction info to pick up from an accelerometer.
- Gyroscopes are fine for finding angles in ANY direction. Also horizontal. But are useless over time as their readings tend to drift away over time. This is because they measure angular RATE rather than angles, so angles are calculated from Angle=Rate*time. This mean that if measured Rate is not exactly zero when real rate is zero, then we get a constantly growing angular error.
- Magnetometers are rather unreliable due to distortion from surrounding metals.

Having said this, all is not hopeless because there are methods to let these three sensor types cooperate in a way where their shortcomings are covered by their companions that do not suffer from the SAME shortcoming.

This was rather difficult until the Kalman filter was invented. Such a filter is based on successive (iterative) statistical calculations. You can consider it as a means of constantly judging which sensor that for the moment gives the most reliable output. This sensor will get a high rank whils the others are given less influence.
Some other ranking is also made. For instance:
As gyroscopes ar fast, but not so reliable over long time, they will receive high rank for fast rotations.
Magnetometers on the other hand will win over gyros in the long run as the have no drift.
Accelerometes will be very trustworthy for Roll and Pitch and out rule both magnetometers and gyros for all angles BUT horizontal rotations. (Azimuth)
If the Kalman filter is trimmed to balance all these judgements so that respective sensor's capabilities is used in an optimal way, then a true rotation around all three angles can be measured in a MUCH MUCH better way than if we simply use one isolated sensor type at a time.

The theory behind a KF is unfortunately rather hard to grasp, but the math is quite simple (and fast), so even a small AVR could do the calculations. I am by no means a KF expert but I recommend anyone interested in "sensor fusing" to start at Wikipedia.

Another very interesting algorithm for sensor fusing can be found here:
https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
for examples and more details)
https://github.com/TobiasSimon/MadgwickTests/blob/master/MadgwickAHRS.c

I tried an implementation of Magdwicks filter algorithms in Bascom, but it was no immediate success. I fear that it might have been that I missed some part of the porting from C.
The lack of parenthesis in math expressions complicates things and make expressions very lengthy and prone to human mistakes.
I someone is interested I can show my implementation of Magdwicks AHRS-filter here on the forum. It was written for Xmega128, but should run on any AVR big enough.

/Per

enniom · Joined: 20 Oct 2009 Posts: 548

Per Svensson · Joined: 03 Oct 2004 Posts: 238 Location: Gothenburg, Sweden

Hi Enniom,

Your suggested use of Excel for Bascom math expression evaluation is quite clever I must say.
However, when expressions become very complex even typing them the Bascom way is so tedious that
you really don't want to do it.
To give you an idea I have here below listed the code I wrote to mimic Sebastian Madgwick's code for an AHRS filter implementation some years ago. The original code is shown as comments

I never thought of comparing it in Excel, but feel free to do so Smile

...
If someone can make it work I am willing to give the code to this community for free, But you must publish it here.

/Per
--------------------------------------------------------------------------------------------------------
[code]
Sub Madgwick_quaternion_update(byval Ax As Single , Byval Ay As Single , Byval Az As Single , _
Byval Gx As Single , Byval Gy As Single , Byval Gz As Single , _
Byval Mx As Single , Byval My As Single , Byval Mz As Single , _
Quaternion() As Single , Byval Delta_t As Single , Byval Beta As Single)

' Implementation of Sebastian Madgwick' s "...efficient orientation filter for... inertial/magnetic sensor arrAys"
' (see http:' www.x-io.co.uk/category/open-source/ for examples and more details)
' and https://github.com/TobiasSimon/MadgwickTests/blob/master/MadgwickAHRS.c
' See also https://github.com/kriswiner/LSM9DS0/blob/master/LSM9DS0_MS5611_BasicAHRS_t3.ino
' which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of '
' absolute device orientation, which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
' The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
' but is much less computationally intensive.

' SUMMARY:
' For Definitions of Axis and rotations, see top of this file !
' Gyroscope Axis measurement in radians/s.
' Accelerometer Axis measurement in any calibrated units.
' Magnetometer Axis measurement in any calibrated units.
' Optimised for minimal arithmetics.
' CPU = 36561 cycles

' Axis directions
' --------------------------------------------------------------------------------------------------
' Madgwicks algorithm assumes that Acceleration is positive when device is
' In upright position. For instance, Z shall be positive when standing upright
' [This is opposite to definitions used by Monolitsystem.]

' It also assumes that Rotation is positive when device is clockwise rotated
' seen from device center. For instance, Z shall give positive signal when device is
' clockwise rotated on a horizontal surface
' This is in line with Monolitsystems definitions.

' Finally it need the following Magnetometer data:
' Assume that the vertical magnet vector is pointing down. (=Northen hemisphere)
' If the device is in its normal position with Z-Axis pointing down, then Magn_Z give negative output.
' This is opposite to Monolitsystems definitions.

Local _2bx As Single , _2bz As Single
Local S0 As Single , S1 As Single , S2 As Single , S3 As Single
Local Qdot0 As Single , Qdot1 As Single , Qdot2 As Single , Qdot3 As Single
Local Tmp1 As Single , Tmp2 As Single , Tmp3 As Single

' Short Name Local Variable For Readability
Q0 Alias Quaternion(0)
Q1 Alias Quaternion(1)
Q2 Alias Quaternion(2)
Q3 Alias Quaternion(3)

' Temporary variables to avoid repeated arithmetic
' Array A, B and C are temporary global variable (single), defined at the top of this file
' Make sure they are not used elswhere as non temporary variables, as this routine will destroy them.
Q0q0 Alias A(0) : Q0q0 = Q0 * Q0
Q0q1 Alias A(1) : Q0q1 = Q0 * Q1
Q0q2 Alias A(2) : Q0q2 = Q0 * Q2
Q0q3 Alias A(3) : Q0q3 = Q0 * Q3
Q1q1 Alias A(4) : Q1q1 = Q1 * Q1
Q1q2 Alias A(5) : Q1q2 = Q1 * Q2
Q1q3 Alias A(6) : Q1q3 = Q1 * Q3
Q2q2 Alias A(7) : Q2q2 = Q2 * Q2
Q2q3 Alias A( Cool

: Q2q3 = Q2 * Q3
Q3q3 Alias B(0) : Q3q3 = Q3 * Q3
_2q0 Alias B(1) : _2q0 = 2 * Q0
_2q1 Alias B(2) : _2q1 = 2 * Q1
_2q2 Alias B(3) : _2q2 = 2 * Q2
_2q3 Alias B(4) : _2q3 = 2 * Q3
_2q0mx Alias B(5) : _2q0mx = _2q0 * Mx
_2q0my Alias B(6) : _2q0my = _2q0 * My
_2q0mz Alias B(7) : _2q0mz = _2q0 * Mz
_2q1mx Alias B( Cool

: _2q1mx = _2q1 * Mx
_2q0q2 Alias C(0) : _2q0q2 = 2 * Q0q2
_2q2q3 Alias C(1) : _2q2q3 = 2 * Q2q3
Corr1 Alias C(2)
Corr2 Alias C(3)
Corr3 Alias C(4)
Corr4 Alias C(5)
Corr5 Alias C(6)
Corr6 Alias C(7)
Hx Alias C( Cool

Hy Alias C(9)
Norm Alias C(10)
_4bx Alias C(11)
_4bz Alias C(12)
_8bx Alias C(13)
_8bz Alias C(14)

' Compute rate of change of quaternion from gyro

' Qdot0 = -0.5 *(+q1*Gx + q2*Gy + q3*Gz)
Qdot0 = Q1 * Gx : Tmp1 = Q2 * Gy : Tmp2 = Q3 * Gz
Qdot0 = Qdot0 + Tmp1
Qdot0 = Qdot0 + Tmp2
' Shift Qdot0 , Right , 1 , Signed
Qdot0 = -0.5 * Qdot0

' Qdot1 = 0.5 *( q0*Gx + q2*Gz - q3*Gy)
Qdot1 = Q0 * Gx : Tmp1 = Q2 * Gz : Tmp2 = Q3 * Gy
Qdot1 = Qdot1 + Tmp1
Qdot1 = Qdot1 - Tmp2
' Shift Qdot1 , Right , 1 , Signed
Qdot1 = 0.5 * Qdot1

' Qdot2 = 0.5 *( q0*Gy - q1*Gz + q3*Gx)
Qdot2 = Q0 * Gy : Tmp1 = Q1 * Gz : Tmp2 = Q3 * Gx
Qdot2 = Qdot2 - Tmp1
Qdot2 = Qdot2 + Tmp2
' Shift Qdot2 , Right , 1 , Signed
Qdot2 = 0.5 * Qdot2

' Qdot3 = 0.5 *( q0*Gz + q1*Gy - q2*Gx)
Qdot3 = Q0 * Gz : Tmp1 = Q1 * Gy : Tmp2 = Q2 * Gx
Qdot3 = Qdot3 + Tmp1
Qdot3 = Qdot3 - Tmp2
' Shift Qdot3 , Right , 1 , Signed
Qdot3 = 0.5 * Qdot3

' Normalise accelerometer measurement
Norm = Vector_length_sing(ax , Ay , Az)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm ' use reciprocal for division
Ax = Ax * Norm
Ay = Ay * Norm
Az = Az * Norm

' Normalise magnetometer measurement
Norm = Vector_length_sing(mx , My , Mz)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm ' use reciprocal for division
Mx = Mx * Norm
My = My * Norm
Mz = Mz * Norm

' Reference direction of Earth' s magnetic field

' Hx = Mx*q0q0
' - _2q0My*q3
' + _2q0Mz*q2
' + Mx*q1q1
' + _2q1*My*q2
' + _2q1*Mz*q3
' - Mx*q2q2
' - Mx*q3q3;

Hx = Mx * Q0q0
Tmp1 = _2q0my * Q3 : Hx = Hx - Tmp1
Tmp1 = _2q0mz * Q2 : Hx = Hx + Tmp1
Tmp1 = Mx * Q1q1 : Hx = Hx + Tmp1
Tmp1 = _2q1 * My : Tmp1 = Tmp1 * Q2 : Hx = Hx + Tmp1
Tmp1 = _2q1 * Mz : Tmp1 = Tmp1 * Q3 : Hx = Hx + Tmp1
Tmp1 = Mx * Q2q2 : Hx = Hx - Tmp1
Tmp1 = Mx * Q3q3 : Hx = Hx - Tmp1

' Hy = _2q0Mx*q3
' + My*q0q0
' - _2q0Mz*q1
' + _2q1Mx*q2
' - My*q1q1
' + My*q2q2
' + Mz*_2q2*q3
' - My*q3q3;

Hy = _2q0mx * Q3
Tmp1 = My * Q0q0 : Hy = Hy + Tmp1
Tmp1 = _2q0mz * Q1 : Hy = Hy - Tmp1
Tmp1 = _2q1mx * Q2 : Hy = Hy + Tmp1
Tmp1 = My * Q1q1 : Hy = Hy - Tmp1
Tmp1 = My * Q2q2 : Hy = Hy + Tmp1
Tmp1 = Mz * _2q2 : Tmp1 = Tmp1 * Q3 : Hy = Hy + Tmp1
Tmp1 = My * Q3q3 : Hy = Hy - Tmp1

_2bx = Hx * Hx
Tmp1 = Hy * Hy
_2bx = _2bx + Tmp1
_2bx = Sqr(_2bx)

' _2bz = _2q1Mx*q3
' - _2q0Mx*q2
' + _2q0My*q1
' + Mz*q0q0
' - Mz*q1q1
' + My*_2q2*q3
' - Mz*q2q2
' + Mz*q3q3;

_2bz = _2q1mx * Q3
Tmp1 = _2q0mx * Q2 : _2bz = _2bz - Tmp1
Tmp1 = _2q0my * Q1 : _2bz = _2bz + Tmp1
Tmp1 = Mz * Q0q0 : _2bz = _2bz + Tmp1
Tmp1 = Mz * Q1q1 : _2bz = _2bz - Tmp1
Tmp1 = My * _2q2 : Tmp1 = Tmp1 * Q3 : _2bz = _2bz + Tmp1
Tmp1 = Mz * Q2q2 : _2bz = _2bz - Tmp1
Tmp1 = Mz * Q3q3 : _2bz = _2bz + Tmp1

_4bx = 2 * _2bx
_4bz = 2 * _2bz
_8bx = 2 * _4bx
_8bz = 2 * _4bz

' Gradient decent algorithm corrective step
' Magdwicks C-implementation (Including late fix at: http://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-sQuare-root?id=705844%3ATopic%3A1018435&page=4#comments )
' ---------- CORR1 ---- ------ CORR2 ------ ----------- CORR3 -------- --------------------- CORR4 ---------------------- ---------------------- CORR5 --------------- ----------------------- CORR6 --------------------
' S0 = -_2Q2*(2 *(Q1Q3 - Q0Q2) - Ax) + _2Q1*(2*(Q0Q1 + Q2Q3) - Ay) - _4bz*Q2*(_4bx*(0.5 - Q2Q2 - Q3Q3) + _4bz*(Q1Q3 - Q0Q2) - Mx) + (-_4bx*Q3 + _4bz*Q1) * (_4bx*(Q1Q2 - Q0Q3) + _4bz*(Q0Q1 + Q2Q3) - My) + _4bx*Q2 *(_4bx*(Q0Q2 + Q1Q3) + _4bz*(0.5 - Q1Q1 - Q2Q2) - Mz);
' S1= _2Q3*(2 *(Q1Q3 - Q0Q2) - Ax) + _2Q0*(2*(Q0Q1 + Q2Q3) - Ay) - 4*Q1 *(2*(0.5 - Q1Q1 - Q2Q2) - Az) + _4bz*Q3*(_4bx*(0.5 - Q2Q2 - Q3Q3) + _4bz*(Q1Q3 - Q0Q2) - Mx) + ( _4bx*Q2+_4bz*Q0) * (_4bx*(Q1Q2 - Q0Q3) + _4bz*(Q0Q1 + Q2Q3) - My) + (_4bx*Q3 -_8bz*Q1)*(_4bx*(Q0Q2 + Q1Q3) + _4bz*(0.5 - Q1Q1 - Q2Q2) - Mz);
' S2= -_2Q0*(2 *(Q1Q3 - Q0Q2) - Ax) + _2Q3*(2*(Q0Q1 + Q2Q3) - Ay) - 4*Q2 *(2*(0.5 - Q1Q1 - Q2Q2) - Az) + (-_8bx*Q2-_4bz*Q0)*(_4bx*(0.5 - Q2Q2 - Q3Q3) + _4bz*(Q1Q3 - Q0Q2) - Mx) + ( _4bx*Q1+_4bz*Q3) * (_4bx*(Q1Q2 - Q0Q3) + _4bz*(Q0Q1 + Q2Q3) - My) + (_4bx*Q0 -_8bz*Q2)*(_4bx*(Q0Q2 + Q1Q3) + _4bz*(0.5 - Q1Q1 - Q2Q2) - Mz);
' S3 = _2Q1*(2 *(Q1Q3 - Q0Q2) - Ax) + _2Q2*(2*(Q0Q1 + Q2Q3) - Ay) + (-_8bx*Q3 + _4bz*Q1)*(_4bx*(0.5 - Q2Q2 - Q3Q3) + _4bz*(Q1Q3 - Q0Q2) - Mx) + (-_4bx*Q0 + _4bz*Q2) * (_4bx*(Q1Q2 - Q0Q3) + _4bz*(Q0Q1 + Q2Q3) - My) + _4bx*Q1 *(_4bx*(Q0Q2 + Q1Q3) + _4bz*(0.5 - Q1Q1 - Q2Q2) - Mz);

' Substitution terms:
Corr1 = Q1q3 - Q0q2 : Corr1 = Corr1 * 2 : Corr1 = Corr1 - Ax ' 2*(Q1Q3 - Q0Q2) - Ax
Corr2 = Q0q1 + Q2q3 : Corr2 = Corr2 * 2 : Corr2 = Corr2 - Ay : Corr2 = Corr2 * _2q1 ' _2Q1*(2*(Q0Q1 + Q2Q3) - Ay)
Corr3 = 0.5 - Q1q1 : Corr3 = Corr3 - Q2q2 : Corr3 = Corr3 * 2 : Corr3 = Corr3 - Az ' 2*(0.5 - Q1Q1 - Q2Q2) - Az
Corr4 = 0.5 - Q2q2 : Corr4 = Corr4 - Q3q3 : Corr4 = Corr4 * _4bx : Tmp1 = Q1q3 - Q0q2 : Tmp1 = Tmp1 * _4bz : Corr4 = Corr4 + Tmp1 : Corr4 = Corr4 - Mx ' _4bx*(0.5 - Q2Q2 - Q3Q3) + _4bz*(Q1Q3 - Q0Q2) - Mx
Corr5 = Q1q2 - Q0q3 : Corr5 = Corr5 * _4bx : Tmp1 = Q0q1 + Q2q3 : Tmp1 = Tmp1 * _4bz : Corr5 = Corr5 + Tmp1 : Corr5 = Corr5 - My ' _4bx*(Q1Q2 - Q0Q3) + _4bz*(Q0Q1 + Q2Q3) - My
Corr6 = Q0q2 + Q1q3 : Corr6 = Corr6 * _4bx : Tmp1 = 0.5 - Q1q1 : Tmp1 = Tmp1 - Q2q2 : Tmp1 = Tmp1 * _4bz : Corr6 = Corr6 + Tmp1 : Corr6 = Corr6 - Mz ' _4bx*(Q0Q2 + Q1Q3) + _4bz*(0.5 - Q1Q1 - Q2Q2) - Mz

' S0 = -_2Q2*CORR1 + _2Q1*CORR2 - _4bz*Q2*CORR4 + (-_4bx*Q3 + _4bz*Q1)*CORR5 + _4bx*Q2*CORR6
Tmp1 = _2q2 * Corr1 : Tmp2 = _2q1 * Corr2 : S0 = Tmp2 - Tmp1
Tmp1 = _4bz * Q2 : Tmp1 = Tmp1 * Corr4 : S0 = S0 - Tmp1
Tmp1 = _4bx * Q3 : Tmp2 = _4bz * Q1 : Tmp1 = Tmp2 - Tmp1 : Tmp1 = Tmp1 * Corr5 : S0 = S0 + Tmp1
Tmp1 = _4bx * Q2 : Tmp1 = Tmp1 * Corr6 : S0 = S0 + Tmp1

' S1 = _2Q3*CORR1 + _2Q0*CORR2 - 4*Q1*CORR3 + _4bz*Q3 * CORR4 + ( _4bx*Q2+_4bz*Q0)*CORR5 + (_4bx*Q3 -_8bz*Q1)*CORR6
Tmp1 = _2q3 * Corr1 : Tmp2 = _2q0 * Corr2 : S1 = Tmp2 + Tmp1
Tmp1 = Q1 * Corr3 : Tmp1 = Tmp1 * 4 : S1 = S1 - Tmp1
Tmp1 = _4bz * Q3 : Tmp1 = Tmp1 * Corr4 : S1 = S1 + Tmp1
Tmp1 = _4bx * Q2 : Tmp2 = _4bz * Q0 : Tmp1 = Tmp2 + Tmp1 : Tmp1 = Tmp1 * Corr5 : S1 = S1 + Tmp1
Tmp1 = _4bx * Q3 : Tmp2 = _8bz * Q1 : Tmp1 = Tmp1 - Tmp2 : Tmp1 = Tmp1 * Corr6 : S1 = S1 + Tmp1

' S2= -_2Q0*CORR1 + _2Q3*CORR2 - 4*Q2*CORR3 + (-_8bx*Q2 - _4bz*Q0)*CORR4 + ( _4bx*Q1+_4bz*Q3)*CORR5 + (_4bx*Q0 -_8bz*Q2)*CORR6
Tmp1 = _2q0 * Corr1 : Tmp2 = _2q3 * Corr2 : S2 = Tmp2 - Tmp1
Tmp1 = Q2 * Corr3 : Tmp1 = Tmp1 * 4 : S2 = S2 - Tmp1
Tmp1 = _8bx * Q2 : Tmp2 = _4bz * Q0 : Tmp1 = Tmp1 + Tmp2 : Tmp1 = Tmp1 * Corr4 : S2 = S2 - Tmp1
Tmp1 = _4bx * Q1 : Tmp2 = _4bz * Q3 : Tmp1 = Tmp2 + Tmp1 : Tmp1 = Tmp1 * Corr5 : S2 = S2 + Tmp1
Tmp1 = _4bx * Q0 : Tmp2 = _8bz * Q2 : Tmp1 = Tmp1 - Tmp2 : Tmp1 = Tmp1 * Corr6 : S2 = S2 + Tmp1

' S3 = _2Q1*CORR1 + _2Q2*CORR2 + (-_8bx*Q3 + _4bz*Q1)*CORR4 + (-_4bx*Q0 + _4bz*Q2)*CORR5 + _4bx*Q1*CORR6
Tmp1 = _2q1 * Corr1 : Tmp2 = _2q2 * Corr2 : S3 = Tmp2 + Tmp1
Tmp1 = _8bx * Q3 : Tmp2 = _4bz * Q1 : Tmp1 = Tmp2 - Tmp1 : Tmp1 = Tmp1 * Corr4 : S3 = S3 + Tmp1
Tmp1 = _4bx * Q0 : Tmp2 = _4bz * Q2 : Tmp1 = Tmp2 - Tmp1 : Tmp1 = Tmp1 * Corr5 : S3 = S3 + Tmp1
Tmp1 = _4bx * Q1 : Tmp1 = Tmp1 * Corr6 : S3 = S3 + Tmp1

Norm = S0 * S0 : Tmp1 = S1 * S1 : Tmp2 = S2 * S2 : Tmp3 = S3 * S3
Norm = Norm + Tmp1 : Norm = Norm + Tmp2 : Norm = Norm + Tmp3
Norm = Sqr(norm)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm
S0 = S0 * Norm
S1 = S1 * Norm
S2 = S2 * Norm
S3 = S3 * Norm

' If P100 = 0 Then Print #buart_ext , _
' Hx ; " " ; Hy ; " " ; Tab ; _
' S0 ; " " ; S1 ; " " ; S2 ; " " ; S3 ; " " ; Tab ; _
' "[" ; Q0 ; " " ; Q1 ; " " ; Q2 ; " " ; Q3 ; "]"

' Apply feedback step
Tmp1 = Beta * S0 : Qdot0 = Qdot0 - Tmp1
Tmp1 = Beta * S1 : Qdot1 = Qdot1 - Tmp1
Tmp1 = Beta * S2 : Qdot2 = Qdot2 - Tmp1
Tmp1 = Beta * S3 : Qdot3 = Qdot3 - Tmp1

' Integrate to yield quaternion
Tmp1 = Qdot0 * Delta_t : Q0 = Q0 + Tmp1
Tmp1 = Qdot1 * Delta_t : Q1 = Q1 + Tmp1
Tmp1 = Qdot2 * Delta_t : Q2 = Q2 + Tmp1
Tmp1 = Qdot3 * Delta_t : Q3 = Q3 + Tmp1

' Norm = Sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3) ' normalise quaternion
Norm = Q0 * Q0 : Tmp1 = Q1 * Q1 : Tmp2 = Q2 * Q2 : Tmp3 = Q3 * Q3
Norm = Norm + Tmp1 : Norm = Norm + Tmp2 : Norm = Norm + Tmp3
Norm = Sqr(norm)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm
Q0 = Q0 * Norm
Q1 = Q1 * Norm
Q2 = Q2 * Norm
Q3 = Q3 * Norm

' Print #buart_ext , Ax ; ", " ; Ay ; ", " ; Az ; ", " ; Tab ;
' Print #buart_ext , Gx ; ", " ; Gy ; ", " ; Gz ; ", " ; Tab ;
' Print #buart_ext , Mx ; ", " ; My ; ", " ; Mz ; ", " ; Tab ;

' Print #buart_ext , "[" ; Q0 ; ", " ; Q1 ; ", " ; Q2 ; ", " ; Q3 ; "]"

End Sub
' ------------------------------------------------------------------------------------------------------

Sub Madgwick_mayhoney_update(byval Ax As Single , Byval Ay As Single , Byval Az As Single , _
Byval Gx As Single , Byval Gy As Single , Byval Gz As Single , _
Byval Mx As Single , Byval My As Single , Byval Mz As Single , _
Quaternion() As Single , Errint() As Single , Byval Delta_t As Single , Byval Kp As Single , Byval Ki As Single)

' AHRS.c
' S.O.H. Madgwick
' 25th August 2010
' www.varesano.net/blog/fabio/initial-implementation-9-domdof-marg-imu-orientation-filter-adxl345-itg3200-and-hmc5843-a
' =====================================================================================================
' Description:
'
' Quaternion implementation of the' DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
' compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
' direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
' axis only.
'
' User must define' half_t' as the (Delta_T / 2), and the filter gains' Kp' and' Ki' .
' Kp is typically 2.0 and Ki is 0.005
'
' Global variables' q0' ,' q1' ,' q2' ,' q3' are the quaternion elements representing the estimated
' orientation. See Magdwick report for an overview of the use of quaternions in this application.
'
' User must call' AHRSupdate()' every sample period and parse calibrated gyroscope (' gx' ,' gy' ,' gz' ),
' accelerometer (' ax' ,' ay' ,' ay' ) and magnetometer (' mx' ,' my' ,' mz' ) data. Gyroscope units are
' radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
'
' =====================================================================================================

' SUMMARY:
' For Definitions of Axis orientations, see top of this file !
' Gyroscope Axis measurement in radians/s.
' Accelerometer Axis measurement in any calibrated units.
' Magnetometer Axis measurement in any calibrated units.
' Optimised for minimal arithmetic.
' CPU = cycles

' Axis directions
' --------------------------------------------------------------------------------------------------
' Madgwicks_Mayhoneys algorithm assumes that Acceleration is positive when device is
' In upright position. For instance, Z shall be positive when standing upright.
' This is opposite to Monolitsystems definitions.

' It also assumes that Rotation is positive when device is clockwise rotated
' seen from device center. For instance, Z shall give positive signal when device is
' clockwise rotated on a horizontal surface
' This is in line with Monolitsystems definitions.

' Finally it need the following Magnetometer data:
' Assume that the vertical magnet vector is pointing down. (=Northen hemisphere)
' If the device is in its normal position with Z-Axis pointing down, then Magn_Z give negative output.
' This is opposite to Monolitsystems defections.

Local Tmp1 As Single , Tmp2 As Single , Tmp3 As Single

' Short Name Local Variable For Readability
Q0 Alias Quaternion(0) ' 1
Q1 Alias Quaternion(1) ' 0 Initial values
Q2 Alias Quaternion(2) ' 0
Q3 Alias Quaternion(3) ' 0

' Temporary variables to avoid repeated arithmetic
' Array A(0.. Cool

and C(0..17) are temporary global variables (single), defined at the top of this file
' Make sure they are not used elsewhere as non temporary variables, as this routine will destroy them.
Q0q0 Alias C(0) : Q0q0 = Q0 * Q0
Q0q1 Alias C(1) : Q0q1 = Q0 * Q1
Q0q2 Alias C(2) : Q0q2 = Q0 * Q2
Q0q3 Alias C(3) : Q0q3 = Q0 * Q3
Q1q1 Alias C(4) : Q1q1 = Q1 * Q1
Q1q2 Alias C(5) : Q1q2 = Q1 * Q2
Q1q3 Alias C(6) : Q1q3 = Q1 * Q3
Q2q2 Alias C(7) : Q2q2 = Q2 * Q2
Q2q3 Alias C( Cool

: Q2q3 = Q2 * Q3
Q3q3 Alias C(9) : Q3q3 = Q3 * Q3
Ex Alias C(10)
Ey Alias C(11)
Ez Alias C(12)
Hx Alias C(13)
Hy Alias C(14)
Hz Alias C(15)
Bx Alias C(16)
Bz Alias C(17)

Vx Alias A(0)
Vy Alias A(1)
Vz Alias A(2)
Wx Alias A(3)
Wy Alias A(4)
Wz Alias A(5)
Half_t Alias A(6) : Half_t = Delta_t * 0.5
Norm Alias A(7)

' Normalize accelerometer measurement
Norm = Vector_length_sing(ax , Ay , Az)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm ' use reciprocal for division
Ax = Ax * Norm
Ay = Ay * Norm
Az = Az * Norm

' Normalize magnetometer measurement
Norm = Vector_length_sing(mx , My , Mz)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm ' use reciprocal for division
Mx = Mx * Norm
My = My * Norm
Mz = Mz * Norm

' Reference direction of Earth' s magnetic field
' ---------------------------------------------

' Hx = 2 * Mx *(0.5 - Q2Q2 - Q3Q3) + 2 * My *(Q1Q2 - Q0Q3) + 2 * Mz *(Q1Q3 + Q0Q2);
Hx = 0.5 - Q2q2
Hx = Hx - Q3q3
Hx = Hx * Mx
Tmp1 = Q1q2 - Q0q3
Tmp1 = Tmp1 * My
Hx = Hx + Tmp1
Tmp1 = Q1q3 + Q0q2
Tmp1 = Tmp1 * Mz
Hx = Hx + Tmp1
Hx = Hx * 2

' Hy = 2 * Mx *(Q1Q2 + Q0Q3) + 2 * My *(0.5 - Q1Q1 - Q3Q3) + 2 * Mz *(Q2Q3 - Q0Q1);
Hy = Q1q2 + Q0q3
Hy = Hy * Mx
Tmp1 = 0.5 - Q1q1
Tmp1 = Tmp1 - Q3q3
Tmp1 = Tmp1 * My
Hy = Hy + Tmp1
Tmp1 = Q2q3 - Q0q1
Tmp1 = Tmp1 * Mz
Hy = Hy + Tmp1
Hy = Hy * 2

' Hz = 2 * Mx *(Q1Q3 - Q0Q2) + 2 * My *(Q2Q3 + Q0Q1) + 2 * Mz *(0.5 - Q1Q1 - Q2Q2);
Hz = Q1q3 - Q0q2
Hz = Hz * Mx
Tmp1 = Q2q3 + Q0q1
Tmp1 = Tmp1 * My
Hz = Hz + Tmp1
Tmp1 = 0.5 - Q1q1
Tmp1 = Tmp1 - Q2q2
Tmp1 = Tmp1 * Mz
Hz = Hz + Tmp1
Hz = Hz * 2

' Bx = SQrt((hx * Hx) +(hy * Hy));
Tmp1 = Hx * Hx
Tmp2 = Hy * Hy
Bx = Tmp1 + Tmp2
Bx = Sqr(bx)

Bz = Hz

' Estimated direction of gravity
' -------------------------------

' vx = 2*(Q1Q3 - Q0Q2);
Vx = Q1q3 - Q0q2
Vx = Vx * 2

' vy = 2*(Q0Q1 + Q2Q3);
Vy = Q0q1 + Q2q3
Vy = Vy * 2

' vz = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
Vz = Q0q0 - Q1q1
Vz = Vz - Q2q2
Vz = Vz + Q3q3

' Estimated direction of flux
' -----------------------------

' wx = 2*bx*(0.5 - Q2Q2 - Q3Q3) + 2*bz*(Q1Q3 - Q0Q2);
Wx = 0.5 - Q2q2
Wx = Wx - Q3q3
Wx = Wx * Bx
Tmp1 = Q1q3 - Q0q2
Tmp1 = Tmp1 * Bz
Wx = Wx + Tmp1
Wx = Wx * 2

' wy = 2*bx*(Q1Q2 - Q0Q3) + 2*bz*(Q0Q1 + Q2Q3);
Wy = Q1q2 - Q0q3
Wy = Wy * Bx
Tmp1 = Q0q1 + Q2q3
Tmp1 = Tmp1 * Bz
Wy = Wy + Tmp1
Wy = Wy * 2

' wz = 2*bx*(Q0Q2 + Q1Q3) + 2*bz*(0.5 - Q1Q1 - Q2Q2);
Wz = Q0q2 + Q1q3
Wz = Wz * Bx
Tmp1 = 0.5 - Q1q1
Tmp1 = Tmp1 - Q2q2
Tmp1 = Tmp1 * Bz
Wz = Wz + Tmp1
Wz = Wz * 2

' Error is sum of cross product between reference
' direction of fields and direction measured by sensors
' ---------------------------------------------------------

' Ex = (ay*vz - az*vy) + (my*wz - mz*wy);
Tmp1 = Ay * Vz
Tmp2 = Az * Vy
Ex = Tmp1 - Tmp2
Tmp1 = My * Wz
Tmp2 = Mz * Wy
Tmp2 = Tmp1 - Tmp2
Ex = Ex + Tmp2

' Ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
Tmp1 = Az * Vx
Tmp2 = Ax * Vz
Ey = Tmp1 - Tmp2
Tmp1 = Mz * Wx
Tmp2 = Mx * Wz
Tmp2 = Tmp1 - Tmp2
Ey = Ey + Tmp2

' Ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
Tmp1 = Ax * Vy
Tmp2 = Ay * Vx
Ez = Tmp1 - Tmp2
Tmp1 = Mx * Wy
Tmp2 = My * Wx
Tmp2 = Tmp1 - Tmp2
Ez = Ez + Tmp2

' Integral error scaled integral gain
' -----------------------------------

' ErrInt(0) = ErrInt(0) + ex*Ki;
Tmp1 = Ex * Ki
Errint(0) = Errint(0) + Tmp1

' ErrInt(1) = ErrInt(1) + ey*Ki;
Tmp1 = Ey * Ki
Errint(1) = Errint(1) + Tmp1

' ErrInt(2) = ErrInt(2) + ez*Ki;
Tmp1 = Ez * Ki
Errint(2) = Errint(2) + Tmp1

' Adjusted Gyroscope measurements
' -------------------------------

' Gx = Gx + Kp*ex + ErrInt(0);
Tmp1 = Kp * Ex
Gx = Gx + Tmp1
Gx = Gx + Errint(0)

' Gy = Gy + Kp*ey + ErrInt(1);
Tmp1 = Kp * Ey
Gy = Gy + Tmp1
Gy = Gy + Errint(1)

' Gz = Gz + Kp*ez + ErrInt(2);
Tmp1 = Kp * Ez
Gz = Gz + Tmp1
Gz = Gz + Errint(2)

' Integrate Quaternion Rate
' --------------------------

' Q0 = Q0 + (-Q1*Gx - Q2*Gy - Q3*Gz)*half_t;
Tmp1 = Q1 * Gx
Tmp2 = Q2 * Gy
Tmp1 = Tmp1 + Tmp2
Tmp2 = Q3 * Gz
Tmp1 = Tmp1 + Tmp2
Tmp1 = Tmp1 * Half_t
Q0 = Q0 - Tmp1

' Q1 = Q1 + (Q0*Gx + Q2*Gz - Q3*Gy)*half_t;
Tmp1 = Q0 * Gx
Tmp2 = Q2 * Gz
Tmp1 = Tmp1 + Tmp2
Tmp2 = Q3 * Gy
Tmp1 = Tmp1 - Tmp2
Tmp1 = Tmp1 * Half_t
Q1 = Q1 + Tmp1

' Q2 = Q2 + (Q0*Gy - Q1*Gz + Q3*Gx)*half_t;
Tmp1 = Q0 * Gy
Tmp2 = Q1 * Gz
Tmp1 = Tmp1 - Tmp2
Tmp2 = Q3 * Gx
Tmp1 = Tmp1 + Tmp2
Tmp1 = Tmp1 * Half_t
Q2 = Q2 + Tmp1

' Q3 = Q3 + (Q0*Gz + Q1*Gy - Q2*Gx)*half_t;
Tmp1 = Q0 * Gz
Tmp2 = Q1 * Gy
Tmp1 = Tmp1 + Tmp2
Tmp2 = Q2 * Gx
Tmp1 = Tmp1 - Tmp2
Tmp1 = Tmp1 * Half_t
Q3 = Q3 + Tmp1

' If P100 = 0 Then Print #buart_ext , _
' Hx ; " " ; Hy ; " " ; Tab ; _
' S0 ; " " ; S1 ; " " ; S2 ; " " ; S3 ; " " ; Tab ; _
' "[" ; Q0 ; " " ; Q1 ; " " ; Q2 ; " " ; Q3 ; "]"

' normalize Quaternion
' ---------------------

' Norm = SQrt(Q0*Q0 + Q1*Q1 + Q2*Q2 + Q3*Q3)
Norm = Q0 * Q0 : Tmp1 = Q1 * Q1 : Tmp2 = Q2 * Q2 : Tmp3 = Q3 * Q3
Norm = Norm + Tmp1 : Norm = Norm + Tmp2 : Norm = Norm + Tmp3
Norm = Sqr(norm)
If Norm = 0 Then Exit Sub ' handle NaN
Norm = 1 / Norm
Q0 = Q0 * Norm
Q1 = Q1 * Norm
Q2 = Q2 * Norm
Q3 = Q3 * Norm

' Print #buart_ext , Ax ; ", " ; Ay ; ", " ; Az ; ", " ; Tab ;
' Print #buart_ext , Gx ; ", " ; Gy ; ", " ; Gz ; ", " ; Tab ;
' Print #buart_ext , Mx ; ", " ; My ; ", " ; Mz ; ", " ; Tab ;
' Print #buart_ext , "[" ; Errint(0) ; ", " ; Errint(1) ; ", " ; Errint(2) ; "] , [" ; Q0 ; ", " ; Q1 ; ", " ; Q2 ; ", " ; Q3 ; "]"

End Sub
' ------------------------------------------------------------------------------------------------------

Sub Quaternion2taitbryan(q() As Single , Roll As Single , Pitch As Single , Yaw As Single)
' Convert quaternion to Tait-Bryan angles, (Roll, Pitch, Yaw).
' Angles are returned in RADIANS
' In this coordinate system, the positive z-axis is down toward Earth.
' Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local
' declination), looking down on the sensor positive yaw is counterclockwise.
' Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive,
' up toward the sky is negative.
' Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
' These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
' Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct
' orientation the rotations must be applied in the correct order which for this configuration is
' yaw, pitch, and then roll.
' For More See Http://http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
' or
' http://www.vectornav.com/docs/default-source/documentation/vn-100-documentation/AN002.pdf?sfvrsn=10

Local Tmp1 As Single
Local Tmp2 As Single
Local Q0q0 As Single
Local Q1q1 As Single
Local Q2q2 As Single
Local Q3q3 As Single

Q0q0 = Q(0) * Q(0)
Q1q1 = Q(1) * Q(1)
Q2q2 = Q(2) * Q(2)
Q3q3 = Q(3) * Q(3)

' Roll = Atan2(2*(Q0*Q1 + Q2*Q3) , Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3);
Tmp1 = Q(0) * Q(1)
Tmp2 = Q(2) * Q(3)
Tmp1 = Tmp1 + Tmp2
Tmp1 = 2 * Tmp1
Tmp2 = Q0q0 - Q1q1
Tmp2 = Tmp2 - Q2q2
Tmp2 = Tmp2 + Q3q3
Roll = Atn2(tmp1 , Tmp2)

' Pitch = -asin(2*(Q1*Q3 - Q0*Q2));
Tmp1 = Q(1) * Q(3)
Tmp2 = Q(0) * Q(2)
Tmp1 = Tmp1 - Tmp2
Tmp1 = 2 * Tmp1
Pitch = 0 - Asin(tmp1)

' Yaw = Atan2(2*(Q1*Q2 + Q0*Q3) , Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3);
Tmp1 = Q(1) * Q(2)
Tmp2 = Q(0) * Q(3)
Tmp1 = Tmp1 + Tmp2
Tmp1 = 2 * Tmp1
Tmp2 = Q0q0 + Q1q1
Tmp2 = Tmp2 - Q2q2
Tmp2 = Tmp2 - Q3q3
Yaw = Atn2(tmp1 , Tmp2)

End Sub
' ------------------------------------------------------------------------------------------------------

Sub Quaternion2euler(q() As Single , Roll As Single , Pitch As Single , Yaw As Single)
' Convert quaternion to Euler angles, (Yaw, Pitch, Roll).
' Angles are returned in RADIANS
' The formulas below are from the original document: "An efficient orientation filter for inertial and
' inertial/magnetic sensor arrays" by Sebastian O.H. Madgwick April 30 , 2010

' In this coordinate system, the positive z-Axis is down toward Earth.
' Yaw is the angle between Sensor x-Axis and Earth magnetic North (or true North if corrected for local
' declination), looking down on the sensor positive yaw is counterclockwise.
' Pitch is angle between sensor x-Axis and Earth ground plane, toward the Earth is positive,
' up toward the sky is negative.
' Roll is angle between sensor y-Axis and Earth ground plane, y-Axis up is positive roll.
' These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
' Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct
' orientation the rotations must be applied in the correct order which for this configuration is
' yaw, pitch, and then roll.
' For More See Http://http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

Local Tmp1 As Single
Local Tmp2 As Single

' Yaw = atan2(q[1]*q[2] - q[0]*q[3] , q[0]*q[0] + q[1]*q[1] - 0.5);
Tmp1 = Q(1) * Q(2)
Tmp2 = Q(0) * Q(3)
Yaw = Tmp1 - Tmp2
Tmp1 = Q(0) * Q(0)
Tmp2 = Q(1) * Q(1)
Tmp1 = Tmp1 + Tmp2
Tmp1 = Tmp1 - 0.5
Yaw = Atn2(yaw , Tmp1)

' Pitch = -Asin(2 *( q[1]*q[3] + q[0]*q[2]));
Tmp1 = Q(1) * Q(3)
Tmp2 = Q(0) * Q(2)
Tmp1 = Tmp1 + Tmp2
Tmp1 = Tmp1 * 2
Pitch = 0 - Asin(tmp1)

' Roll = atan2(q[2]*q[3] - q[0]*q[1] , q[0]*q[0] + q[3]*q[3] - 0.5);
Tmp1 = Q(2) * Q(3)
Tmp2 = Q(0) * Q(1)
Roll = Tmp1 - Tmp2
Tmp1 = Q(0) * Q(0)
Tmp2 = Q(3) * Q(3)
Tmp1 = Tmp1 + Tmp2
Tmp1 = Tmp1 - 0.5
Roll = Atn2(roll , Tmp1)

End Sub

' ------------------------------------------------------------------------------------------------------

[/code]

albertsm · Joined: 09 Apr 2004 Posts: 6198 Location: Holland

this is the SHARE your working code forum.
it seems because of the [] in the code the code tags will not work properly.
Large code is better posted as attachment.