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 get_memory_pct():
# 3 ways to get different type of memory
# os.system("adb shell vmstat")
# os.system("adb shell top") # memory for each application
result_dictionary = Phone.__get_adb_dictionary(
'shell "cat /proc/meminfo"')
mem_total = Convert.get_number(result_dictionary['MemTotal'])
if mem_total is None or mem_total == 0:
return -1
mem_free = Convert.get_number(result_dictionary['MemFree'])
if mem_free is None:
return -1
return int(mem_free) * 100 / int(mem_total)
def main(argv):
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_imu = IMU(_config, Level.INFO)
while True:
# x, y, z = _imu.read_magnetometer()
mag = _imu.read_magnetometer()
# ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro()
acc = _imu.read_accelerometer_gyro()
heading = Convert.heading_from_magnetometer(_imu._amin, _imu._amax, mag)
print(Fore.CYAN + 'Accel: {:05.2f} {:05.2f} {:05.2f} '.format(acc[0], acc[1], acc[2]) \
+ Fore.YELLOW + '\tGyro: {:05.2f} {:05.2f} {:05.2f} '.format(acc[3], acc[4], acc[5]) \
+ Fore.MAGENTA + '\tMag: {:05.2f} {:05.2f} {:05.2f} '.format(mag[0], mag[1], mag[2]) \
+ Fore.GREEN + '\tHeading: {:d}°'.format(heading) + Style.RESET_ALL)
time.sleep(0.25)
except KeyboardInterrupt:
print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...')
except Exception:
print(Fore.RED + Style.BRIGHT + 'error starting imu: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
def _read(self):
'''
The function that reads sensor values in a loop. This checks to see
if the 'sys' calibration is at least 3 (True), and if the Euler and
Quaternion values are within an error range of each other, this sets
the class variable for heading, pitch and roll. If they aren't within
range for more than n polls, the values revert to None.
'''
self._log.info('starting sensor read...')
_heading = None
_pitch = None
_roll = None
_quat_w = None
_quat_x = None
_quat_y = None
_quat_z = None
count = 0
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
# grab data at 10Hz
rate = Rate(10)
while not self._closed:
if self._enabled:
header = 91
print(Fore.CYAN + ('-'*header) + Style.RESET_ALL)
print(Fore.CYAN + "| {:17} | {:20} | {:20} | {:21} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading") + Style.RESET_ALL)
print(Fore.CYAN + ('-'*header) + Style.RESET_ALL)
for _ in range(10):
a, m, g = self._imu.get()
r, p, h = self._imu.getOrientation(a, m)
deg = Convert.to_degrees(h)
# self._log.info(Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h) + Style.RESET_ALL)
print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} '.format(a[0], a[1], a[2])
+ Fore.YELLOW + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format(m[0], m[1], m[2])
+ Fore.MAGENTA + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format(g[0], g[1], g[2])
+ Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL)
time.sleep(0.50)
print('-'*header)
print(' uT: micro Tesla')
print(' g: gravity')
print('dps: degrees per second')
print('')
# accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2
# mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
# gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
#
# print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\
# + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\
# + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL)
rate.wait()
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')
def _test_offset(log, angle, offset, expected):
log.heading(
'Rotate Test', '{:5.2f}°, offset {:5.2f}°, expected {:5.2f}°'.format(
angle, offset, expected), '1/4')
result = Convert.offset_in_degrees(angle, offset)
if not math.isclose(result, expected):
log.error('expected {:5.2f}°, not {:5.2f}°\n'.format(expected, result))
# now radians...
result2 = Convert.to_degrees(
Convert.offset_in_radians(Convert.to_radians(angle),
Convert.to_radians(offset)))
if not math.isclose(result2, expected):
log.error('expected {:5.2f}°, not {:5.2f}°\n'.format(
expected, result2))
log.info(Fore.GREEN + 'result:\t{:>5.2f}°'.format(result))
def test_offset_rotate():
_log = Logger("offset-rotate-test", Level.INFO)
_count = 0
for offset in arange(0.0, 360.0, 90.0):
_count += 1
_log.heading('Offset Rotate Test', 'offset={:<5.2f};'.format(offset),
'{:d}/4'.format(_count))
for angle in arange(0.0, 360.0, 30.0):
_alt = Convert.offset_in_degrees(angle, offset)
_log.info(Fore.GREEN +
'from {:>5.2f}°\tto {:>5.2f}°;'.format(angle, _alt) +
Fore.BLACK + '\toffset={:<5.2f};'.format(offset))
time.sleep(0.01)
_log.info('complete.')
def test_bno08x(loop=False):
_log = Logger("bno085-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
if not _i2c_scanner.has_address([0x4a]):
_log.warning('test ignored: no bno085 found.')
return
_log.info('starting test...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_bno = BNO08x(_config, _queue, Level.INFO)
# begin ............
_log.info('starting BNO08x read loop...')
_count = 0
_errors = 0
_amin = list(_bno.magneto())
_amax = list(_bno.magneto())
while _count < 10 if not loop else True:
_count += 1
mag = _bno.magneto()
if mag is not None:
heading = Convert.heading_from_magnetometer(_amin, _amax, mag)
_log.info('\tMag: {:05.2f} {:05.2f} {:05.2f} '.format(mag[0], mag[1], mag[2]) \
+ Fore.YELLOW + '\tHeading: {:d}°'.format(heading))
else:
_log.info('null result')
# result = _bno.read()
# if result != None:
# _log.info('[{:>3d}] result: {:>5.2f} | {:>5.2f} | {:>5.2f} |\t'.format(_count, result[0], result[1], result[2]) + Fore.BLACK + '{:d} errors.'.format(_errors))
# else:
# _errors += 1
# _log.warning('[{:>3d}] result: NA'.format(_count))
time.sleep(0.25)
def main(argv):
_rgbmatrix = RgbMatrix(Level.INFO)
_rgbmatrix.set_display_type(DisplayType.SOLID)
_rgbmatrix.enable()
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_imu = IMU(_config, Level.INFO)
while True:
# x, y, z = _imu.read_magnetometer()
mag = _imu.read_magnetometer()
# ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro()
acc = _imu.read_accelerometer_gyro()
_heading = Convert.heading_from_magnetometer(
_imu._amin, _imu._amax, mag)
_cardinal = Cardinal.get_heading_from_degrees(_heading)
_color = Cardinal.get_color_for_direction(_cardinal)
_rgbmatrix.set_color(_color)
print(Fore.CYAN + 'Accel: {:05.2f} {:05.2f} {:05.2f} '.format(acc[0], acc[1], acc[2]) \
+ Fore.YELLOW + '\tGyro: {:05.2f} {:05.2f} {:05.2f} '.format(acc[3], acc[4], acc[5]) \
+ Fore.MAGENTA + '\tMag: {:05.2f} {:05.2f} {:05.2f} '.format(mag[0], mag[1], mag[2]) \
+ Fore.GREEN + '\tHeading: {:d}°;\tcardinal: {}'.format(_heading, _cardinal.display) + Style.RESET_ALL)
time.sleep(0.25)
except KeyboardInterrupt:
print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...')
except Exception:
print(Fore.RED + Style.BRIGHT +
'error starting imu: {}'.format(traceback.format_exc()) +
Style.RESET_ALL)
finally:
if _rgbmatrix:
_rgbmatrix.set_color(Color.BLACK)
def main():
args = arguments(__file__, FEED_INPUT)
conv = Convert(args, FEED_INPUT, extra=lvz_fetch_full)
return 0 if Generate(conv)(BASE_URL) else 1
def __get_adb_dictionary(command):
text = Phone.__get_adb(command)
return Convert.get_dictionary(text)
def read(self):
'''
The function that reads sensor values in a loop. This checks to see
if the 'sys' calibration is at least 3 (True), and if the Euler and
Quaternion values are within an error range of each other, this sets
the class variable for heading, pitch and roll. If they aren't within
range for more than n polls, the values revert to None.
'''
# self._log.info('starting sensor read...')
try:
# reports ......................................
# _confidence = self._activity_report()
# self._stability_report()
# _calibration_status = self._calibration_report()
# if _calibration_status >= self._min_calib_status:
if True:
# Accelerometer ..................................................................
# gyro_x, gyro_y, gyro_z = self._bno.gyro # pylint:disable=no-member
# self._log.info(Fore.RED + 'Gyroscope:\tX: {:0.6f} Y: {:0.6f} Z: {:0.6f} rads/s'.format(gyro_x, gyro_y, gyro_z))
# Magnetometer ...................................................................
# self._log.info(Fore.BLACK + 'self._bno.magnetic...')
mag_x, mag_y, mag_z = self._bno.magnetic # pylint:disable=no-member
_mag_degrees = Convert.convert_to_degrees(mag_x, mag_y, mag_z)
if self._heading_trim != 0.0:
_mag_degrees = Convert.offset_in_degrees(
_mag_degrees, self._heading_trim)
self._log.info(
Fore.YELLOW +
'heading: {:>6.2f}°\t(magneto)'.format(_mag_degrees))
# Rotation Vector Quaternion .....................................................
# self._log.info(Fore.BLACK + 'self._bno.quaternion...')
(quat_i, quat_j, quat_k, quat_real) = self._bno.quaternion # pylint:disable=no-member
_quaternion = self._bno.quaternion
self._process_quaternion(Fore.GREEN, 'rot-quat',
self._bno.quaternion)
# Geomagnetic Rotation Vector Quatnernion ........................................
# self._log.info(Fore.BLACK + 'self._bno.geometric_quaternion...')
_geomagnetic_quaternion = self._bno.geomagnetic_quaternion
(
geo_quat_i,
geo_quat_j,
geo_quat_k,
geo_quat_real,
) = _geomagnetic_quaternion # pylint:disable=no-member
self._process_quaternion(Fore.GREEN + Style.BRIGHT, 'geo-quat',
_geomagnetic_quaternion)
return _mag_degrees, _quaternion[0], _geomagnetic_quaternion[0]
else:
self._log.warning('uncalibrated...')
return 0.0, 0.0, 0.0
self._log.debug('read ended.')
except KeyError as ke:
if self._verbose:
self._log.error('bno08x key error: {} {}'.format(
ke, traceback.format_exc()))
else:
self._log.error('bno08x key error: {}'.format(ke))
except RuntimeError as re:
if self._verbose:
self._log.error('bno08x runtime error: {} {}'.format(
re, traceback.format_exc()))
else:
self._log.error('bno08x runtime error: {}'.format(re))
except IndexError as ie:
if self._verbose:
self._log.error('bno08x index error: {} {}'.format(
ie, traceback.format_exc()))
else:
self._log.error('bno08x index error: {}'.format(ie))
except OSError as oe:
if self._verbose:
self._log.error('bno08x os error: {} {}'.format(
oe, traceback.format_exc()))
else:
self._log.error('bno08x OS error: {}'.format(oe))
except PacketError as pe:
if self._verbose:
self._log.error('bno08x packet error: {} {}'.format(
pe, traceback.format_exc()))
else:
self._log.error('bno08x packet error: {}'.format(pe))
except Exception as e:
if self._verbose:
self._log.error('bno08x error: {} {}'.format(
e, traceback.format_exc()))
else:
self._log.error('bno08x error: {}'.format(e))
def read(self):
'''
The function that reads multiple sensor values. This checks to see if
the 'sys' calibration is at least 3 (True), and if the Euler and
Quaternion values are within an error range of each other, this sets
the class variable self._heading, pitch and roll. If they aren't within
range for more than n polls, the values revert to None.
Returns a tuple containing the calibration status followed by the
Euler and Quaternion headings.
'''
self._log.debug('starting sensor read...')
_e_heading, _e_pitch, _e_roll, _e_yaw, _orig_quat_heading, _euler_converted_heading, _q_pitch, _q_roll, _q_yaw = [
None
] * 9
_quat_w = 0
_quat_x = 0
_quat_y = 0
_quat_z = 0
self.get_calibration_status()
_euler = self._bno055.euler
if _euler != None and _euler[0] != None:
# self._log.info(Fore.BLUE + Style.NORMAL + 'euler: {:>5.4f}°'.format(_euler))
# self._log.info(Fore.BLUE + Style.NORMAL + 'euler: {:>5.4f}°'.format(_euler[0]))
_orig_euler_heading = _euler[0]
_euler_converted_heading = Convert.offset_in_degrees(
_orig_euler_heading, self._euler_heading_trim)
self._log.info(
Fore.BLUE + Style.NORMAL +
'heading:\t{:>9.2f}°\torig: {:>9.2f}°\ttrim: {:>9.2f}°; euler'.
format(_euler_converted_heading, _orig_euler_heading,
self._euler_heading_trim))
else:
self._log.info(Fore.BLUE + Style.DIM + 'heading:\tNA\t(euler)')
# BNO055 Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ..............
_quat = self._bno055.quaternion
if _quat != None:
_quat_w = _quat[0] if _quat[0] != None else None
_quat_x = _quat[1] if _quat[1] != None else None
_quat_y = _quat[2] if _quat[2] != None else None
_quat_z = _quat[3] if _quat[3] != None else None
if _quat_w != None \
and _quat_x != None \
and _quat_y != None \
and _quat_z != None:
_q = Quaternion(_quat_w, _quat_x, _quat_y, _quat_z)
_orig_quat_heading = _q.degrees
_q_yaw_pitch_roll = _q.yaw_pitch_roll
_q_yaw = _q_yaw_pitch_roll[0]
_q_pitch = _q_yaw_pitch_roll[1]
_q_roll = _q_yaw_pitch_roll[2]
if _orig_quat_heading < 0.0:
print(Fore.YELLOW + Style.BRIGHT + 'HEADING+360' +
Style.RESET_ALL)
_orig_quat_heading += 360.0
# heading ............................................................
# self._log.debug(Fore.BLUE + Style.NORMAL + '_quat_w={:>5.4f}, _quat_x={:>5.4f}, _quat_y={:>5.4f}, _quat_z={:>5.4f}'.format(_quat_w, _quat_x, _quat_y, _quat_z) )
if self._is_calibrated:
self._heading = Convert.offset_in_degrees(
_orig_quat_heading, self._quat_heading_trim)
self._log.info(Fore.MAGENTA + 'heading:\t{:>9.2f}°\t'.format(self._heading) \
+ Style.DIM + 'orig: {:>9.2f}°\ttrim: {:>9.2f}°; quat; mode: {})'.format(_orig_quat_heading, self._quat_heading_trim, self._bno_mode.name.lower()))
# + Fore.CYAN + 'p={:>5.4f}; r={:>5.4f}; y={:>5.4f}'.format(_q_pitch, _q_roll, _q_yaw))
else:
# we don't do an update if not calibrated
self._log.info(Fore.MAGENTA + 'heading:\t{:>6.2f}°\t'.format(self._heading) \
+ Fore.BLACK + Style.DIM + '(quat; UNCHANGED; mode: {})'.format(self._bno_mode.name.lower()))
# # # pitch ..............................................................
# # if Convert.in_range(_e_pitch, _q_pitch, self._error_range):
# # self._pitch = _e_pitch + self._pitch_trim
# # self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (calibrated)'.format(_e_pitch))
# # else:
# # self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'pitch: \t{:>5.4f}° (euler)'.format(_e_pitch))
# # self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (quaternion)'.format(_q_pitch))
# # self._pitch = None
# # # roll ...............................................................
# # if Convert.in_range(_e_roll, _q_roll, self._error_range):
# # self._roll = _e_roll + self._roll_trim
# # self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'roll: \t{:>5.4f}° (calibrated)'.format(_e_roll))
# # else:
# # self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'roll: \t{:>5.4f}° (euler)'.format(_e_roll))
# # self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'roll: \t{:>5.4f}° (quaternion)'.format(_q_roll))
# # self._roll = None
else:
self._log.info(Fore.BLACK +
'null quaternion components {}.'.format(_quat))
else:
self._log.info(Fore.BLACK + 'null quaternion.')
pass
self._log.debug('read ended.')
return (self._is_calibrated, _euler_converted_heading, self._heading)