Gyroscope, theoretical discussion
- Thread starter edmunds
- Start date
Dear all,
Here is an idea.
Looking at magnetometer data output, I cannot help but wonder if I need to convert anything to degrees at all. I do not need absolute North, so while I would like the magnetometer to be calibrated correctly, this is not for finding the true North, but for correct magnitude of deviation from zero on all axes.
And really, for the navigation purposes, I do not have any coordinate system and I might not need one. The only thing I'm interested in is the magnitude of deviation for every step of 0.9mm of forward motion. I'm not even interested in trigonometry, because the distances and angles are so small and I'm not interested in deviation from the line in millimetres, because there is nothing special I can do with this value. I'm sure, the magnetometer deviation can be considered proportional to the distance of the line in one or the other direction and just scaled to become an error that the PD algorithm can deal with.
While this is prone to error accumulation with every step, so would be a gyro/accelerometer based approach or even more so. Now it remains to be seen, if the magnetometer has enough resolution and not too much noise for the deviation magnitude I'm trying to judge. Just in case I have one with 10 times the sensitivity of LSM303AGR
.
Need to enjoy some Saturday night now, so lets see if I get anything more done today, but the route seems pretty straight forward to the next milestone - put the magnetometer in the car along with external EEPROM and record a few laps of my test track and then blurt it out to Excel to see if there is any pattern in the data to be recognised.
Edmunds
Here is an idea.
Looking at magnetometer data output, I cannot help but wonder if I need to convert anything to degrees at all. I do not need absolute North, so while I would like the magnetometer to be calibrated correctly, this is not for finding the true North, but for correct magnitude of deviation from zero on all axes.
And really, for the navigation purposes, I do not have any coordinate system and I might not need one. The only thing I'm interested in is the magnitude of deviation for every step of 0.9mm of forward motion. I'm not even interested in trigonometry, because the distances and angles are so small and I'm not interested in deviation from the line in millimetres, because there is nothing special I can do with this value. I'm sure, the magnetometer deviation can be considered proportional to the distance of the line in one or the other direction and just scaled to become an error that the PD algorithm can deal with.
While this is prone to error accumulation with every step, so would be a gyro/accelerometer based approach or even more so. Now it remains to be seen, if the magnetometer has enough resolution and not too much noise for the deviation magnitude I'm trying to judge. Just in case I have one with 10 times the sensitivity of LSM303AGR
.Need to enjoy some Saturday night now, so lets see if I get anything more done today, but the route seems pretty straight forward to the next milestone - put the magnetometer in the car along with external EEPROM and record a few laps of my test track and then blurt it out to Excel to see if there is any pattern in the data to be recognised.
Edmunds
OK : that is the good way !
You only need X and Y axis (not the vertical one).
They are SIN and COS from heading angle.
As said, It's possible to interpolate ATN even with a PICAXE.
And with some head scratching, it maybe possible to navigate without using any angle.
Before running it on the track, it'is very usefull to rotate the sensor 10° by 10° (from 0 to 360°) and record corresponding data (X and Y).
Ok, I understand why this would be good, but that is another new jig. Or maybe a stepper motor. If I take a stepper motor with resolution of 1.8 degrees per step, I can record 200 measurements. I think this is easier than making something manual. Hmm.
Edmunds
Well done... Y data make a beautifull SIN :
But I suspect X data is vertical...
Please try with all 3 data : X, Y and Z so we can choose what is what.
Please, make data "Excel compatible" :
X Y Z
39 26367 12345
58 27135 12333
66 26879 12221
85 27903 12123
...
You are reading my mind
. I must have fogoten all about Excel formating. Sticking a semicolon in now. Will add the third axis. Coming in a minute.Thank you!
Edmunds
YES ! We have now a beautiful SIN/COS :Here is something that works. X is now fixed (there was a bug in the code) and numbers are signed after I learned what PITA it is to do this in Excel
We can deduce Alpha (Degrees) from X and Y :
Code:
X Y Z Alpha
13824 29183 12544 -98,9
17152 27903 13568 -91,1
23040 27647 12800 -78,4
24576 29439 13824 -74,1
27648 30207 13056 -66,9
31744 30719 13056 -58,4
-30720 32511 12800 -50,9
-28672 -29697 13056 -42,6
-26112 -26881 13824 -34,1
-24064 -26625 13056 -31,3
-22528 -23297 12800 -23,5
-22272 -17921 14080 -12,0
-22272 -16897 14592 -9,7
-21760 -14337 13568 -3,8
-21760 -10241 14336 5,5
-23808 -6145 14336 15,7
-23296 -2817 14592 22,6
-26112 -1281 15616 28,5
-27392 2560 14336 37,6
-27904 3072 15104 39,2
-31744 7168 16128 51,9
30208 6656 17152 57,9
28928 8960 15872 63,3
25856 12032 16896 72,2
20224 10240 16896 83,8
17664 10496 16384 90,0
12288 12544 17664 101,6
8960 11264 16128 109,3
6656 9472 16128 115,6
4096 8704 17408 121,5
512 7680 16896 129,1
-769 5632 17408 134,2
-3073 3840 16896 140,5
-4865 512 17920 148,8
-7169 -2305 16640 156,6
-6913 -5121 16128 162,3
-8449 -9985 17152 173,9
-6913 -11265 15616 176,6
-6401 -17409 15616 -168,5
-6145 -18945 15104 -164,7
-5377 -22785 14592 -155,6
-3585 -24065 14592 -151,0
-1281 -27137 14848 -141,7
512 -29185 14080 -135,1
1792 32767 13312 -127,3
4096 32767 13056 -123,1
7936 29951 13568 -112,31) Go -90°, (IE X close to 17152, Y close to 27903)
2) GO 0°, (IE X close to -21760, Y close to -14337)
3) Go 90° , (IE X close to 17664, Y close to 10496)
4) Go 180°, (IE X close to -6913, Y close to -11265)
The big deal is : what to do when readed values for X and Y are differents from theses ?
In others words, if X is bigger than expected and Y smaller, does I have to go left or right ?
PS for next test :
- please add at least one measurement to be abble to compare firsts and lasts values.
Last edited:
Aren't X and Y related to each other? Just add up the two or something? If that does not work, arctan is still an OK option and then we have a single number. If actual is less than what we need - go right (CW), if more - go left (CCW).
Do you have any suggestion on how to plot the data from the Map file? Just out of curiosity, strictly speaking it is not needed.
I will try to get the connection between main picaxe and steering co-processor working now.
Edmunds
Do you have any suggestion on how to plot the data from the Map file? Just out of curiosity, strictly speaking it is not needed.
I will try to get the connection between main picaxe and steering co-processor working now.
Edmunds
Yes they are : X^2 + Y^2= Constant
To be exact :
X^2 + Y^2 + Z^2 = Constant
but Z is supposed to be constant. This is not exactly the case as per your measurements.
You needs to calibrate magnetometer, IE, using few "full 360°" datasets :
- find MIN and MAX value for X and for Y
- adjust zero to be (MIN+MAX)/2
- adjust scale to be (-1;1) or whatever needed for ATAN2
ATAN2 to be exact. This is the only one I know...
Is 350° more or less that 5° ?
I usually use Excel (cloud graph) but theses numbers do not look like coordinates. What are they supposed to be ?
Last edited:
They are raw, but signed and decimal data from magnetometer. The same as for the 360 degree thing + some crap (a lot of it, actually) in the end. If I convert this into an angle and add some line between - approx 2mm long, then I have segments + angle. They should constitute one or two squares depending on how much actually got recorded. I'll look into cloud graphs. I'm sure, there is some crazy matlab thing that takes two weeks to understand, but I would be very glad to avoid it
. At least for now.Of course, people tend to mix them quite often, but I meant ATAN2. Will try to behave
.I think this is not a problem at all. It is not a super-fast differential steered robot. My min radius (what the car is capable off with steering wheels hitting one side) is 75mm. This is about 523 segments of 0.9mm that I can measure (my odometer resolution) in a full circle. This is ~0.688° degree declination from segment to segment. There will never be a directive 'go 273° from the current heading'. Clearly, reading every 0.9mm segment and doing something about it is not really possible if going fast (3ms between pulses), but even if I add up 10 segments for 9mm my max. declination from the previous heading is only ~7°. So 12° is always CW from 5° and 358° is always CCW from 5°.
Thank you for your input,
Edmunds
Last edited:
Yes, so I have read. What I'm trying to figure out, however, is how to come up with a routine for the car to calibrate magnetometer. Drive a circle or figure eight should suffice physically, but I have not come up with an algorithm for what to do with the data. Just a few hours ago I got some blob of numbers not really making any sense for now into an EEPROM finally
. Will get there.Thank you for your time,
Edmunds
That seems easy. Please, publish your code to do that...
Did you manage to do that with Excel ?
I intentionaly published Alpha (#90) with no explanation...
Are you confident for theses calculations ?
For calibration, drive few circles will be my preference. You only have to find MIN and MAX values for X and Y.
No needs to store data.
Last edited:
No, not yet, just got the hserin to work between the chips. Now need to get some sleep. But I will try something like:
heading (y>0) = 90 - [arcTAN(x/y)]*180/Pi
heading (y<0) = 270 - [arcTAN(x/y)]*180/Pi
And a special case (which will kind of almost never happen
):heading (y=0, x<0) = 180.0
heading (y=0, x>0) = 0.0
This is maybe where I got the arctan from. Hmm.
Edmunds
Last edited:
With Excel, you have the ATAN2 function...No, not yet, just got the hserin to work between the chips. Now need to get some sleep. But I will try something like:
heading (y>0) = 90 - [arcTAN(x/y)]*180/Pi
heading (y<0) = 270 - [arcTAN(x/y)]*180/Pi
And a special case (which will kind of almost never happen
heading (y=0, x<0) = 180.0
heading (y=0, x>0) = 0.0
This is maybe where I got the arctan from. Hmm.
Edmunds
But first, you have to calibrate the sensor.
Depending where you are on Earth :
- X and Y scale factors are not identical, so ATAN is not directly usable,
- measurements are not zero centered ; there are X and Y offsets...
==> #93
Then you have to (re-)write the ATAN2 function for Picaxe...
and to do that, decide what is your angular unit :
1) 0=>360 ° (within a word )
2) 0=>3600 1/10 °
3) 0 => 36000 1/100°
4) -180 => 180°
5) 0==>65535
The last one is interesting because there is only one possibility for an angle ;
300°+90°=30°
become :
54613+16384=5461
Personnaly, I would avoid using negatives numbers with a Picaxe to represent an angle...
Last edited:
Sorry : there is no Nobel prize for mathematics...For an immediate Nobel prize?
Edmunds
only our consideration
Thank you for your comments!
Digging in.
Edmunds
I will try to do this today. The good news is that offsets can be stored in the device registers, so 'permanent' offsets I won't have to waste program space and time for in my code.
My gut feeling is that 4 and 5 should be preferable in terms of execution effort. My steering response speed is a constraint I have to keep in mind. Ideally, I would not introduce any overhead if compared to line following. So the best option is an option that fits in one byte or a 16bit word max to be sent fast over hserout, contains 'a sign' or steering direction and magnitude or number of steps to pulse the stepper with. Some bits of this byte or word should be available for other commands - like stop steering altogether, because we parked and resume steering again, because we are going again.
Digging in.
Edmunds
Hi,
For the angles, the two MSBs define the Quadrant (i.e. permutations of the sign of X and Y) and you can use a simple (interpolated) lookup table for the (equivalent of the) angle from 0 to 90 degrees. For ATAN{2} use the third MSB to define the Octant, i.e. whether X > Y or whether X < Y (nb: X , Y are the unsigned modulus here). This then keeps the TAN value always less than (or = to) 1.
Never again will you have to write (even in a macro): IF theta => 360 THEN : theta = theta - 360 : ENDIF . I used this method for a generic Sunrise / Sunset calculation for any time / date / location (i.e. 3D geometry), which runs quite accurately (and with codespace to spare) even in an 08M2. One day (perhaps) I will get it all documented in a Code Snippett.
Cheers, Alan.
That gets my vote, it basically uses "Fractional Binary", i.e. the Most Significant Bit of a Word (or Byte) represents 0.5, the next 0.25, etc.. You can use this for both the angle, i.e. fractions of a revolution (360 degrees or 2 x Pi) and for the SIN and COS values. Or for the actual SIN and COS values you might use the MSB{s} for the sign and/or 1.0 (if you want unity stored exactly).
For the angles, the two MSBs define the Quadrant (i.e. permutations of the sign of X and Y) and you can use a simple (interpolated) lookup table for the (equivalent of the) angle from 0 to 90 degrees. For ATAN{2} use the third MSB to define the Octant, i.e. whether X > Y or whether X < Y (nb: X , Y are the unsigned modulus here). This then keeps the TAN value always less than (or = to) 1.
Never again will you have to write (even in a macro): IF theta => 360 THEN : theta = theta - 360 : ENDIF
. I used this method for a generic Sunrise / Sunset calculation for any time / date / location (i.e. 3D geometry), which runs quite accurately (and with codespace to spare) even in an 08M2. One day (perhaps) I will get it all documented in a Code Snippett. Cheers, Alan.
Wondered if circle code useful. https://en.wikipedia.org/wiki/Midpoint_circle_algorithm
Stan, thanks. Maybe this part 'Note that if the angles are given as slopes, then no trigonometry or square roots are necessary: one simply checks that y/x is between the desired slopes.' could be specifically interesting. Not there yet, however, working on 'drive three circles and calculate and store magnetometer offsets' procedure.
Edmunds
Edmunds
Dear @Besqueut,
I have worked on this for a couple of hours now, and I can arrive at all sorts of different results from the data, but not the ones you have. And I kind a like yours, because they seem to be quite close to 7.5 degree step between them and this is what it actually was.
Now, pen and paper version. I can see that both, 13824 and 29183 are positive values, so my angle is in quadrant 1, 0 to 90 degrees. Since my y is bigger than x, I calculate atan of x/y, which is atan(13824/29183) = atan(0.473700442) = 25.3469355 degrees. Because it is the 1st quadrant, my heading should be 90 - 25.3469355 = ~64 degrees. Again, minus the E declination - about 57 degree heading.
I have tried to swap the numbers and do other bad things to them, but nothing takes me away from the result of 64, not 98.9.
Can you explain what I'm doing wrong here?
Edmunds
I have worked on this for a couple of hours now, and I can arrive at all sorts of different results from the data, but not the ones you have. And I kind a like yours, because they seem to be quite close to 7.5 degree step between them and this is what it actually was
.I have tried ATAN2(x,y) in excel (take first row for an example). This gives 0.44238748. This is radians, so I want to get degrees. I multiply with 180 and divide with Pi and get 25.35979184. If I now put it in the first quadrant, where it kind of belongs, then 90 - 25.35979184 = 64.64020816 degrees. If I subtract 8.05 degrees of E declination for where I am, I get roughy 57 degree heading, not -(!)98.9 as per your example.
Now, pen and paper version. I can see that both, 13824 and 29183 are positive values, so my angle is in quadrant 1, 0 to 90 degrees. Since my y is bigger than x, I calculate atan of x/y, which is atan(13824/29183) = atan(0.473700442) = 25.3469355 degrees. Because it is the 1st quadrant, my heading should be 90 - 25.3469355 = ~64 degrees. Again, minus the E declination - about 57 degree heading.
I have tried to swap the numbers and do other bad things to them, but nothing takes me away from the result of 64, not 98.9.
Can you explain what I'm doing wrong here?
Edmunds
Last edited:
Only three years to practice remaining so I'm eligibleNot for me... but you can try Fields_Medal
.Edmunds
You are not wrong.... but I am ! Sorry, there where a mistake for MIN and MAX values...
While I have something that looks like it works, but does not (cannot get the bloody octants right, tried three times now ), I found this. Very relevant. Needless to say I had no idea about Q15 nor fixed point concept when Alan and @Besqueut mentioned it first. Now I'm starting to get the beauty. Will try to digest the C code linked to here as well.
Cheers (literally),
Edmunds
), I found this. Very relevant. Needless to say I had no idea about Q15 nor fixed point concept when Alan and @Besqueut mentioned it first. Now I'm starting to get the beauty. Will try to digest the C code linked to here as well.Cheers (literally),
Edmunds
Dear all,
This seems long and boring, but I don't think there is anything better for 'octanifying' the angles. Written in pseudo code sort of - no variable definitions and no actual code to do something about values, only structure.
Edmunds
This seems long and boring, but I don't think there is anything better for 'octanifying' the angles. Written in pseudo code sort of - no variable definitions and no actual code to do something about values, only structure.
Code:
If y = 0 or x = 0 or x = y 'Handle special cases
endif
if y < 0 and x < 0 then 'Octants 5 and 4
if absx < absy then 'Octant 5
else 'Octant 4
endif
elseif y < 0 and x > 0 then 'Octants 7 and 6
if x < absy then 'Octant 7
else 'Octant 6
endif
elseif y > 0 and x < 0 then 'Octants 3 and 2
if absx > y then 'Octant 3
else 'Octant 2
endif
elseif y > 0 and x > 0 then 'Octants 1 and 0
if x < y then 'Ocatnt 1
else 'Octant 0
endif
endifHi,
Here is the "octant" (8 sectors) part of some ATAN code that I wrote a few years ago. The complete version of the very preliminary code is in post #20 here. But that version works in degrees, which is why I've never submitted it to the code snippetts section.
Cheers, Alan.
Here is the "octant" (8 sectors) part of some ATAN code that I wrote a few years ago. The complete version of the very preliminary code is in post #20 here. But that version works in degrees, which is why I've never submitted it to the code snippetts section.
Code:
symbol negx = 128
symbol deltx= b10
symbol delty= b11
symbol sect = b0 ; Sector number from 0 (0 - 45 degs) to 7 (315 - 360 degs) MUST BE b0
if deltx < negx then
sect = 0
else
sect = 7
deltx = - deltx
endif
if delty > negx then
sect = sect xor 3
delty = - delty
endif
if deltx > delty then
sect = sect xor 1
swap deltx, delty ; swap uses 12 - 14 bytes of codespace!
endifHere is an interesting idea for ATAN2 approximation:
1. Forget octants and forget ATAN as such. Forget lookup tables.
2. Calculate r = (x-y)/(x+y) for x>0 and r = (x+y)/(y-x) for x<0 for all y>0.
3. Calculate theta = 180/4 - 180/4*r for x>0 and theta = 3*180/4 - 180/4*r for x<0
4. In case y<0, just negate theta.
Done with ~4 degree accuracy in excel or some floating point monster. The best I could get on picaxe due to overflows is 7 degree accuracy, which is likely not good enough.
Edmunds
1. Forget octants and forget ATAN as such. Forget lookup tables.
2. Calculate r = (x-y)/(x+y) for x>0 and r = (x+y)/(y-x) for x<0 for all y>0.
3. Calculate theta = 180/4 - 180/4*r for x>0 and theta = 3*180/4 - 180/4*r for x<0
4. In case y<0, just negate theta.
Done with ~4 degree accuracy in excel or some floating point monster. The best I could get on picaxe due to overflows is 7 degree accuracy, which is likely not good enough.
Edmunds
Why not simply using the Picaxe ATAN function ?
Code:
Picaxe Excel
TAN ATAN ATAN
0 0 0,0
1 0 0,6
2 1 1,1
3 1 1,7
4 2 2,3
5 3 2,9
6 3 3,4
7 4 4,0
8 4 4,6
9 5 5,1
10 6 5,7
11 6 6,3
12 7 6,8
13 7 7,4
14 8 8,0
15 8 8,5
16 9 9,1
17 9 9,6
18 10 10,2
19 10 10,8
20 11 11,3
21 11 11,9
22 12 12,4
23 12 13,0
24 13 13,5
25 14 14,0
26 14 14,6
27 15 15,1
28 15 15,6
29 16 16,2
30 17 16,7
31 17 17,2
32 18 17,7
33 18 18,3
34 19 18,8
35 19 19,3
36 20 19,8
37 20 20,3
38 21 20,8
39 21 21,3
40 22 21,8
41 22 22,3
42 23 22,8
43 23 23,3
44 24 23,7
45 24 24,2
46 25 24,7
47 25 25,2
48 26 25,6
49 26 26,1
50 27 26,6
51 27 27,0
52 27 27,5
53 28 27,9
54 28 28,4
55 29 28,8
56 29 29,2
57 29 29,7
58 30 30,1
59 30 30,5
60 31 31,0
61 31 31,4
62 31 31,8
63 32 32,2
64 32 32,6
65 33 33,0
66 33 33,4
67 33 33,8
68 34 34,2
69 34 34,6
70 35 35,0
71 35 35,4
72 35 35,8
73 36 36,1
74 36 36,5
75 37 36,9
76 37 37,2
77 37 37,6
78 38 38,0
79 38 38,3
80 39 38,7
81 39 39,0
82 39 39,4
83 39 39,7
84 40 40,0
85 40 40,4
86 40 40,7
87 41 41,0
88 41 41,3
89 41 41,7
90 42 42,0
91 42 42,3
92 42 42,6
93 42 42,9
94 43 43,2
95 43 43,5
96 43 43,8
97 44 44,1
98 44 44,4
99 44 44,7
100 45 45,0Read about slopes for less trig but this thread has too many sums. https://en.wikipedia.org/wiki/Slope
There is also this one :
FastATAN(X)=PI_4*x - x*(abs(x) - 1)*(0.2447 + 0.0663*abs(x))
And also :
Better-atan-approximations
FastATAN(X)=PI_4*x - x*(abs(x) - 1)*(0.2447 + 0.0663*abs(x))
Code:
Excel Fast
X ATAN ATAN
0,01 0,57 0,59
0,02 1,15 1,18
0,03 1,72 1,76
0,04 2,29 2,34
0,05 2,86 2,92
0,06 3,43 3,50
0,07 4,00 4,08
0,08 4,57 4,65
0,09 5,14 5,23
0,1 5,71 5,80
0,11 6,28 6,36
0,12 6,84 6,93
0,13 7,41 7,49
0,14 7,97 8,05
0,15 8,53 8,61
0,16 9,09 9,17
0,17 9,65 9,72
0,18 10,20 10,27
0,19 10,76 10,82
0,2 11,31 11,36
0,21 11,86 11,91
0,22 12,41 12,45
0,23 12,95 12,99
0,24 13,50 13,52
0,25 14,04 14,06
0,26 14,57 14,59
0,27 15,11 15,12
0,28 15,64 15,64
0,29 16,17 16,16
0,3 16,70 16,68
0,31 17,22 17,20
0,32 17,74 17,72
0,33 18,26 18,23
0,34 18,78 18,74
0,35 19,29 19,24
0,36 19,80 19,75
0,37 20,30 20,25
0,38 20,81 20,74
0,39 21,31 21,24
0,4 21,80 21,73
0,41 22,29 22,22
0,42 22,78 22,70
0,43 23,27 23,19
0,44 23,75 23,67
0,45 24,23 24,14
0,46 24,70 24,62
0,47 25,17 25,09
0,48 25,64 25,55
0,49 26,10 26,02
0,5 26,57 26,48
0,51 27,02 26,94
0,52 27,47 27,39
0,53 27,92 27,84
0,54 28,37 28,29
0,55 28,81 28,74
0,56 29,25 29,18
0,57 29,68 29,62
0,58 30,11 30,05
0,59 30,54 30,48
0,6 30,96 30,91
0,61 31,38 31,34
0,62 31,80 31,76
0,63 32,21 32,18
0,64 32,62 32,59
0,65 33,02 33,00
0,66 33,42 33,41
0,67 33,82 33,81
0,68 34,22 34,21
0,69 34,61 34,61
0,7 34,99 35,00
0,71 35,37 35,39
0,72 35,75 35,78
0,73 36,13 36,16
0,74 36,50 36,54
0,75 36,87 36,91
0,76 37,23 37,28
0,77 37,60 37,65
0,78 37,95 38,01
0,79 38,31 38,37
0,8 38,66 38,73
0,81 39,01 39,08
0,82 39,35 39,43
0,83 39,69 39,77
0,84 40,03 40,11
0,85 40,36 40,45
0,86 40,70 40,78
0,87 41,02 41,11
0,88 41,35 41,43
0,89 41,67 41,75
0,9 41,99 42,07
0,91 42,30 42,38
0,92 42,61 42,69
0,93 42,92 42,99
0,94 43,23 43,29
0,95 43,53 43,59
0,96 43,83 43,88
0,97 44,13 44,17
0,98 44,42 44,45
0,99 44,71 44,73
1 45,00 45,00Better-atan-approximations
Last edited:
Simply using theta=47 * X
error is less that 3 units...
So, time to post some code that does not work . Below are two procedures that attempt to calculate the angle. I have gone through the code thoroughly and I know that things like dropping the angle into the right octant work. Still, I'm getting very, very jittery results compared to the results I had when using the calibrating jig, albeit with calculations done in Excel.
My scaling can be questioned, I have not given it a lot of thought for now, but I think whatever it would be, I should be getting consistent numbers out.
I have put offset data that I obtained from the calibration jig project into the offset registers of the magnetometer so it is not completely raw thing.
Here is some data of me slowly rotating the car about 45 degrees to one side and then 45 to the other from the initial position a couple of times:
Is there anything that I'm just tired and cannot see anymore today?
Edmunds
. Below are two procedures that attempt to calculate the angle. I have gone through the code thoroughly and I know that things like dropping the angle into the right octant work. Still, I'm getting very, very jittery results compared to the results I had when using the calibrating jig, albeit with calculations done in Excel.My scaling can be questioned, I have not given it a lot of thought for now, but I think whatever it would be, I should be getting consistent numbers out.
Code:
#macro ScaleABSMagData() 'Scale with sensitivity and make positive, but remember the sign
if MX_word >= $8000 then 'If negative value ...
MX_word = -MX_word / 1000 * 15 'Scale factor fixed at 1.5 for this magnetometer
else 'If positive value ...
MX_word = MX_word / 1000 * 15 'Scale factor fixed at 1.5 for this magnetometer
endif
if MY_word >= $8000 then 'If negative value ...
MY_word = -MY_word / 1000 * 15 'Scale factor fixed at 1.5 for this magnetometer
else 'If positive value ...
MY_word = MY_word / 1000 * 15 'Scale factor fixed at 1.5 for this magnetometer
endif
#endmacro
#macro GetHeading
if MX_word < $8000 then 'If X is positive ...
octant = 0 '... it must on the right hand side, lets assume octant 0 for now
else 'If X is negative ...
octant = 7 '... it must be on the left hand side, lets assume octant 7 for now
MX_word = -MX_word 'Get ABS(X) for further calculations
endif
if MY_word >= $8000 then 'If Y is negative ...
octant = octant xor 3 '... xor 0 or 7 with 3 for octants 3 or 4 respectively
MY_word = - MY_word 'Get ABS(Y) for further calculations
endif
if MX_word < MY_word then 'If ABS(X) < ABS(Y), we will move to the oposite octant ...
octant = octant xor 1 '... xor 0 or 7 with 1 for octants 1 and 6 or 3 or 4 with 1 for octants 2 and 5
swap MX_word, MY_word '... since in calculating in the oposite octant, swap ABS(X) and ABS(Y)
endif
M_quot = MX_Word / MY_Word * 100 'Calculate quetient of ABS(X) and ABS(Y)
Heading = atan M_quot 'Calculate Heading with ATAN, 0 to 45 degrees
if bit0 = 1 then 'In case we had moved to the opposite quadrant ...
Heading = 45 - Heading '... calculate the angle backwards
endif
Heading = octant * 45 + Heading 'Add 45 degrees for every octant and the actual angle for Heading
#endmacroI have put offset data that I obtained from the calibration jig project into the offset registers of the magnetometer so it is not completely raw thing.
Code:
#macro LoadMOffsets
read 0, WORD M_Offset
hi2cout [MAddress], OFFSET_X_REG_L_M, (MO_L_byte,MO_H_byte)
read 2, WORD M_Offset
hi2cout [MAddress], OFFSET_X_REG_L_M, (MO_L_byte,MO_H_byte)
read 4, WORD M_Offset
hi2cout [MAddress], OFFSET_X_REG_L_M, (MO_L_byte,MO_H_byte)
#endmacro
'Constants
Symbol MX_L_offset = $0 'Initial offset for X axis, $0 split into bytes for eeprom function
Symbol MX_H_offset = $0
'positive version
;Symbol MY_L_offset = $FF 'Initial offset for Y axis, $5FF split into bytes for eeprom function
;Symbol MY_H_offset = $05
;Symbol MZ_L_offset = $80 'Initial offset for Z axis, $3B80 split into bytes for eeprom function
;Symbol MZ_H_offset = $3B
'negative version
Symbol MY_L_offset = $01 'Initial offset for Y axis, $5FF split into bytes for eeprom function
Symbol MY_H_offset = $FA
Symbol MZ_L_offset = $80 'Initial offset for Z axis, $3B80 split into bytes for eeprom function
Symbol MZ_H_offset = $C4
read 5, Temp
if Temp <> 0 then 'if no device calibration data present in memory ...
write 0, MX_L_offset,MX_H_offset '... load initial magnetometer calibration approximation into memory
write 2, MY_L_offset,MY_H_offset
write 4, MZ_L_offset,MZ_H_offset
endif
Code:
X: 54016; Y: 62711; Z: 5887
Heading: 435
X: 60416; Y: 60151; Z: 8959
Heading: 225
X: 60928; Y: 62711; Z: 8959
Heading: 225
X: 61696; Y: 60663; Z: 6399
Heading: 225
X: 60672; Y: 62455; Z: 6655
Heading: 225
X: 60160; Y: 61175; Z: 7423
Heading: 225
X: 60928; Y: 60919; Z: 6143
Heading: 225
X: 59904; Y: 61943; Z: 6911
Heading: 225
X: 61184; Y: 61687; Z: 6655
Heading: 225
X: 61184; Y: 60663; Z: 7423
Heading: 225
X: 60160; Y: 63479; Z: 7935
Heading: 351
X: 55808; Y: 1528; Z: 8191
Heading: 358
X: 53248; Y: 4856; Z: 8447
Heading: 189
X: 51200; Y: 4856; Z: 8447
Heading: 189
X: 47872; Y: 6648; Z: 8703
Heading: 189
X: 47104; Y: 8184; Z: 9215
Heading: 189
X: 44800; Y: 9720; Z: 9727
Heading: 189
X: 41984; Y: 10744; Z: 9471
Heading: 189
X: 40704; Y: 12280; Z: 9983
Heading: 189
X: 39424; Y: 11512; Z: 9215
Heading: 189
X: 36864; Y: 13304; Z: 10495
Heading: 6921
X: 36096; Y: 12280; Z: 9983
Heading: 189
X: 36864; Y: 13560; Z: 9983
Heading: 189
X: 38400; Y: 13304; Z: 10239
Heading: 285
X: 39424; Y: 12280; Z: 10239
Heading: 299
X: 44288; Y: 8696; Z: 10239
Heading: 189
X: 48128; Y: 4600; Z: 10495
Heading: 164
X: 50944; Y: 3832; Z: 10751
Heading: 164
X: 54016; Y: 3320; Z: 11007
Heading: 164
X: 53504; Y: 2296; Z: 9727
Heading: 360
X: 54528; Y: 2040; Z: 9471
Heading: 360
X: 55040; Y: 2040; Z: 9471
Heading: 360
X: 54272; Y: 1272; Z: 9727
Heading: 356
X: 56320; Y: 760; Z: 6911
Heading: 345
X: 57600; Y: 65271; Z: 7935
Heading: 185
X: 57856; Y: 63223; Z: 9215
Heading: 376
X: 59136; Y: 64759; Z: 9727
Heading: 184
X: 59392; Y: 62711; Z: 9471
Heading: 351
X: 61184; Y: 61943; Z: 9471
Heading: 225
X: 60928; Y: 59383; Z: 7423
Heading: 225
X: 62208; Y: 59895; Z: 7423
Heading: 225
X: 62464; Y: 58871; Z: 9727
Heading: 99
X: 63744; Y: 56055; Z: 9471
Heading: 270
X: 63744; Y: 55799; Z: 10239
Heading: 337
X: 65280; Y: 55031; Z: 9983
Heading: 142
X: 769; Y: 52983; Z: 7935
Heading: 325
X: 1025; Y: 52471; Z: 8447
Heading: 105
X: 513; Y: 51447; Z: 10239
Heading: 93
X: 2049; Y: 49399; Z: 7167
Heading: 286
X: 1537; Y: 48887; Z: 8703
Heading: 90
X: 2049; Y: 46071; Z: 7423
[7F]Heading: 90
X: 2305; Y: 44023; Z: 7679
[7F][7F][FF]Heading: 90
X: 4097; Y: 38391; Z: 8191
Heading: 92
X: 1793; Y: 33271; Z: 8191
Heading: 94
X: 4353; Y: 33783; Z: 8959
Heading: 286
X: 3329; Y: 34295; Z: 8447
Heading: 90
X: 4097; Y: 33015; Z: 8447
Heading: 286
X: 4097; Y: 36087; Z: 7935
Heading: 286
X: 4097; Y: 38135; Z: 6399
Heading: 92
X: 5121; Y: 39415; Z: 7679
Heading: 90
X: 4609; Y: 40439; Z: 8447
Heading: 90
X: 3585; Y: 41207; Z: 6911
Heading: 92
X: 2049; Y: 44023; Z: 7935
Heading: 90
X: 3841; Y: 48375; Z: 6655
Heading: 345
X: 2561; Y: 48887; Z: 7935
Heading: 92
X: 1537; Y: 53495; Z: 8191
Heading: 286
X: 257; Y: 53495; Z: 7935
Heading: 95
X: 64512; Y: 55799; Z: 8959
Heading: 270
X: 64000; Y: 57335; Z: 8959
Heading: 270
X: 61952; Y: 61943; Z: 8447
Heading: 225
X: 60416; Y: 61687; Z: 8959
Heading: 225
X: 61696; Y: 64247; Z: 9471
Heading: 351
X: 59136; Y: 65271; Z: 8191
Heading: 195
X: 58368; Y: 248; Z: 9215
Heading: 129
X: 57344; Y: 760; Z: 8703
Heading: 360
X: 56320; Y: 2296; Z: 11519
Heading: 105
X: 55040; Y: 3832; Z: 9215
Heading: 189
X: 52736; Y: 6136; Z: 8959
Heading: 189
X: 50176; Y: 5624; Z: 10751
Heading: 189
X: 51968; Y: 6392; Z: 9983
Heading: 189
X: 47104; Y: 9464; Z: 11263
Heading: 315
X: 44800; Y: 9976; Z: 11263
Heading: 189
X: 43008; Y: 10488; Z: 9727
Heading: 189
X: 40704; Y: 13048; Z: 11263
Heading: 409
X: 37120; Y: 13560; Z: 10751
X: 35584; Y: 14584; Z: 10495
Heading: 411Edmunds
Last edited:
I now removed the scaling altogether as it is not strictly needed before the calculations. This did not really change a thing in the fact that the angles jump some seemingly arbitrary number back and forth instead of increasing or decreasing with turning the vehicle.
Help, please
Edmunds
Help, please
Edmunds