def init():
# Setup I2C connection with BNO055
rst_pin = 'P9_12'
device = get_platform()
if device == PLATFORM[0]: # BBB
bno = BNO055.BNO055(rst=rst_pin) # I2C2 bus number is 1
elif device == PLATFORM[1]: #BBB_WIFI
# Modify BNO055.py from Adafruit by adding kw argument busnum = None
bno = BNO055.BNO055(rst=rst_pin, busnum=2) # I2C2 bus number is 2
# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
# FileIO setup
myFile = uh.FileIO(subjID=subjID, logfilekey=filekey)
myFile.make_expfiles()
txtheader = 'Time Acc_x Acc_y Acc_z Heading Roll Pitch gaitEvent\n'
myFile.logfile['IMU'].write(txtheader)
timer = time
return bno, myFile, timer
def __init__(self,
calibration_data=None,
axis_remap=None,
sensor_update_frequency_hz=20.0):
# Initialize BNO sensor with up to one retry
try:
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
self._sensor = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
self._sensor.begin()
except RuntimeError:
# Second try, just in case
self._sensor = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
self._sensor.begin()
if axis_remap is not None:
self._sensor.set_axis_remap(**axis_remap)
if calibration_data is not None:
self._sensor.set_calibration(calibration_data)
# The 5 elements of the shared memory array are (update count, update time, linear_acceleration_x, linear_acceleration_y, linear_acceleration_z)
self._multiproc_shared_data = multiproc.Array('d', 5)
self._condition = multiproc.Condition()
self._state = {}
self.sensor_update_frequency_hz = sensor_update_frequency_hz
# TODO: it's probably bad form to spawn a process within the constructor of an object. Could move this
# to a public start() method
sensor_proc = multiproc.Process(
target=read_bno_multiproc,
args=(self._sensor, self.sensor_update_frequency_hz,
self._multiproc_shared_data, self._condition))
sensor_proc.daemon = True # Don't let the BNO reading thread block exiting.
sensor_proc.start()
def __init__(self):
self.sys = None
self.gyro = None
self.accel = None
self.mag = None
self.heading = None
self.roll = None
self.pitch = None
self.temp_c = None
self.bno = BNO055.BNO055(serial_port='/dev/ttyUSB0',
rst=18) # apparently rst value is not needed
if not self.bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = self.bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning')
print('Reading BNO055 data...')
print('')
self.readAll()
def __init__(self):
self.bno = BNO055.BNO055(serial_port='/dev/serial0')
self.IMU_pub = rospy.Publisher('/IMU', IMUOrientation, queue_size=1)
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
logging.basicConfig(level=logging.DEBUG)
if not self.bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = self.bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = self.bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
print('Reading BNO055 data, press Ctrl-C to quit...')
def __init__(self):
# use i2c by not passing arg in constructor
self.bno055 = BNO055.BNO055()
if not self.bno055.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
self.__self_test()
def init_bno():
from Adafruit_BNO055 import BNO055
bno = BNO055.BNO055(rst=13)
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
logging.basicConfig(level=logging.DEBUG)
# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
print('Reading BNO055 data, press Ctrl-C to quit...')
return bno
def __init__(self):
self.bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
# Initialize the self.bno055 and stop if something went wrong.
if not self.bno.begin():
raise RuntimeError(
'Failed to initialize bno055! Is the sensor connected?')
# logging.debug system status and self test result.
status, self_test, error = self.bno.get_system_status()
logging.debug('System status: {0}'.format(status))
logging.debug(
'Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# logging.debug out an error if system status is in error mode.
if status == 0x01:
logging.debug('System error: {0}'.format(error))
logging.debug('See datasheet section 4.3.59 for the meaning.')
# logging.debug self.bno055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = self.bno.get_revision()
logging.debug('Software version: {0}'.format(sw))
logging.debug('Bootloader version: {0}'.format(bl))
logging.debug('Accelerometer ID: 0x{0:02X}'.format(accel))
logging.debug('Magnetometer ID: 0x{0:02X}'.format(mag))
logging.debug('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
def initBNO(self):
self.bno = BNO055.BNO055(serial_port='/dev/serial0')
# Enable verbose debug logging if -v is passed as a parameter.
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
logging.basicConfig(level=logging.DEBUG)
# Initialize the BNO055 and stop if something went wrong.
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = self.bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = self.bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
self.calibrateBNO()
def run(self):
# Create an ew BNO055 instance
bno = BNO055.BNO055(serial_port='/dev/ttyUSB0')
# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# read the acceleration data
print('Reading BNO055 data...')
while True:
# Read the linear acceleration (without gravity)
x, y, z = bno.read_linear_acceleration()
if abs(x) > 0.7 or abs(y) > 0.7 or abs(z) > 0.7:
print('x={0:07.2F} y={1:07.2F} z={2:07.2F}'.format(x, y, z))
self.mutex.acquire(blocking=True, timeout=-1)
self.moved = True
self.mutex.release()
# Sleep for a second until the next reading.
time.sleep(0.1)
def main():
imupub = rospy.Publisher("imu", sensor_msgs.Imu, queue_size=10)
rospy.init_node("imu", anonymous=False)
bno = BNO055.BNO055()
if not bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
rate = rospy.Rate(60)
while not rospy.is_shutdown():
imumsg = sensor_msgs.Imu()
sys, gyro, accel, mag = bno.get_calibration_status()
qx, qy, qz, qw = bno.read_quaternion()
imumsg.orientation.x = qx
imumsg.orientation.y = qy
imumsg.orientation.z = qz
imumsg.orientation.w = qw
lax, lay, laz = bno.read_linear_acceleration()
imumsg.linear_acceleration.x = lax
imumsg.linear_acceleration.y = lay
imumsg.linear_acceleration.z = laz
#rospy.loginfo(imumsg)
imupub.publish(imumsg)
rate.sleep()
def Start(self):
if not self.started:
operational = False
self.bno = BNO055.BNO055(serial_port='/dev/serial0')
for i in range(10):
try:
if not self.bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?'
)
else:
operational = True
break
except RuntimeError:
pass
if not operational:
raise RuntimeError('Cant connect to compass after 10 attempts')
self.SetValues([
240, 255, 216, 255, 240, 255, 222, 246, 167, 3, 91, 241, 254,
255, 254, 255, 255, 255, 232, 3, 225, 2
])
print("Making sure not reading 0 heading")
while True:
if self.Heading() != 0:
break
print("You need to shake to stop heading being 0")
print("now safe to put down")
self.started = True
def initBNO():
checkFileExists()
# Create and configure the BNO sensor connection. Make sure only ONE of the
# below 'bno = ...' lines is uncommented:
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
# BeagleBone Black configuration with default I2C connection (SCL=P9_19, SDA=P9_20),
# and RST connected to pin P9_12:
#bno = BNO055.BNO055(rst='P9_12')
# Enable verbose debug logging if -v is passed as a parameter.
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
logging.basicConfig(level=logging.DEBUG)
# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
def bno_init():
"""
Initializes the BNO055 sensor. Handles when BNO connection fails.
"""
global bno
bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
#if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
# logging.basicConfig(level=logging.DEBUG)
while True:
try:
if not bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
status, self_test, error = bno.get_system_status()
break
except Exception as e:
print("error :{}".format(e))
time.sleep(0.5)
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
def setup(self, serial_port='/dev/serial0', rst=18):
"""
Performs one-time hardware setup and configuration nescessary before
reading data from the sensor.
"""
self.bno = BNO055.BNO055(serial_port=serial_port, rst=rst)
# allow multiple attempts to establish hardware connection
error_count = 0
while True:
try:
status = self.bno.begin()
break
except RuntimeError as e:
self.logger.warn(
"IMUSensor: RuntimeError encountered during setup")
error_count += 1
if error_count >= MAX_STARTUP_ERRORS:
raise RuntimeError("cannot startup BNO055 sensor")
time.sleep(2)
# verify/report system status
status, self_test, error = self.bno.get_system_status()
status = "good" if status == STATUS_GOOD else "bad"
self_test = "good" if self_test == SELF_TEST_GOOD else "bad"
self.logger.info(
"IMUSensor: status: {}, self_test: {}, error: {}".format(
status, self_test, error))
if status == "good" and self_test == "good" and error == 0:
self.logger.info("IMUSensor: ready")
else:
raise RuntimeError("IMUSensor: something is wrong")
super(IMUSensor, self).setup()
def __init__(self):
'''
Initializes all the sensors we want to consume data from
'''
# I2C bus readers
self.adc = Adafruit_ADS1x15.ADS1115()
self.imu = BNO055.BNO055()
if not self.imu.begin():
raise RuntimeError(
'Failed to initialize IMU! Is the sensor connected?')
# Distance sensors
self.leftDistSensor = DistanceSensor(
echo=Constants.DIST_LEFT_ECHO_PIN,
trigger=Constants.DIST_LEFT_TRIGGER_PIN,
max_distance=Constants.DIST_MAX_DISTANCE,
queue_len=Constants.DIST_QUEUE_LENGTH)
self.rightDistSensor = DistanceSensor(
echo=Constants.DIST_RIGHT_ECHO_PIN,
trigger=Constants.DIST_RIGHT_TRIGGER_PIN,
max_distance=Constants.DIST_MAX_DISTANCE,
queue_len=Constants.DIST_QUEUE_LENGTH)
# Sensor values (-0 means that the sensor is not reading data)
self.steeringPotValue = -0
self.leftTachValue = -0
self.rightTachValue = -0
self.leftDistance = -0
self.rightDistance = -0
self.heading = -0
self.roll = -0
self.pitch = -0
self.sysCal = -0
self.gyroCal = -0
self.accelCal = -0
self.magCal = -0
def start(self):
if not self.started:
connected=False # allows multiple attempts at connecting as initialization has a suboptimal success rate
self.bno=BNO055.BNO055(serial_port='/dev/serial0')
for i in range(10):
try:
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
else:
connected = True
break
except RuntimeError:
pass
if not connected:
raise RuntimeError("Compass not found after 10 attempts")
self.SetValues([240, 255, 216, 255, 240, 255, 222, 246, 167, 3, 91, 241, 254, 255, 254, 255, 255, 255, 232, 3, 225, 2]) # initial values to make calibration easier
while True:
if self.getHeading() != 0:
break
print("Please shake to initialize")
time.sleep(0.3)
print("Please place on a flat surface for finnish initialization")
self.calibrate()
print("Initialization complete")
self.started = True
def __init__(self, use_BNO=True):
#Constants
self.delay_time = .001
self.use_BNO = use_BNO
#I2C Initialization
self.i2c = busio.I2C(board.SCL, board.SDA)
if use_BNO:
#BNO Initialization
#self.bno = adafruit_bno055.BNO055(self.i2c)
self.bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
#self.bno.use_external_crystal = True
else:
self.bno_euler = [0, 0, 0]
#MPL Initialization
self.mpl = adafruit_mpl3115a2.MPL3115A2(self.i2c)
self.mpl._ctrl_reg1 = adafruit_mpl3115a2._MPL3115A2_CTRL_REG1_OS1 | adafruit_mpl3115a2._MPL3115A2_CTRL_REG1_ALT
time.sleep(1)
self.zero_mpl()
# self.led = LED(21)
#Initialize ADXL
self.adxl = adafruit_adxl34x.ADXL345(self.i2c)
self.adxl.range = adafruit_adxl34x.Range.RANGE_16_G
self.adxl.data_rate = adafruit_adxl34x.DataRate.RATE_100_HZ
def imu_compass():
global heading
x_offset = 3.5
y_offset = .6
bno = BNO055.BNO055(rst='P9_12')
#for calibration
xmax = 0
ymax = 0
xmin = 0
ymin = 0
while True:
x_out, y_out, z_out = bno.read_magnetometer()
h1 = math.atan2(y_out + y_offset, x_out + x_offset)
#print 'x_out ',x_out
#print 'y_out ',y_out
if (h1 < 0):
h1 += 2 * math.pi
#for calibration
heading = math.degrees(h1)
if x_out > xmax:
xmax = x_out
if y_out > ymax:
ymax = y_out
if x_out < xmin:
xmin = x_out
if y_out < ymin:
ymin = y_out
print str((xmax + xmin) / 2)
print str((ymax + ymin) / 2)
print str((xmax - xmin) / 2)
print str((ymax - ymin) / 2)
time.sleep(.05)
def __init__(self):
# IMU Reset Pin connected to Pin 18
self._bno = _BNO055.BNO055(rst=18)
# Fail if it cannot be initialized
if not self._bno.begin():
raise RuntimeError('Failed to initialize BNO055!')
self._data = {
'euler': {
# Resolution found from a forumn post
'yaw': 0, # Rotation about z axis (vertical) +/- 0.01 degree
'roll': 0, # Rotation about y axix (perpindicular to the pins IMU) +/- 0.01 degree
'pitch': 0, # Rotation about x axis (parallel to the pins of IMU) +/- 0.01 degree
},
'gyro': {
'x': 0, # 3e-2 degree/sec
'y': 0, # 3e-2 degree/sec
'z': 0, # 3e-2 degree/sec
},
'acceleration': {
'x': 0, # +/- 5e-4 g
'y': 0, # +/- 5e-4 g
'z': 0, # +/- 5e-4 g
},
'linear_acceleration': {
'x': 0, # +/- 0.25 m/s^2
'y': 0, # +/- 0.25 m/s^2
'z': 0, # +/- 0.25 m/s^2
},
'temp': 0, # Good enough
}
def __init__(self):
self.bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
# Initialize the BNO055 and stop if something went wrong.
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
# Load Calibration profile
self.load_calibration_profile()
self.calibration_status()
def __init__(self, poll_delay=0.01):
# Create and configure the BNO sensor connection. Make sure only ONE of the
# below 'bno = ...' lines is uncommented:
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
self.bno = b.BNO055(serial_port='/dev/ttyAMA0', rst=21)
# Initialize the BNO055 and stop if something went wrong.
if not self.bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
# Print system status and self test result.
status, self_test, error = self.bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# LOAD json
CALIBRATION_FILE = os.path.join(
os.path.split(os.path.realpath(__file__))[0], 'calibration.json')
try:
with open(CALIBRATION_FILE, 'r') as cal_file:
data = json.load(cal_file)
self.bno.set_calibration(data)
except IOError:
print("404 - Calibration file not found")
# calibration process
calibrated = False
while not calibrated:
sys, gyro, accel, mag = self.bno.get_calibration_status()
print('sys=',
sys,
' gyro=',
gyro,
' accel=',
accel,
' mag=',
mag,
end='\r',
flush=True)
time.sleep(0.2)
if sys == 3 and gyro == 3 and accel == 3 and mag == 3:
calibrated = True
print('\nCalibration Complete!')
# save the lastest calibration json file
data = self.bno.get_calibration()
with open(CALIBRATION_FILE, 'w') as cal_file:
json.dump(data, cal_file)
self.heading = self.roll = self.pitch = self.sys = self.gyro = self.accel = self.mag = \
self.ori_x = self.ori_y = self.ori_z = self.ori_w = self.temp_c = self.mag_x = self.mag_y = \
self.mag_z = self.gyr_x = self.gyr_y = self.gyr_z = self.acc_x = self.acc_y = self.acc_z = \
self.lacc_x = self.lacc_y = self.lacc_z = self.gra_x = self.gra_y = self.gra_z = 0
self.poll_delay = poll_delay
self.on = True
self.vm = 0
self.vp = 0
def __init__(self):
rospy.init_node('imu_node', anonymous=True)
self.bno = BNO055.BNO055(busnum = 0)
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
self.bno_status_and_test()
self.pub_imu_data = rospy.Publisher('imu_data', Imu, queue_size=5)
self.pub_imu_rate = rospy.Publisher('imu_msg_rate', Int8, latch=True)
self.pub_rate = 1
def __init__(self):
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
self.bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055')
# Adjust IMU frame since it is mounted 90 degrees to what is expected
self.bno.set_axis_remap(0, 1, 2, 1, 0, 1)
def __init__(self):
self.__bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
calibration = [
223, 255, 249, 255, 219, 255, 66, 253, 142, 255, 49, 251, 254, 255,
253, 255, 0, 0, 232, 3, 151, 2
]
if not self.__bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
def get_direction(t_compass=.2,t_gyro=.2):
data=[26, 0, 254, 255, 33, 0, 159, 253, 217, 0, 244, 255, 255, 255, 0, 0, 1, 0, 232, 3, 88, 1]
bno = BNO055.BNO055(rst='P8_4')
bno.begin()
bno.set_calibration(data)
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Read the Euler angles for heading, roll, pitch (all in degrees).
heading, roll, pitch = bno.read_euler()
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys, gyro, accel, mag = bno.get_calibration_status()
# Print everything out.
print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}\tSys_cal={3} Gyro_cal={4} Accel_cal={5} Mag_cal={6}'.format(heading, roll, pitch, sys, gyro, accel, mag))
global heading
global incline
global yaw_rate
global yaw
end1=0.0
end2=0.0
while True:
#get compass heading
x,y,z = bno.read_magnetometer()
t_diff=time.time()-end1
end1=time.time()
h1 = math.atan2(y, x)
#average compass results
x1=math.cos(h1)
x2=math.cos(math.radians(heading))
y1=math.sin(h1)
y2=math.sin(math.radians(heading))
if t_diff<t_compass:
x3=((t_diff)*(x1)+x2*(t_compass-t_diff))/t_compass
y3=((t_diff)*(y1)+y2*(t_compass-t_diff))/t_compass
else:
x3=x1
y3=y1
h1=math.atan2(y3,x3)
if (h1 < 0):
h1 += 2 * math.pi
heading=math.degrees(h1)
#gyro
x,y,yaw_rate_new=bno.read_gyroscope()#angular velocity (degrees/sec)
#print 'l_accel,', bno.read_linear_acceleration()
pitch, incline1, yaw1 = bno.read_euler()
t_diff=time.time()-end2
end2=time.time()
if t_diff<t_gyro:
incline=((t_diff)*(incline1)+incline*(t_gyro-t_diff))/t_gyro
yaw_rate=((t_diff)*(yaw_rate_new*(-180/math.pi))+yaw_rate*(t_gyro-t_diff))/t_gyro
yaw=((t_diff)*(yaw1*(-180/math.pi))+yaw*(t_gyro-t_diff))/t_gyro
else:
incline=(incline1)
yaw_rate=yaw_rate_new
yaw=yaw1
time.sleep(.009)
def get_direction(t_compass=.2, t_gyro=.1):
bno = BNO055.BNO055(rst='P9_12')
global heading
global incline
global inc_offset
global yaw_rate
end1 = 0.0
end2 = 0.0
yaw_old = 0.0
while True:
#get compass heading
x, y, z = bno.read_magnetometer()
t_diff = time.time() - end1
end1 = time.time()
h1 = math.atan2((1.0722 * (y + 0.6)), (x + 3.5))
#average compass results
x1 = math.cos(h1)
x2 = math.cos(math.radians(heading))
y1 = math.sin(h1)
y2 = math.sin(math.radians(heading))
#x3=.9*x2+.1*x1
#y3=.9*y2+.1*y1
if t_diff < t_compass:
x3 = ((t_diff) * (x1) + x2 * (t_compass - t_diff)) / t_compass
y3 = ((t_diff) * (y1) + y2 * (t_compass - t_diff)) / t_compass
else:
x3 = x1
y3 = y1
h1 = math.atan2(y3, x3)
if (h1 < 0):
h1 += 2 * math.pi
heading = math.degrees(h1)
#gyro
#angles
pitch, incline1, yaw = bno.read_euler()
t_diff = time.time() - end2
end2 = time.time()
#incline=.05*(incline1-inc_offset)+incline*.95
x1 = math.cos(yaw)
x2 = math.cos(yaw_old)
y1 = math.sin(yaw)
y2 = math.sin(yaw_old)
yaw_rate_n = math.atan2((y1 - y2), (x1 - x2)) / t_diff
yaw_old = yaw
#print 'yaw rate ',yaw_rate_n
#print str(t_diff/t_gyro)
#print str((t_gyro-t_diff)/t_gyro)
if t_diff < t_gyro:
incline = ((t_diff) * (incline1 - inc_offset) + incline *
(t_gyro - t_diff)) / t_gyro
yaw_rate = ((t_diff) * (yaw_rate_n) + yaw_rate *
(t_gyro - t_diff)) / t_gyro
else:
incline = (incline1 - inc_offset)
yaw_rate = yaw_rate_n
time.sleep(.01)
def __init__(self, poll_delay=0.0166):
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
self.bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
self.self_test()
self.accel_linear = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.accel_gravity = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.gyro = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.calibration = { 'sys' : 0., 'gyro' : 0., 'accel' : 0, 'mag' : 0. }
self.temp = 0.
self.poll_delay = poll_delay
self.on = True
def __init__(self, poll_delay=0.02):
from Adafruit_BNO055 import BNO055
self.sensor = BNO055.BNO055(rst=12)
self.accel = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.gyro = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.temp = 0.
self.heading = 0.
self.roll = 0.
self.pitch = 0.
self.poll_delay = poll_delay
self.on = True
def __init__(self):
"""."""
# Rasp-Pi configuration with serial UART and RST connected to GPIO 18:
self.bno = BNO055.BNO055(serial_port='/dev/ttyS0', rst=18)
# Initialize the BNO055 and stop if something went wrong.
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor' +
'connected?')
self.check_status()
self.diagnostics()
def __init__(self, calibration_data=None, axis_remap=None):
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
self._sensor = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
if not self._sensor.begin():
raise RuntimeError('Failed to initialize BNO055!')
if axis_remap is not None:
self._sensor.set_axis_remap(**axis_remap)
if calibration_data is not None:
self._sensor.set_calibration(calibration_data)
