How to code this in spin?
simonl
Posts: 866
Hi folks,
Sorry if this isn't the right place to post this (happy for it to be moved to a more appropriate place).
I've got the following formulae that I need to code in Spin (maybe PASM later), but I'm not a math' guru and don't understand it! Are there any math & Spin guru's out there that would be kind enough to give me the clues on how this should be coded please?
phi = -atan2( ay, -az );
theta = -asin( ax, |a| );
(I'm particularly interested in what |a| means!)
Thanks in advance.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
BTW: I type as I'm thinking, so please don't take any offense at my writing style
www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
Sorry if this isn't the right place to post this (happy for it to be moved to a more appropriate place).
I've got the following formulae that I need to code in Spin (maybe PASM later), but I'm not a math' guru and don't understand it! Are there any math & Spin guru's out there that would be kind enough to give me the clues on how this should be coded please?
phi = -atan2( ay, -az );
theta = -asin( ax, |a| );
(I'm particularly interested in what |a| means!)
Thanks in advance.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
BTW: I type as I'm thinking, so please don't take any offense at my writing style
www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
Comments
but, take this with a grain of salt, i could be wrong... i am just a college drop out after all :-P
it kinda looks like youre dealing with 3 dimensional plotting with some trig and calc that i am most definitely rusty on. sorry i cant help more
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
I know just enough to tinker with something... but not enough to fix it when I break it. Thank the techie gods theres forums...
Thanks for the info' -- I never got that far with my maths!
The formulae are sitting inside some C code for a UAV -- my dream is to use a Propeller at the heart of a helicopter UAV, so I can build a Sea King and give it a rock-solid hover (hey, I like dreaming LOL)
BTW: Love your sig line [noparse]:)[/noparse]
OK, that just leaves how to code the atan2 and asin functions; I'm pretty sure that in Spin asin only takes one parameter...
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
BTW: I type as I'm thinking, so please don't take any offense at my writing style
www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
Here's what I've got:
---[noparse][[/noparse]start]
An strapdown AHRS outputs the roll (phi), pitch (theta) and heading (psi) angles of the body relative to the local tangent plane, as well as the body frame angular rates.
In an unaccelerated reference frame, such as a stationary object, the pitch and roll attitude of the body can be estimated from the body frame tri-axial accelerometers alone. The formula for estimating the angles is:
phi = -atan2( ay, -az );
theta = -asin( ax, |a| );
However, a simple inertial measurement unit is insufficient for estimating the attitude if the body is subject to any significant accelerations (such as a banked turn) since the accelerometers will measure the forces other than gravity. In a 60 degree coordinated bank, for instance, the body is subject to a net force of 2 G straight down through the floor. Meanwhile, the ax and ay sensors will be reading perfectly level, leading to an zero pitch and roll angles.
To remedy this, the body frame angular rate sensors can be used to compensate for short term divergence from nominal 1 G reference frames. When the body is near level, the body frame p and q angular rates directly correspond to the pitch and roll rates.
Due to discontinuities in the Euler angles, most AHRS internaly use a quaternion representation. Quaternions are an alternative way of representing orientation that has no discontinuities. To update the quatnerion estimate based on the body frame angular rates is much easier.
q+ = Wxq( quatW( prq ) * q
---[noparse][[/noparse]end]
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
BTW: I type as I'm thinking, so please don't take any offense at my writing style
www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
|a| is probably the length of the three dimensional vector (hypotenuse) defined by the three sides,
|a| = SQR(ax^2 + bx^2 + cx^2)
Not being familar with the problem, it might also be one of the two dimensional hyponenuses.
ASIN(ax,|a|) is the angle subtented by side ax and hyponenuse |a|. That is, the angle for which SIN(theta) = ax / |a|.
ATAN(ay,-az) is the angle subtended in a triangle whose adjacent and opposite sides are -az and ay. That is, the angle for which TAN(phi)=-az/ay. I may have the adjacent and opposite sides reversed. It depends on how the function parameters are defined in the language syntax.
You don't necessarily need both an ASIN and an ATAN function. They are related by geometry. So for example,
theta = ASIN(ax, |a|) = ATAN(ay,SQR(ax^2+az^2)) <-- don't quote me on this!
off the cuff it's something like that, if |a| is the three dimensional hypontenuse.
The CORDIC algorithms were in fact first developed for in flight navigation problems like this. Graham posted a CORDIC asm implementation and tutorial in an earlier thread, CORDIC documentation.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com
The problem is that it is in assembly and needs to be in order to be fast, you could program it up so it sits in a cog and provides functions for Asine and Atan or put all the accelerometer jiggery pokery in one assembly cog.
However that is Maths and assembly in one lump!
Graham
p.s. Yes |a| means the absolute signless value of a.
Thanks again.
BTW: Anyone know how a kalman filter works? All the IMU stuff I've read uses one -- it's something to do with taking all the (noisy) data from sensors and 'fusing' it together to get an estimate of attitude, but no matter how much I read I'm still confused (Math never was my strong point!)
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
BTW: I type as I'm thinking, so please don't take any offense at my writing style
www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
As luck would have it, I'm writing a kalman filter for the propeller myself.
This thread on the Sparkfun forums has a lot of good info, several good articles, and well-commented C examples.
The hardest part IMHO is deciding between doing it with floats internally, with an external FPU, or with fixed point math. At the moment, I'm tending towards the second option, because it would save me a bunch of hassle, but eventually I plan to move to a propeller-only solution.
Compare the explanation on the first page of the cordic thread with the second page of the attached spreadsheet (add xls extension).
Cordic is actually very simple to code, you just have a set of angles and on each step you either add or subtract the next angle from the set (they get smaller and smaller a bit like binary bits). The add/subtract decision is made by looking at the result of the angle change on the sign of o (the opposite side of the triangle), this is made possible by clever choice of the angles.
The algorithms are really trivial its just the math that is hard to explain even though it is also not all that difficult really.
Lets put it this way, when was the last time you could put an algorithm in a spreadsheet and the only thing you needed to do that was abnormal was use an if statement!
Graham
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,
Simon
BTW: I type as I'm thinking, so please don't take any offense at my writing style
www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)