 |
 |
| View previous topic :: View next topic |
| Author |
Message |
curt2go
Joined: 21 Nov 2003
Posts: 200
|
| ATAN2 calculation speed. |
 Posted: Thu Oct 13, 2016 11:53 am |
 |
|
I am wondering if there is an optimisation for ATAN2 calculations. This is still for the same project I just did not want to bog that other thread down with a different but sort of related question. I obviously need to keep my processing time down even though I'm using a 24EP256GP206 at 140MHz. Code optimisation is always good.
Here is an optimisation of ATAN I have not tried it yet but I do need ATAN2.
Also I believe the squareRoot of the 2 squared values is just to normalise them. Is there a faster way for that or is that code not too bad?
Thanx for any help. As always.
| Code: |
////////////////// here are my ATAN2 calcs//////////////////////
squareRootThis = sqrt((Az2ed) + (Ay2ed));
roll = (atan2(squareRootThis,Ax)) * 57.29;
squareRootThis = sqrt((Ax2ed) + (Az2ed));
pitch = (atan2(Ay,squareRootThis)) * 57.29;
squareRootThis = sqrt((Ax2ed) + (Az2ed));
yaw = (atan2(Az,squareRootThis)) * 57.29;
|
This is Ttelmah's code on ATAN below. Optomized.
| Code: |
float angle(float X, float Y)
{
//routine to give a fast solution for angle, from X/Y co-ordinates - result in degrees
float AX,AY,ival,oval,aival;
int8 quad;
AX=fabs(X);
AY=fabs(Y);
//Now the approximation used works for tan from -1 to 1, so we have to keep the
//values inside this range and adjust the input/output.
//Four 'quadrants' are decoded -1 to 1, (315 to 45 degrees), then 45 to 135,
//135 to 225, 225 to 315
If (X >= 0) //Right hand half of the circle
{
If (AY > X)
{
If (Y < 0)
{
quad = 4;
ival = -X / Y;
}
Else
{
quad = 2;
ival = X / -Y;
}
}
Else
{
If (AY > X)
{
quad = 4;
ival = -Y / X;
}
else
{
quad = 1;
ival = Y / X;
}
}
}
else
{
//Now the others
If (Y > AX)
{
quad = 2;
ival = X / -Y;
}
Else
{
If (AY > AX)
{
quad = 4;
ival = -X / Y;
}
Else
{
quad = 3;
ival = -Y / -X;
}
}
}
//A lot of lines of code, but small and quick really.....
//Now the solution
//Now approximation for atan from -1 to +1, giving an answer in degrees.
aival = fAbs(ival);
oval = 45 * ival - ival * (aival - 1) * (14.02 + 3.79 * aival);
//Now solve back to the final result
If (quad != 1)
{
If (quad == 2)
oval = oval + 90;
Else
{
If (quad == 3)
oval = oval + 180;
Else
oval = oval + 270;
}
}
if (oval<0)
oval+=360;
return oval;
} |
|
|
 |
Ttelmah
Joined: 11 Mar 2010
Posts: 19326
|
|
| Posted: Thu Oct 13, 2016 12:09 pm |
|
|
If you look at my code, it is for atan2. X/Y. You can use it for tan, by solving X/Y first, but it is written to perform atan2 (and as atan2 should do, it traps the illegal points).
Key thing about all the current things about gyros, is that the fetching of the data needs to be synchronised to what the gyro chips are doing. Retrieving the data when these say they have a reading. There is a little flexibility provided by the chip buffering, but the basic reads must all occur before the internal data is overwritten.
Then this data needs to be combined/integrated to provide cumulative movements up to a point in time. The 'processing' code then needs to snapshot these movements at a point in time, and then perform the maths on these, while the fetching is still progressing in the background. |
|
|
newguy
Joined: 24 Jun 2004
Posts: 1903
|
|
| Posted: Thu Oct 13, 2016 12:11 pm |
|
|
I avoid floats like the plague. I needed a sqrt function for unsigned 32 bit ints. It's quick. Enjoy.
| Code: | unsigned int32 my_sqrt(unsigned int32 n) {
unsigned int32 root = 0;
unsigned int32 bit, trial;
if (n >= 0x10000) {
bit = 0x40000000;
}
else {
bit = 0x4000;
}
do {
trial = root + bit;
if (n >= trial) {
n -= trial;
root = trial + bit;
}
root >>= 1;
bit >>= 2;
} while (bit != 0);
return (root);
} |
|
|
|
curt2go
Joined: 21 Nov 2003
Posts: 200
|
|
| Posted: Thu Oct 13, 2016 12:23 pm |
|
|
Thanx guys. I will give them both a shot later.
On the sample speed note I will be changing to SPI of 20MHz to the XL, Gyro chip because I2C is just way too slow at 400KHz (at fast speed)
Ttelmah i am not that good at the trig functions so I did not notice. Thank you as always for the help and guidance. |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19326
|
|
| Posted: Fri Oct 14, 2016 3:32 am |
|
|
| Ttelmah wrote: | If you look at my code, it is for atan2. X/Y. You can use it for tan, by solving X/Y first, but it is written to perform atan2 (and as atan2 should do, it traps the illegal points).
Key thing about all the current things about gyros, is that the fetching of the data needs to be synchronised to what the gyro chips are doing. Retrieving the data when these say they have a reading. There is a little flexibility provided by the chip buffering, but the basic reads must all occur before the internal data is overwritten.
Then this data needs to be combined/integrated to provide cumulative movements up to a point in time. The 'processing' code then needs to snapshot these movements at a point in time, and then perform the maths on these, while the fetching is still progressing in the background. |
Let me just add a tiny bit to this. It actually depends on what you want to do with the data.
You can use just a sample at a point in time, omitting some of the values, from the acceleration/rotation sensors, for a job like a mouse equivalent movement, so you move a wand, and when this is rotated the object on the screen rotates with this rotation. It doesn't matter here if you miss part of the movement data, so long as when you rotate in a particular direction, the object tracks, and when you stop it stops. It won't matter if the actual rotation amounts do not match. However conversely, you can't afford to miss any samples, if you are trying to actually match/record a physical movement. So if you want the object to turn through the same angle as you move, then every single return from the sensor has to be recorded. The rotation sensor, actually returns a differential of a rotation.
Now, with the full data recorded, there will still be a significant 'creep'. The sensors will often return small rotations when they are sitting still, or equally may omit rotations when they are moved slowly. This is why you normally need some other 'reference' to keep the zero points aligned. So systems using these gyroscopes will commonly use perhaps a magnetic sensor to give a reference that is used to re-index the X-Y origin point when the sensors are still, and also use the acceleration sensors to detect where is 'down' again when otherwise still, to provide the vertical (Z) reference.
It's far simpler to do the maths, when you 'separate' it. Either running a routine synchronised by the interrupt from the sensor, independant of the actual 'position' calculation, or (probably simpler and better), use a separate PIC to perform this sampling. So you have a PIC reading the sensor, with the interrupt from the sensor set to trigger when there are a small number of readings present, and have this PIC integrate the signals to give a 'sum to now' value for each, then a separate PIC taking these sums, and performing the conversion maths, and zero offsetting when an index is available.
For the gyros, you have sensor rate of angle change. Integrate this and you have angle. For the accelerometer, you have rate of change of velocity. Integrate this, and you have velocity. Integrate again, with angle included, and you have position.
Quaternions can provide a useful tool in performing the integrations (dual quaternions for the final sum).
Now, you need at all points to be aware of just how much maths is involved. If you look at processors offering code to do this, they are typically something like 300MHz+ processors, often with hardware fp. The PIC (particularly chips like the PIC24), are capable of doing the work, but as curt2go is looking at, with great care in the routines used, and preferably using possibly DMA to handle the actual readings. You cannot 'take a reading, stop, do the maths', and expect to get properly working results.....  |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
