|
16 | 16 | Y = 1 |
17 | 17 | Z = 2 |
18 | 18 |
|
| 19 | +# The two axes which relate to heading, depends on orientation of the sensor |
| 20 | +# Think Left & Right, Forwards and Back, ignoring Up and Down |
19 | 21 | AXES = Y, Z |
20 | 22 |
|
| 23 | +# Initialise the imu |
21 | 24 | imu = ICM20948() |
22 | 25 |
|
| 26 | +# Store an initial two readings from the Magnetometer |
23 | 27 | amin = list(imu.read_magnetometer_data()) |
24 | 28 | amax = list(imu.read_magnetometer_data()) |
25 | 29 |
|
26 | 30 | while True: |
| 31 | + # Read the current, uncalibrated, X, Y & Z magnetic values from the magnetometer and save as a list |
27 | 32 | mag = list(imu.read_magnetometer_data()) |
| 33 | + |
| 34 | + # Step through each uncalibrated X, Y & Z magnetic value and calibrate them the best we can |
28 | 35 | for i in range(3): |
29 | 36 | v = mag[i] |
| 37 | + # If our current reading (mag) is less than our stored minimum reading (amin), then save a new minimum reading |
| 38 | + # ie save a new lowest possible value for our calibration of this axis |
30 | 39 | if v < amin[i]: |
31 | 40 | amin[i] = v |
| 41 | + # If our current reading (mag) is greater than our stored maximum reading (amax), then save a new maximum reading |
| 42 | + # ie save a new highest possible value for our calibration of this axis |
32 | 43 | if v > amax[i]: |
33 | 44 | amax[i] = v |
| 45 | + |
| 46 | + # Calibrate value by removing any offset when compared to the lowest reading seen for this axes |
34 | 47 | mag[i] -= amin[i] |
| 48 | + |
| 49 | + # Scale value based on the higest range of values seen for this axes |
| 50 | + # Creates a calibrated value between 0 and 1 representing magnetic value |
35 | 51 | try: |
36 | 52 | mag[i] /= amax[i] - amin[i] |
37 | 53 | except ZeroDivisionError: |
38 | 54 | pass |
| 55 | + # Shift magnetic values to between -0.5 and 0.5 to enable the trig to work |
39 | 56 | mag[i] -= 0.5 |
40 | 57 |
|
| 58 | + # Convert from Gauss values in the appropriate 2 axis to a heading in Radians using trig |
| 59 | + # Note this does not compensate for tilt |
41 | 60 | heading = math.atan2( |
42 | 61 | mag[AXES[0]], |
43 | 62 | mag[AXES[1]]) |
44 | 63 |
|
| 64 | + # If heading is negative, convert to positive, 2 x pi is a full circle in Radians |
45 | 65 | if heading < 0: |
46 | 66 | heading += 2 * math.pi |
| 67 | + |
| 68 | + # Convert heading from Radians to Degrees |
47 | 69 | heading = math.degrees(heading) |
| 70 | + # Round heading to nearest full degree |
48 | 71 | heading = round(heading) |
49 | 72 |
|
| 73 | + # Note: Headings will not be correct until a full 360 deg calibration turn has been completed to generate amin and amax data |
50 | 74 | print("Heading: {}".format(heading)) |
51 | 75 |
|
52 | 76 | time.sleep(0.1) |
0 commit comments