def __init__(self):
self.pose_publisher = rospy.Publisher('/Outdoor_Blimp/imu/pose',
PoseStamped,
queue_size=10)
self.rpy_publisher = rospy.Publisher('/Outdoor_Blimp/imu/orientation',
Vector3,
queue_size=10)
self.twist_publisher = rospy.Publisher('/Outdoor_Blimp/imu/twist',
TwistStamped,
queue_size=10)
# self.cam_pose_publisher = rospy.Publisher('/Outdoor_Blimp/camera/imu/pose',PoseStamped,queue_size = 10)
# self.cam_rpy_publisher = rospy.Publisher('/Outdoor_Blimp/camera/imu/orientation',Vector3,queue_size = 10)
# self.cam_twist_publisher = rospy.Publisher('/Outdoor_Blimp/camera/imu/twist', TwistStamped, queue_size=10)
#self.gps_info_publisher = rospy.Publisher('/Outdoor_Blimp/gps/general_info', gps_info, queue_size=10)
#self.gps_vel_publisher = rospy.Publisher('/Outdoor_Blimp/gps/velocity_info', gps_velocity, queue_size=10)
port = "/dev/serial0"
self.gps_serial = serial.Serial(port, baudrate=9600, timeout=0.5)
self.bno = BNO055.BNO055()
if self.bno.begin() is not True:
print "Error initializing device"
exit()
time.sleep(1)
self.bno.setExternalCrystalUse(True)
def __init__(self, tilt_angle):
''' Construct an IMU task function by initilizing any shared
variables and initialize the IMU object
@param tilt_angle The shared variable between tasks that contains the IMU title readings
'''
self.tilt_angle = tilt_angle
self.imu = BNO055.bno055 (pyb.I2C (1, pyb.I2C.MASTER, baudrate = 100000), micropython.const(0x28))
def IMUstart(): bno = BNO055() if bno.begin() is not True: print "Error initializing IMU" exit() time.sleep(1) bno.setExternalCrystalUse(True) return bno
def imu_driver(device_port):
bno = BNO055.BNO055(serial_port=device_port)
initialized = False
while not initialized and not rospy.is_shutdown():
try:
# 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))
initialized = True
except:
print 'initialization failed, retrying... '
pass
while not rospy.is_shutdown():
try:
# Gyroscope data (in degrees per second):
wx, wy, wz = bno.read_gyroscope()
# Accelerometer data (in meters per second squared):
ax, ay, az = bno.read_accelerometer()
# print ('x=',x,' y=',y,' z=',z)
imu_pub(wx, wy, wz, ax, ay, az)
except:
print 'reading failed'
pass
def imu_startup_procedure(self):
self.bno = BNO055.BNO055(i2c=constants.BNO055_I2C_INDEX)
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
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:
pass
# print('System error: {0}'.format(error))
# print('See datasheet section 4.3.59 for the meaning.')
self.bno.set_calibration(self.IMU_CALDATA)
# print(self.bno.get_calibration())
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = self.bno.get_revision()
class AUV:
def __init__(self):
"""
Instantiate the AUV object
"""
self.mc = MotorController()
# Connection to onboard radio.
try:
# Jack Silberman's radio
#self.radio = Radio('/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DN038PQU-if00-port0')
# Yonder's radio
self.radio = Radio(
'/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0')
except Exception, e:
print("Radio not found. Exception is: ", e)
print("Exiting")
exit(1)
self.is_manual = True
self.pressure_sensor = ms5837.MS5837_30BA()
self.imu_sensor = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
self.controller = PID(self.mc, TARGET_HEADING, CONTROL_TOLERANCE,
TARGET_TOLERANCE, IS_DEBUG_MODE, P, I, D)
print("Radio is not connected.")
def imu_startup_procedure(self):
self.bno = BNO055.BNO055(i2c=1)
if not self.bno.begin():
raise RuntimeError(
'Failed to initialize BNO055! Is the sensor connected?')
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.')
self.bno.set_calibration(self.IMU_CALDATA)
print(self.bno.get_calibration())
# 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))
def publisher():
dataPub = rospy.Publisher('rov/bno055', Imu, queue_size=3)
infoPub = rospy.Publisher('rov/bno055_info', bno055_info, queue_size=3)
rospy.init_node('bno055')
rate = rospy.Rate(30) #30Hz data read
# Setup BNO055
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with I2C and RST connected to GPIO 27:
sensor = BNO055.BNO055(rst=27)
attempts = 0
# Initialize the BNO055 and stop if something went wrong.
while(attempts < 10):
try:
sensor.begin()
break
except Exception as e:
attempts += 1
rospy.sleep(0.25)
if(attempts == 10):
rospy.logerr('Failed to initialize BNO055! Program end')
exit(1)
# Print system status and self test result.
try:
status, self_test, error = sensor.get_system_status()
except Exception as e:
rospy.logerr('Failed to read BNO055 system status! %s', e)
rospy.logdebug('System status: %s', status)
rospy.logdebug('Self test result (0x0F is normal): %s', hex(self_test))
# Print out an error if system status is in error mode.
if(status == 0x01):
rospy.logerr('System error: %s', error)
rospy.logerr('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
try:
sw, bl, accel, mag, gyro = sensor.get_revision()
except Exception as e:
rospy.logerr('Failed to read BNO055 meta-inforamtion! %s', e)
rospy.loginfo('Software version: %d', sw)
rospy.loginfo('Bootloader version: %d', bl)
rospy.loginfo('Accelerometer ID: %s', hex(accel))
rospy.loginfo('Magnetometer ID: %s', hex(mag))
rospy.loginfo('Gyroscope ID: %s', hex(gyro))
rospy.loginfo('Reading BNO055 data...')
while not rospy.is_shutdown():
# Define messages
msgHeader = Header()
infoHeader = Header()
msg = Imu()
info = bno055_info()
orientation = Quaternion()
angular_vel = Vector3()
linear_accel = Vector3()
#no covarience vlaues known
# Update data
attempts = 0
while(attempts < 4):
try:
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys, gyro, accel, mag = sensor.get_calibration_status()
temp_c = sensor.read_temp()
break
except Exception as e:
rospy.logdebug('Failed to read BNO055 calibration stat and temp! %s', e)
attempts += 1
rospy.sleep(0.01)
if(attempts != 4):
info.sysCalibration = sys
info.accelCalibration = accel
info.gyroCalibration = gyro
info.magnoCalibration = mag
info.tempC = temp_c
attempts = 0
while(attempts < 4):
try:
# Orientation as a quaternion:
orientation.x, orientation.y, orientation.z, orientation.w = sensor.read_quaternion()
# Gyroscope data (in degrees per second converted to radians per second):
gry_x, gry_y, gry_z = sensor.read_gyroscope()
angular_vel.x = math.radians(gry_x)
angular_vel.y = math.radians(gry_y)
angular_vel.z = math.radians(gry_z)
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
linear_accel.x, linear_accel.y, linear_accel.z = sensor.read_linear_acceleration()
break
except Exception as e:
rospy.logdebug('Failed to read BNO055 data! %s', e)
attempts += 1
rospy.sleep(0.01)
if(attempts != 4):
msg.orientation = orientation
msg.angular_velocity = angular_vel
msg.linear_acceleration = linear_accel
#update message headers
msgHeader.stamp = rospy.Time.now()
msgHeader.frame_id = 'imu_data'
msg.header = msgHeader
infoHeader.stamp = rospy.Time.now()
infoHeader.frame_id = 'imu_info'
info.header = infoHeader
dataPub.publish(msg)
infoPub.publish(info)
rate.sleep()
import logging
import sys
import time
import BNO055
bno = BNO055.BNO055()
if not bno.begin(mode=BNO055.OPERATION_MODE_NDOF):
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))
# 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()
# bno.set_external_crystal(True)
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 get_euler_from_bno():
for i in range (10):
from marvelmind_cy_mm import MarvelmindHedge
from opticalflow import Optical_Flow
import BNO055
from time import sleep
# Initiate IMU Thread
optic = Optical_Flow()
hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=4, debug=False)
bno = BNO055.BNO055(serial_port='/dev/ttyS0', rst=18)
#SETUP PHASE FOR SENSORS
def Setup():
hedge.start()
optic.start()
bno.connect_IMU()
sleep(2)
#FETCH RESULTS FROM SENSORS
def fetch_beacon():
state = hedge.position()
return state
def fetch_optic():
state = optic.position()
return state
def fetch_IMU():
def publisher():
rospy.init_node('imu')
dataPub = rospy.Publisher('/imu/data', Imu, queue_size=3)
infoPub = rospy.Publisher('/imu/info', bno055_info, queue_size=3)
headingPub = rospy.Publisher('/imu/heading', Float64, queue_size=3)
load_calibration = rospy.get_param("~load_calibration", False)
rate = rospy.Rate(30) #30Hz data read
# Setup BNO055
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with I2C and RST connected to GPIO 27:
global sensor
sensor = BNO055.BNO055()
try:
sensor.begin()
except Exception as e:
rospy.logerr('Failed to initialize BNO055! %s', e)
sys.exit(1)
#sensor.set_axis_remap(BNO055.AXIS_REMAP_Y, BNO055.AXIS_REMAP_X, BNO055.AXIS_REMAP_Z) #swap x,y axis
# Print system status and self test result.
try:
status, self_test, error = sensor.get_system_status()
except Exception as e:
rospy.logerr('Failed to read BNO055 system status! %s', e)
sys.exit(2)
rospy.logdebug('System status: %s', status)
rospy.logdebug('Self test result (0x0F is normal): %s', hex(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
rospy.logerr('System error: %s', error)
rospy.logerr('See datasheet section 4.3.59 for the meaning.')
sys.exit(3)
# Print BNO055 software revision and other diagnostic data.
try:
sw, bl, accel, mag, gyro = sensor.get_revision()
except Exception as e:
rospy.logerr('Failed to read BNO055 meta-inforamtion! %s', e)
sys.exit(4)
rospy.loginfo('Software version: %d', sw)
rospy.loginfo('Bootloader version: %d', bl)
rospy.loginfo('Accelerometer ID: %s', hex(accel))
rospy.loginfo('Magnetometer ID: %s', hex(mag))
rospy.loginfo('Gyroscope ID: %s', hex(gyro))
rospy.loginfo('Reading BNO055 data...')
if load_calibration:
try:
sensor.set_calibration(
np.load(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'calibration.npy')).tolist())
except Exception as e:
rospy.logerr("Error loading calibration data: " + str(e))
save_calibration_srv = rospy.Service('/imu/save_calibration', Trigger,
save_calibration)
while not rospy.is_shutdown():
# Define messages
msg = Imu()
info = bno055_info()
heading = Float64()
orientation = Quaternion()
angular_vel = Vector3()
linear_accel = Vector3()
# Update meta data
attempts = 0
sys_cal = 0
temp_c = 0
while attempts < 4:
try:
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys_cal, gyro, accel, mag = sensor.get_calibration_status()
temp_c = sensor.read_temp()
break
except Exception as e:
rospy.logerr(
'Failed to read BNO055 calibration stat and temp! %s', e)
attempts += 1
rospy.sleep(0.01)
if attempts != 4:
info.sysCalibration = sys_cal
info.accelCalibration = accel
info.gyroCalibration = gyro
info.magnoCalibration = mag
info.tempC = temp_c
# Update real data
attempts = 0
while attempts < 4:
try:
# Orientation as a quaternion:
orientation.x, orientation.y, orientation.z, orientation.w = sensor.read_quaternion(
)
# Gyroscope data (in degrees per second converted to radians per second):
gry_x, gry_y, gry_z = sensor.read_gyroscope()
angular_vel.x = math.radians(gry_x)
angular_vel.y = math.radians(gry_y)
angular_vel.z = math.radians(gry_z)
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
linear_accel.x, linear_accel.y, linear_accel.z = sensor.read_linear_acceleration(
)
heading.data = sensor.read_euler()[0]
break
except Exception as e:
rospy.logerr('Failed to read BNO055 data! %s', e)
attempts += 1
rospy.sleep(0.01)
if attempts != 4:
msg.orientation = orientation
msg.orientation_covariance[0] = 0.01 # covariance in x,y,z (3x3)
msg.orientation_covariance[4] = 0.01
msg.orientation_covariance[8] = 0.01
msg.angular_velocity = angular_vel
msg.angular_velocity_covariance[
0] = 0.01 # covariance in x,y,z (3x3)
msg.angular_velocity_covariance[4] = 0.01
msg.angular_velocity_covariance[8] = 0.01
msg.linear_acceleration = linear_accel
msg.linear_acceleration_covariance[
0] = 0.01 # covariance in x,y,z (3x3)
msg.linear_acceleration_covariance[4] = 0.01
msg.linear_acceleration_covariance[8] = 0.01
#update message headers
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu_link'
dataPub.publish(msg)
info.header.stamp = rospy.Time.now()
info.header.frame_id = 'imu_link'
infoPub.publish(info)
headingPub.publish(heading)
rate.sleep()
from math import sin, tan
from Walk import Walk
from Leg import Leg
from numpy import *
import BNO055
ftau = 0.75
fpitch = 0
froll = 0
told = time.time() - .1
tnow = time.time()
dt = tnow - told
bno = BNO055.BNO055(rst=None)
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))
def main():
# Setup BNO055
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with I2C and RST connected to GPIO 27:
sensor = BNO055.BNO055(rst=27)
attempts = 0
# Initialize the BNO055 and stop if something went wrong.
while(attempts < 10):
try:
sensor.begin()
break
except Exception as e:
attempts += 1
time.sleep(0.25)
if(attempts == 10):
print('Failed to initialize BNO055! Program end')
exit(1)
# Print system status and self test result.
try:
status, self_test, error = sensor.get_system_status()
except Exception as e:
print('Failed to read BNO055 system status! ', e)
print('\nSystem status: ', status)
print('Self test result (0x0F is normal): ', hex(self_test))
# Print out an error if system status is in error mode.
if(status == 0x01):
print('System error: ', error)
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
try:
sw, bl, accel, mag, gyro = sensor.get_revision()
except Exception as e:
rospy.logerr('Failed to read BNO055 meta-inforamtion!', e)
print('Software version: ', sw)
print('Bootloader version: ', bl)
print('Accelerometer ID: ', hex(accel))
print('Magnetometer ID: ', hex(mag))
print('Gyroscope ID: ', hex(gyro))
print('Reading BNO055 data...\n\n')
while True:
# Update data
attempts = 0
while(attempts < 4):
try:
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys, gyro, accel, mag = sensor.get_calibration_status()
temp_c = sensor.read_temp()
break
except Exception as e:
print('Failed to read BNO055 calibration stat and temp! ', e)
attempts += 1
time.sleep(0.01)
attempts = 0
while(attempts < 4):
try:
# Orientation as a quaternion:
orientation = sensor.read_quaternion()
# Gyroscope data (in degrees per second converted to radians per second):
angular_vel = sensor.read_gyroscope()
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
linear_accel = sensor.read_linear_acceleration()
break
except Exception as e:
print('Failed to read BNO055 data!', e)
attempts += 1
time.sleep(0.01)
if(attempts != 4):
if sys > 1: #if system calibrartion is 2 or 3
updateTimeSeries(angular_vel, linear_accel, orientation)
time.sleep(0.01) #~100Hz
#matrixData[0,0:3] = gyro_r
matrixData[0, 0:3] = euler_rounded
matrixData[1, 0:3] = grav_r
if ((euler_rounded[0] > 370) or (euler_rounded[1] > 370)
or (euler_rounded[2] > 370)):
return produceVector(bno)
if (euler_rounded[0] > 180):
matrixData[0][0] = abs(euler_rounded[0] - 360)
return matrixData
#This block turns on and starts the IMU
bno = BNO055()
if bno.begin() is not True:
print "Error initializing IMU"
exit()
time.sleep(1)
bno.setExternalCrystalUse(True)
#pullMotorVal()
totalReturn = array([])
tCounter = 0
rPlace = 1000
#type = input("Type 1 if inputting percents, 2 if inputting motor angles: ")
gait = obtainGait()
import CCS811 import BME280 import BNO055 CCS811.save_data() print BME280.get_data() print BNO055.get_data()
"""
#Yellow Encoder Cable: B7
#Gray Encoder Cable: B6
import pyb
import motor
import controller
import utime
import BNO055
import micropython
# Create all objects
DC_Motor_1 = motor.MotorDriver(5, pyb.Pin.board.PC1, pyb.Pin.board.PA0,
pyb.Pin.board.PA1)
IMU = BNO055.bno055(pyb.I2C(1, pyb.I2C.MASTER, baudrate=100000),
micropython.const(0x28))
Controller_1 = controller.Controller(1.35, 0.01) ####
# Setup/Initialization
def setup():
''' This initializes the encoder and motor to zero values. '''
IMU.zero_Euler_vals() #
DC_Motor_1.set_duty_cycle(0)
Controller_1.set_setpoint(10)
# Continuous loop
def loop():
''' This is the main loop that handles the DC Motor step response
functionality. '''
import PCA9685
import BNO055
import LTC2945
import time
# Setup PCA9685
print("Initializing PWM Driver")
pwm = PCA9685.PCA9685(1)
pwm.set_frequency(50)
# Setup BNO055
print("Initializing IMU")
sensor = BNO055.BNO055(1)
# Setup LTC2945
print("Initializing Voltage Monitor")
power = LTC2945.LTC2945(1)
# Setup complete
print("Initialization complete")
counter = 0
while (True):
vin = power.read_vin()
current = power.read_current()
print("Battery Voltage: " + str(vin))
print("Current Consumption: " + str(vin))
if counter == 5:
# Read in run config
with open("config.yml", 'r') as file:
config = load(file, Loader=Loader)
# Load necessary interfaces
if config['mode'] == 'full':
# Setup PCA9685
print("Initializing PWM Driver")
pwm = PCA9685.PCA9685(1)
pwm.set_frequency(50)
# Setup BNO055
print("Initializing IMU")
euler = BNO055.BNO055(1)
# Setup LTC2945
print("Initializing Voltage Monitor")
power = LTC2945.LTC2945(1)
# Setup complete
print("Initialization complete")
else:
# Create fake devices for testing
pwm = PCA9685.Dummy()
euler = BNO055.Dummy()
power = LTC2945.Dummy()
# Set the port for the web server
