CaptainBoing
Guru
Joined: 07/09/2016
Location: United KingdomPosts: 1993 |
| Posted: 07:43am 20 Aug 2017 |
 Copy link to clipboard |
 Print this post |
|
Hello forum but especially matherp.
I can't seem to get any info from the SENSORFUSION statement in MMX Basic.
I am using Pete Mather's AHRS code which has the MadgwickQuaternionUpdate function written in basic - which works really nicely and because it is MMX it happens in the twinkle of an eye.
I thought I would use the SENSORFUSION statement as an improvement - why not have the system call do it? Speed improvement and more compact code
So I added the section to the GetMPUDat sub but I don't get any useful info appearing in the Yaw,Pitch,Roll variables and it doesn't change with the attitude of the xyz sensor.
Any ideas anyone?
Here is the working code (ignore my manadj variables):
Sub GetMPUDat
deltat=timer/1000:timer=0
readMPU9250(60)
'SENSORFUSION type ax, ay, az, gx, gy, gz, mx, my, mz, pitch, roll, yaw [,p1] [,p2]
'SENSORFUSION MADGWICK ax, ay, az, gx, gy, gz, mx, my, -mz, pitch, roll, yaw
MadgwickQuaternionUpdate ax, ay, az, gx, gy, gz, my, mx, -mz
yaw = DEG(atan2(2.0 * (q(1) * q(2) + q(0) * q(3)), q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3)))
pitch = PManAdj + DEG(-asin(2.0 * (q(1) * q(3) - q(0) * q(2))))
roll = RManAdj + DEG(atan2(2.0 * (q(0) * q(1) + q(2) * q(3)), q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3)))
...
and the modified code:
Sub GetMPUDat
deltat=timer/1000:timer=0
readMPU9250(60)
'SENSORFUSION type ax, ay, az, gx, gy, gz, mx, my, mz, pitch, roll, yaw [,p1] [,p2]
SENSORFUSION MADGWICK ax, ay, az, gx, gy, gz, mx, my, -mz, pitch, roll, yaw
'MadgwickQuaternionUpdate ax, ay, az, gx, gy, gz, my, mx, -mz
'yaw = DEG(atan2(2.0 * (q(1) * q(2) + q(0) * q(3)), q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3)))
'pitch = PManAdj + DEG(-asin(2.0 * (q(1) * q(3) - q(0) * q(2))))
'roll = RManAdj + DEG(atan2(2.0 * (q(0) * q(1) + q(2) * q(3)), q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3)))
...
just gives three zeroes (with occasional +/- 1 or 2 if I shake the board
0
0
0
bit lost really... the maths is way over my head so any help gratefully received.
h
Edited by CaptainBoing 2017-08-21 |
Notice. New forum software under development. It's going to miss a few functions and look a bit ugly for a while, but I'm working on it full time now as the old forum was too unstable. Couple days, all good. If you notice any issues, please contact me.