def IMU():
try:
from BNO055 import BNO055
return BNO055()
except Exception as e:
print "Failed to Initialize Sparkfun IMU"
print "Error: %s" % e.message
print "Using Mock IMU"
from IMU_Mock import IMU as IMU_Mock
return IMU_Mock()
def __init__(self):
motLA = 7
motLB = 6
motRA = 5
motRB = 4
minSpeed = 11
maxSpeed = 100
self.wheels = DifferentialDrive(motLA, motLB, motRA, motRB, minSpeed,
maxSpeed)
self.imu = BNO055()
self.battery = ADS1000()
self.cam = Camera()
class HeadingSensor:
""" Keeps track of heading using the BNO055 """
__imu = BNO055()
__debug = False
def __init__(self, isDebug=False):
self.__debug = isDebug
if not self.__imu.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
def get_heading(self):
heading, roll, pitch = bno.getVector(0x1A)
return heading
def save_as_start_heading(self):
heading, roll, pitch = bno.getVector(0x1A)
#!/usr/bin/env python
from BNO055 import BNO055
import time
bno = BNO055()
CAL_TIME = 10
timer = time.time()
while True:
sys, gyro, accel, mag = bno.get_calibration_status()
if sys == 3 and gyro == 3 and accel == 3 and mag == 3:
print("Settling...{}".format(int(CAL_TIME - (time.time() - timer))))
# Must stay calibrated for 3 seconds
if time.time() - timer > CAL_TIME:
print("calibrated. Calibration vector:")
print(bno.get_calibration())
break
else:
print("sys: {}/3, gyro: {}/3, accel: {}/3, mag: {}/3").format(
sys, gyro, accel, mag)
timer = time.time()
time.sleep(0.3)
IMU_PUB_RATE = 50
def message_received(msg):
# This runs on a seperate thread from the pub
pass
if __name__ == "__main__":
rospy.init_node('i2c_node')
imu_pub = rospy.Publisher('imu_data', imu_msg, queue_size=1)
rate = rospy.Rate(IMU_PUB_RATE)
temp_pub = rospy.Publisher('temp_data', temp_msg, queue_size=1)
depth_pub = rospy.Publisher('depth_data', depth_msg, queue_size=1)
try:
imu_sens = BNO055()
temp_sens = TSYS01()
depth_sens = MS5837()
temp_sens.init()
except:
#add mock classes to return 0 and continue and alert pilots
pass
while not rospy.is_shutdown():
# using imu rate to poll the temp sensor as well beacuse IMU updates faster
if imu_sens.update():
# imu sensor update
out_message = imu_msg()
out_message.gyro = [
imu_sens.pitch(),
from BNO055 import BNO055
sys.path.append('.')
import RTIMU
import os.path
import time
import math
from std_msgs.msg import Int8MultiArray, Bool
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu
from sensor_msgs.msg import MagneticField
from tf.transformations import quaternion_from_euler
imu = BNO055.BNO055()
rospy.init_node('holly_imu')
rate = rospy.Rate(25)
def calibration_update_callback(data):
global updateCalibration
updateCalibration = data.data
rospy.Subscriber("/imu/save_cal", Bool, calibration_update_callback)
# setup publisher and classes
imuPub = rospy.Publisher('imu/data', Imu, queue_size=5)
msg = Imu()
#!/usr/bin/python
from BNO055 import BNO055
import time
sensor = BNO055(0x29)
while True:
sensor.readEul()
print("X: {:.1f}, Y: {:.1f}, Z: {:.1f}".format(sensor.euler['x'],
sensor.euler['y'],
sensor.euler['z']))
time.sleep(0.010)
pitch_servo = servo(23)
roll_servo = servo(18, offset=-7)
yaw_servo = servo(19)
throttle_servo = servo(5)
# I2C
I2C_bus = I2C(scl=Pin(22), sda=Pin(21))
# bmp180
bmp180 = BMP180(I2C_bus)
bmp180.oversample_sett = 2
bmp180.baseline = 101325
alt_init = bmp180.altitude
# BNO055
bno055 = BNO055(I2C_bus)
pitch_init = bno055.readEuler().angle_x
pitch_init = pitch_init+180 if pitch_init <= 0 else pitch_init-180
roll_init = bno055.readEuler().angle_y
# Write to file
with open("/Web/www/autre.json", "r") as f:
autre_file = ujson.load(f)
with open("/Web/www/autre.json", "w") as f:
autre_file[2]["data"]["target"] = pitch_init
autre_file[1]["data"]["target"] = roll_init
ujson.dump(autre_file, f)
# Airspeed
speed = airspeed(bmp180)
speed_init = 10
import time
import pigpio
from BNO055 import BNO055
import numpy as np
import pickle
# 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:
pi = pigpio.pi()
bno = BNO055(pi=pi, rst=18, address=0x28, bus=1)
# 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))
input('Press enter to begin reading BNO055 data, press Ctrl-C to quit...')
import pigpio
from BNO055 import BNO055
import time
servos = pigpio.pi()
bno = BNO055(address = 0x28)
if bno.begin() is not True:
print "Error initializing device"
exit()
time.sleep(1)
bno.setExternalCrystalUse(True)
for i in range(1,40):
vector = bno.getVector(BNO055.VECTOR_EULER)
time.sleep(0.1)
print(vector)
print("done!")
try:
while True:
vector = bno.getVector(BNO055.VECTOR_EULER)
roll = vector[2]
pitch = vector[1]
servo_pitch_val = int(1600+8.88*(pitch))
servo_roll_val = int(1250 + 9.44 * roll + 0.0123 * roll**2)