mirror of
https://forge.apps.education.fr/blender-edutech/blender-edutech-tutoriels.git
synced 2024-01-27 09:42:33 +01:00
254 lines
7.2 KiB
Python
254 lines
7.2 KiB
Python
g# Copyright (c) 2016, Xilinx, Inc.
|
|
# SPDX-License-Identifier: BSD-3-Clause
|
|
|
|
|
|
import math
|
|
from . import Arduino
|
|
from . import ARDUINO_GROVE_I2C
|
|
|
|
|
|
|
|
|
|
ARDUINO_GROVE_IMU_PROGRAM = "arduino_grove_imu.bin"
|
|
CONFIG_IOP_SWITCH = 0x1
|
|
GET_ACCL_DATA = 0x3
|
|
GET_GYRO_DATA = 0x5
|
|
GET_COMPASS_DATA = 0x7
|
|
GET_TEMPERATURE = 0xB
|
|
GET_PRESSURE = 0xD
|
|
RESET = 0xF
|
|
|
|
|
|
def _reg2float(reg):
|
|
"""Converts 32-bit register value to floats in Python.
|
|
|
|
Parameters
|
|
----------
|
|
reg: int
|
|
A 32-bit register value read from the mailbox.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
A float number translated from the register value.
|
|
|
|
"""
|
|
if reg == 0:
|
|
return 0.0
|
|
sign = (reg & 0x80000000) >> 31 & 0x01
|
|
exp = ((reg & 0x7f800000) >> 23) - 127
|
|
if exp == 0:
|
|
man = (reg & 0x007fffff) / pow(2, 23)
|
|
else:
|
|
man = 1 + (reg & 0x007fffff) / pow(2, 23)
|
|
result = pow(2, exp) * man * ((sign * -2) + 1)
|
|
return float("{0:.2f}".format(result))
|
|
|
|
|
|
def _reg2int(reg):
|
|
"""Converts 32-bit register value to signed integer in Python.
|
|
|
|
Parameters
|
|
----------
|
|
reg: int
|
|
A 32-bit register value read from the mailbox.
|
|
|
|
Returns
|
|
-------
|
|
int
|
|
A signed integer translated from the register value.
|
|
|
|
"""
|
|
result = -(reg >> 31 & 0x1) * (1 << 31)
|
|
for i in range(31):
|
|
result += (reg >> i & 0x1) * (1 << i)
|
|
return result
|
|
|
|
|
|
class Grove_IMU(object):
|
|
"""This class controls the Grove IIC IMU.
|
|
|
|
Grove IMU 10DOF is a combination of grove IMU 9DOF (MPU9250) and grove
|
|
barometer sensor (BMP180). MPU-9250 is a 9-axis motion tracking device
|
|
that combines a 3-axis gyroscope, 3-axis accelerometer, 3-axis
|
|
magnetometer and a Digital Motion Processor (DMP). BMP180 is a high
|
|
precision, low power digital pressure sensor. Hardware version: v1.1.
|
|
|
|
Attributes
|
|
----------
|
|
microblaze : Arduino
|
|
Microblaze processor instance used by this module.
|
|
|
|
"""
|
|
def __init__(self, mb_info, gr_pin):
|
|
"""Return a new instance of an Grove IMU object.
|
|
|
|
Parameters
|
|
----------
|
|
mb_info : dict
|
|
A dictionary storing Microblaze information, such as the
|
|
IP name and the reset name.
|
|
gr_pin: list
|
|
A group of pins on arduino-grove shield.
|
|
|
|
"""
|
|
if gr_pin not in [ARDUINO_GROVE_I2C]:
|
|
raise ValueError("Group number can only be I2C.")
|
|
|
|
self.microblaze = Arduino(mb_info, ARDUINO_GROVE_IMU_PROGRAM)
|
|
self.reset()
|
|
|
|
def reset(self):
|
|
"""Reset all the sensors on the grove IMU.
|
|
|
|
Returns
|
|
-------
|
|
None
|
|
|
|
"""
|
|
self.microblaze.write_blocking_command(RESET)
|
|
|
|
def get_accl(self):
|
|
"""Get the data from the accelerometer.
|
|
|
|
Returns
|
|
-------
|
|
list
|
|
A list of the acceleration data along X-axis, Y-axis, and Z-axis.
|
|
|
|
"""
|
|
self.microblaze.write_blocking_command(GET_ACCL_DATA)
|
|
data = self.microblaze.read_mailbox(0, 3)
|
|
[ax, ay, az] = [_reg2int(i) for i in data]
|
|
return [float("{0:.2f}".format(ax / 16384)),
|
|
float("{0:.2f}".format(ay / 16384)),
|
|
float("{0:.2f}".format(az / 16384))]
|
|
|
|
def get_gyro(self):
|
|
"""Get the data from the gyroscope.
|
|
|
|
Returns
|
|
-------
|
|
list
|
|
A list of the gyro data along X-axis, Y-axis, and Z-axis.
|
|
|
|
"""
|
|
self.microblaze.write_blocking_command(GET_GYRO_DATA)
|
|
data = self.microblaze.read_mailbox(0, 3)
|
|
[gx, gy, gz] = [_reg2int(i) for i in data]
|
|
return [float("{0:.2f}".format(gx * 250 / 32768)),
|
|
float("{0:.2f}".format(gy * 250 / 32768)),
|
|
float("{0:.2f}".format(gz * 250 / 32768))]
|
|
|
|
def get_compass(self):
|
|
"""Get the data from the magnetometer.
|
|
|
|
Returns
|
|
-------
|
|
list
|
|
A list of the compass data along X-axis, Y-axis, and Z-axis.
|
|
|
|
"""
|
|
self.microblaze.write_blocking_command(GET_COMPASS_DATA)
|
|
data = self.microblaze.read_mailbox(0, 3)
|
|
[mx, my, mz] = [_reg2int(i) for i in data]
|
|
return [float("{0:.2f}".format(mx * 1200 / 4096)),
|
|
float("{0:.2f}".format(my * 1200 / 4096)),
|
|
float("{0:.2f}".format(mz * 1200 / 4096))]
|
|
|
|
def get_heading(self):
|
|
"""Get the value of the heading.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The angle deviated from the X-axis, toward the positive Y-axis.
|
|
|
|
"""
|
|
[mx, my, _] = self.get_compass()
|
|
heading = 180 * math.atan2(my, mx) / math.pi
|
|
if heading < 0:
|
|
heading += 360
|
|
return float("{0:.2f}".format(heading))
|
|
|
|
def get_tilt_heading(self):
|
|
"""Get the value of the tilt heading.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The tilt heading value.
|
|
|
|
"""
|
|
[ax, ay, _] = self.get_accl()
|
|
[mx, my, mz] = self.get_compass()
|
|
|
|
try:
|
|
pitch = math.asin(-ax)
|
|
roll = math.asin(ay / math.cos(pitch))
|
|
except ZeroDivisionError:
|
|
raise RuntimeError("Value out of range or device not connected.")
|
|
|
|
xh = mx * math.cos(pitch) + mz * math.sin(pitch)
|
|
yh = mx * math.sin(roll) * math.sin(pitch) + \
|
|
my * math.cos(roll) - mz * math.sin(roll) * math.cos(pitch)
|
|
_ = -mx * math.cos(roll) * math.sin(pitch) + \
|
|
my * math.sin(roll) + mz * math.cos(roll) * math.cos(pitch)
|
|
tilt_heading = 180 * math.atan2(yh, xh) / math.pi
|
|
if yh < 0:
|
|
tilt_heading += 360
|
|
return float("{0:.2f}".format(tilt_heading))
|
|
|
|
def get_temperature(self):
|
|
"""Get the current temperature in degree C.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The temperature value.
|
|
|
|
"""
|
|
self.microblaze.write_blocking_command(GET_TEMPERATURE)
|
|
value = self.microblaze.read_mailbox(0)
|
|
return _reg2float(value)
|
|
|
|
def get_pressure(self):
|
|
"""Get the current pressure in Pa.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The pressure value.
|
|
|
|
"""
|
|
self.microblaze.write_blocking_command(GET_PRESSURE)
|
|
value = self.microblaze.read_mailbox(0)
|
|
return _reg2float(value)
|
|
|
|
def get_atm(self):
|
|
"""Get the current pressure in relative atmosphere.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The related atmosphere.
|
|
|
|
"""
|
|
return float("{0:.2f}".format(self.get_pressure() / 101325))
|
|
|
|
def get_altitude(self):
|
|
"""Get the current altitude.
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The altitude value.
|
|
|
|
"""
|
|
pressure = self.get_pressure()
|
|
a = pressure / 101325
|
|
b = 1 / 5.255
|
|
c = 1 - pow(a, b)
|
|
altitude = 44300 * c
|
|
return float("{0:.2f}".format(altitude))
|