-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathimu_test.py
executable file
·159 lines (139 loc) · 6.36 KB
/
imu_test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright 2020-2021 by Murray Altheim. All rights reserved. This file is part
# of the Robot Operating System project, released under the MIT License. Please
# see the LICENSE file included as part of this package.
#
# author: Murray Altheim
# created: 2020-08-15
# modified: 2020-08-28
#
# Tests the ICM20948 and Compass class (which uses a BNO055) for an IMU fusion
# used solely for reading an absolute heading (i.e., as a compass). This also
# provides for an optional RGBMatrix 5x5 display.
#
# We know that if an object is not moving it will experience acceleration
# only due to gravity (neglect the other minimal forces). The direction of
# gravitational force is always same with respect to the earth’s frame but
# based on the orientation of IMU, it will experience different amount of
# acceleration along the three axes. These acceleration values can give us
# roll and pitch values.
#
# pitch = 180 * atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ))/PI;
# roll = 180 * atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ))/PI;
#
# As in accelerometer one can use the X, Y and Z magnetometer readings to
# calculate yaw.
#
# mag_x = magReadX*cos(pitch) + magReadY*sin(roll)*sin(pitch) + magReadZ*cos(roll)*sin(pitch)
# mag_y = magReadY * cos(roll) - magReadZ * sin(roll)
# yaw = 180 * atan2(-mag_y,mag_x)/M_PI;
#
import pytest
import time, sys, traceback
from colorama import init, Fore, Style
init()
from lib.i2c_scanner import I2CScanner
from lib.config_loader import ConfigLoader
from lib.logger import Level, Logger
from lib.convert import Convert
from lib.enums import Cardinal
from lib.rgbmatrix import RgbMatrix, Color, DisplayType
from icm20948 import ICM20948
from lib.bno055 import BNO055
# ..............................................................................
class IMU():
'''
Composite IMU.
'''
def __init__(self, config, level):
self._log = Logger("imu", level)
if config is None:
raise ValueError("no configuration provided.")
# ICM20948 configuration .....................................
_icm20948_config = config['ros'].get('icm20948')
self._icm20948_heading_trim = _icm20948_config.get('heading_trim')
self._log.info('trim: heading: {:<5.2f}° (ICM20948)'.format(self._icm20948_heading_trim))
self._icm20948 = ICM20948(i2c_addr=0x69)
self._amin = list(self._icm20948.read_magnetometer_data())
self._amax = list(self._icm20948.read_magnetometer_data())
# BNO055 configuration .......................................
self._bno055 = BNO055(config, Level.INFO)
self._log.info('ready.')
# ..........................................................................
def read_icm20948_magnetometer(self):
return self._icm20948.read_magnetometer_data()
# ..........................................................................
def read_icm20948_accelerometer_gyro_data(self):
# ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro()
return self._icm20948.read_accelerometer_gyro_data()
# ..........................................................................
def read_icm20948_heading(self):
_mag = self._icm20948.read_magnetometer_data()
_orig_heading = Convert.heading_from_magnetometer(self._amin, self._amax, _mag)
_heading = Convert.offset_in_degrees(_orig_heading, self._icm20948_heading_trim)
self._log.info(Fore.GREEN + 'heading:\t{:>9.2f}°\t'.format(_heading) + Style.DIM \
+ 'orig: {:>9.2f}°\ttrim: {:>9.2f}°; icm20948'.format(_orig_heading, self._icm20948_heading_trim))
return _heading
# ..........................................................................
def read_bno055_heading(self):
_bno_reading = self._bno055.read()
return _bno_reading
# ..........................................................................
def read_heading(self):
'''
Read a composite heading, returning the BNO055 and ICM20948 values as a tuple.
'''
_bno_reading = self.read_bno055_heading()
_bno_heading = _bno_reading[1]
_icm20948_heading = self.read_icm20948_heading()
if _bno_heading and _icm20948_heading:
_diff = _bno_heading - _icm20948_heading
if _diff < 90.0:
self._log.info(Fore.CYAN + 'diff: {:5.2f}°\t'.format(_diff) + Fore.BLACK + '(bno: {:5.2f}°; icm: {:5.2f}°)'.format(_bno_heading, _icm20948_heading))
else:
self._log.info(Fore.YELLOW + 'diff: {:5.2f}°\t'.format(_diff) + Fore.BLACK + '(bno: {:5.2f}°; icm: {:5.2f}°)'.format(_bno_heading, _icm20948_heading))
return [ _bno_heading, _icm20948_heading ]
else:
self._log.info('unavailable.')
return [ -1.0, -1.0 ]
# ..............................................................................
@pytest.mark.unit
def test_imu():
_log = Logger("imu-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
_rgbmatrix = RgbMatrix(Level.INFO) if (0x74 in _addresses) else None
if _rgbmatrix:
_rgbmatrix.set_display_type(DisplayType.SOLID)
_rgbmatrix.enable()
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_cardinal = Cardinal.get_heading_from_degrees(0)
_imu = IMU(_config, Level.INFO)
while True:
_heading = _imu.read_heading()[0]
if _rgbmatrix:
_cardinal = Cardinal.get_heading_from_degrees(_heading)
_color = Cardinal.get_color_for_direction(_cardinal)
_rgbmatrix.set_color(_color)
_log.info(Fore.YELLOW + 'heading: {:05.2f}°;\tcardinal: {}'.format(_heading, _cardinal.display) + Style.RESET_ALL)
time.sleep(2.0)
except KeyboardInterrupt:
_log.info('Ctrl-C caught: exiting...')
except Exception as e:
_log.error('error closing: {}\n{}'.format(e, traceback.format_exc()))
finally:
if _rgbmatrix:
_rgbmatrix.set_color(Color.BLACK)
_rgbmatrix.disable()
_log.info('complete.')
# ..............................................................................
def main():
test_imu()
if __name__== "__main__":
main()
#EOF