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)