def ahrs(): #print('') imu = IMU(verbose=False) header = 47 #print('-'*header) #print("| {:20} | {:20} |".format("Accels [g's]", "Orient(r,p,h) [deg]")) #print('-'*header) fall = False #for _ in range(10): a, m, g = imu.get() # get data from nxp-9dof fxos8700 + fxas21002 r, p, h = imu.getOrientation( a, m) # convert sensor data to angle in roll,pitch,yaw axis #print the angle data #print('| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h)) time.sleep(1) r = abs(r) p = abs(p) #h =abs(h) if r > 50 or p > 50: fall = True else: fall = False return fall
def ahrs(): print('') imu = IMU(verbose=True) header = 47 print('-'*header) print("| {:20} | {:20} |".format("Accels [g's]", "Orient(r,p,h) [deg]")) print('-'*header) for _ in range(10): a, m, g = imu.get() r, p, h = imu.getOrientation(a, m) print('| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h)) time.sleep(0.50) print('-'*header) print(' r: roll') print(' p: pitch') print(' h: heading') print(' g: gravity') print('deg: degree') print('')
class NXP9DoF: ''' Reads from an Adafruit NXP 9-DoF FXOS8700 + FXAS21002 sensor. Note that this has been calibrated based on the orientation of the NXP9DoF board as installed on the KR01 robot, with the Adafruit logo on the chip (mounted horizontally) as follows: Fore _--------------- | * | | | | | Port | | Starboard | | ---------------- Aft If you mount the chip in a different orientation you will likely need to multiply one or more of the axis by -1.0. Depending on where you are on the Earth, true North changes because the Earth is not a perfect sphere. You can get the declination angle for your location from: http://www.magnetic-declination.com/ E.g., for Pukerua Bay, New Zealand: Latitude: 41° 1' 57.7" S Longitude: 174° 53' 11.8" E PUKERUA BAY Magnetic Declination: +22° 37' Declination is POSITIVE (EAST) Inclination: 66° 14' Magnetic field strength: 55716.5 nT E.g., for Wellington, New Zealand: Latitude: 41° 17' 11.9" S Longitude: 174° 46' 32.1" E KELBURN Magnetic Declination: +22° 47' Declination is POSITIVE (EAST) Inclination: 66° 28' Magnetic field strength: 55863.3 nT ''' # permitted error between Euler and Quaternion (in degrees) to allow setting value ERROR_RANGE = 5.0 # was 3.0 # .......................................................................... def __init__(self, config, queue, level): self._log = Logger("nxp9dof", level) if config is None: raise ValueError("no configuration provided.") self._config = config self._queue = queue _config = self._config['ros'].get('nxp9dof') self._quaternion_accept = _config.get( 'quaternion_accept') # permit Quaternion once calibrated self._loop_delay_sec = _config.get('loop_delay_sec') # verbose will print some start-up info on the IMU sensors self._imu = IMU(gs=4, dps=2000, verbose=True) # setting a bias correction for the accel only; the mags/gyros are not getting a bias correction self._imu.setBias((0.0, 0.0, 0.0), None, None) # self._imu.setBias((0.1,-0.02,.25), None, None) _i2c = busio.I2C(board.SCL, board.SDA) self._fxos = adafruit_fxos8700.FXOS8700(_i2c) self._log.info('accelerometer and magnetometer ready.') self._fxas = adafruit_fxas21002c.FXAS21002C(_i2c) self._log.info('gyroscope ready.') self._thread = None self._enabled = False self._closed = False self._heading = None self._pitch = None self._roll = None self._is_calibrated = False self._log.info('ready.') # .......................................................................... def get_imu(self): return self._imu # .......................................................................... def get_fxos(self): return self._fxos # .......................................................................... def get_fxas(self): return self._fxas # .......................................................................... def enable(self): if not self._closed: self._enabled = True # if we haven't started the thread yet, do so now... if self._thread is None: self._thread = threading.Thread(target=NXP9DoF._read, args=[self]) self._thread.start() self._log.info('enabled.') else: self._log.warning('cannot enable: already closed.') # .......................................................................... def get_heading(self): return self._heading # .......................................................................... def get_pitch(self): return self._pitch # .......................................................................... def get_roll(self): return self._roll # .......................................................................... def is_calibrated(self): return self._is_calibrated # .......................................................................... @staticmethod def _in_range(p, q): return p >= (q - NXP9DoF.ERROR_RANGE) and p <= (q + NXP9DoF.ERROR_RANGE) # .......................................................................... def imu_enable(self): ''' Enables the handbuilt IMU on a 20Hz loop. ''' if not self._closed: self._enabled = True # if we haven't started the thread yet, do so now... if self._thread is None: self._thread = threading.Thread(target=NXP9DoF._handbuilt_imu, args=[self]) self._thread.start() self._log.info('enabled.') else: self._log.warning('cannot enable: already closed.') # .......................................................................... def _handbuilt_imu(self): ''' The function that reads sensor values in a loop. This checks to see if the 'sys' calibration is at least 3 (True), and if the Euler and Quaternion values are within an error range of each other, this sets the class variable for heading, pitch and roll. If they aren't within range for more than n polls, the values revert to None. ''' self._log.info('starting sensor read...') _heading = None _pitch = None _roll = None _quat_w = None _quat_x = None _quat_y = None _quat_z = None count = 0 # 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................ # grab data at 20Hz _rate = Rate(1) header = 90 print('-' * header) # print("| {:17} | {:20} |".format("Accels [g's]", " IMU Accels")) print("| {:17} |".format("Mag [xyz]")) while not self._closed: if self._enabled: accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2 gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s a, m, g = self._imu.get() mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla mag_x_g = m[0] * 100.0 mag_y_g = m[1] * 100.0 mag_z_g = m[2] * 100.0 # rate_gyro_x_dps = Convert.rps_to_dps(gyro_x) # rate_gyro_y_dps = Convert.rps_to_dps(gyro_y) # rate_gyro_z_dps = Convert.rps_to_dps(gyro_z) # mag_x_g = mag_x * 100.0 # mag_y_g = mag_y * 100.0 # mag_z_g = mag_z * 100.0 heading = 180.0 * math.atan2(mag_y_g, mag_x_g) / math.pi print(Fore.CYAN + '| x{:>6.3f} y{:>6.3f} z{:>6.3f} |'.format( mag_x_g, mag_y_g, mag_z_g) \ + Style.BRIGHT + '\t{:>5.2f}'.format(heading) + Style.RESET_ALL) # a, m, g = self._imu.get() # print(Fore.CYAN + '| {:>6.3f} {:>6.3f} {:>6.3f} | {:>6.3f} {:>6.3f} {:>6.3f} |'.format( accel_x, accel_y, accel_z, a[0], a[1], a[2]) + Style.RESET_ALL) _rate.wait() # + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL) # time.sleep(0.50) # mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla # gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s # header = 90 # print('-'*header) # print("| {:17} | {:20} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading")) # print('-'*header) # for _ in range(10): # a, m, g = self._imu.get() # r, p, h = self._imu.getOrientation(a, m) # deg = Convert.to_degrees(h) # print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} | {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format( # a[0], a[1], a[2], # m[0], m[1], m[2], # g[0], g[1], g[2]) # + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL) # time.sleep(0.50) # print('-'*header) # print(' uT: micro Tesla') # print(' g: gravity') # print('dps: degrees per second') # print('') # # print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\ # + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\ # + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL) else: self._log.info('disabled loop.') time.sleep(10) count += 1 self._log.debug('read loop ended.') # .......................................................................... def _read(self): ''' The function that reads sensor values in a loop. This checks to see if the 'sys' calibration is at least 3 (True), and if the Euler and Quaternion values are within an error range of each other, this sets the class variable for heading, pitch and roll. If they aren't within range for more than n polls, the values revert to None. ''' self._log.info('starting sensor read...') _heading = None _pitch = None _roll = None _quat_w = None _quat_x = None _quat_y = None _quat_z = None count = 0 # 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................ # grab data at 10Hz rate = Rate(10) while not self._closed: if self._enabled: header = 91 print(Fore.CYAN + ('-' * header) + Style.RESET_ALL) print(Fore.CYAN + "| {:17} | {:20} | {:20} | {:21} |".format( "Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading") + Style.RESET_ALL) print(Fore.CYAN + ('-' * header) + Style.RESET_ALL) for _ in range(10): a, m, g = self._imu.get() r, p, h = self._imu.getOrientation(a, m) deg = Convert.to_degrees(h) # self._log.info(Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h) + Style.RESET_ALL) print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} '.format( a[0], a[1], a[2]) + Fore.YELLOW + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format( m[0], m[1], m[2]) + Fore.MAGENTA + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format( g[0], g[1], g[2]) + Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL) time.sleep(0.50) print('-' * header) print(' uT: micro Tesla') print(' g: gravity') print('dps: degrees per second') print('') # accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2 # mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla # gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s # # print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\ # + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\ # + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL) rate.wait() else: self._log.info('disabled loop.') time.sleep(10) count += 1 self._log.debug('read loop ended.') # .......................................................................... def disable(self): self._enabled = False self._log.info('disabled.') # .......................................................................... def close(self): self._closed = True self._log.info('closed.')
def read9(): #--------9dof init---------- imu = IMU(gs=2, dps=2000, verbose=False) i2c = I2C(0x1F) #--------Set calibration values for magnetometer--------- '''Splits avarages to MSB and LSB''' XMSB = (Xavg / 256) & 0xFF XLSB = Xavg & 0xFF YMSB = (Yavg / 256) & 0xFF YLSB = Yavg & 0xFF ZMSB = (Zavg / 256) & 0xFF ZLSB = Zavg & 0xFF '''Writes splitted avarages to offset registers''' i2c.write8(0x3F, int(XMSB)) i2c.write8(0x40, int(XLSB)) i2c.write8(0x41, int(YMSB)) i2c.write8(0x42, int(YLSB)) i2c.write8(0x43, int(ZMSB)) i2c.write8(0x44, int(ZLSB)) #--------Talker node init------------ #pub = rospy.Publisher('chatter', String, queue_size=10) pub = rospy.Publisher('imu', Imu) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 25hz #---------Readloop--------- while not rospy.is_shutdown(): Havg = 0 #Collects samples and returns avaraged value. for x in range(0, 20): try: a, m, g = imu.get() #Gets data from sensors r, p, h = imu.getOrientation( a, m) #calculates orientation from data ac, ma = imu.rawOrientation() #raw data for magnetometer ''' #Prevents error in Havg when heading fluctuates between 359 and 0. if(x > 1): if(Havg > 260): if(h < 100): h += 360 if(Havg < 100): if(h > 260): h -= 360 Havg = (Havg + h)/2 ''' except IOError: pass ''' #Corrects the heading so that 0 < Havg < 359. if(Havg < 0): Havg += 360 if(Havg >= 360): Havg -= 360 heading = str(Havg) ''' roll = r * (3.141592 / 180.0) #converts degrees to radians pitch = p * (3.141592 / 180.0) yaw = h * (3.141592 / 180.0) ''' a[0] = a[0] * 9.806 #converts g's to m/s^2 a[1] = a[1] * 9.806 a[2] = a[2] * 9.806 g[0] = g[0] * (3.141592/180.0) #converts degrees to radians g[1] = g[1] * (3.141592/180.0) g[2] = g[2] * (3.141592/180.0) ''' print("Heading: " + str(h)) #q = toQuaternion(r, p, h) q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) imuMsg.orientation.x = q[0] #magnetometer imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.angular_velocity.x = g[0] * (3.141592 / 180.0) #gyro imuMsg.angular_velocity.y = g[1] * (3.141592 / 180.0) imuMsg.angular_velocity.z = g[2] * (3.141592 / 180.0) imuMsg.linear_acceleration.x = a[0] * 9.806 #accelerometer imuMsg.linear_acceleration.y = a[1] * 9.806 imuMsg.linear_acceleration.z = a[2] * 9.806 rospy.loginfo(imuMsg) pub.publish(imuMsg) rate.sleep()
#!/usr/bin/env python from __future__ import division, print_function from nxp_imu import IMU import time imu = IMU(gs=4, dps=2000, verbose=True) header = 67 print('-' * header) print("| {:17} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]")) print('-' * header) for _ in range(10000): a, m, g = imu.get() print( '| {:>5.2f} {:>5.2f} {:>5.2f} | {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |' .format(a[0], a[1], a[2], m[0], m[1], m[2], g[0], g[1], g[2])) r, p, y = imu.getOrientation(accel=a, mag=m) # print('| {:>5.2f} {:>5.2f} {:>5.2f} {:>5.2f}|'.format(y, p, r, y)) time.sleep(0.10) print('-' * header) print(' uT: micro Tesla') print(' g: gravity') print('dps: degrees per second') print('')
msg = Imu() while not geckopy.is_shutdown(): a, m, g = imu.get() msg.linear_acceleration.x = a[0] msg.linear_acceleration.y = a[1] msg.linear_acceleration.z = a[2] msg.angular_velocity.x = g[0] msg.angular_velocity.y = g[1] msg.angular_velocity.z = g[2] msg.magnetic_field.x = m[0] msg.magnetic_field.y = m[1] msg.magnetic_field.z = m[2] r, p, h = imu.getOrientation(a, m) q = euler2quat(r, p, h, degrees=True) msg.orientation.w = q.w msg.orientation.x = q.x msg.orientation.y = q.y msg.orientation.z = q.z print(msg) pub.publish(protobufPack(msg)) rate.sleep()
'gyroX_dps', 'gyroY_dps', 'gyroZ_dps', 'gyroX_deg', 'gyroY_deg', 'gyroZ_deg', 'temp_C', 'humidity', 'pres_Pa', 'alt_m' ]) createCsv(headers) imu = IMU(gs=8, dps=2000, verbose=True) incrementalData = empty((0, len(headers))) saveIdx = 50 idx = 0 while True: imuError = False bmeError = False try: a, m, g = imu.get() q = imu.getOrientation(a, m) a, m, g, q = [around(a, 3), around(m, 3), around(g, 3), around(q, 3)] except: a, m, g, q = [["Err"] * 3] * 4 try: bmeError = True bmeSensor = BME280(t_mode=BME280_OSAMPLE_8, p_mode=BME280_OSAMPLE_8, h_mode=BME280_OSAMPLE_8) temp = bmeSensor.read_temperature() humidity = bmeSensor.read_humidity() pressure = bmeSensor.read_pressure() alt = (((101325.0 / pressure)**(1 / 5.257) - 1) * (temp + 273.15)) / 0.0065 temp, humidity, pressure, alt = [ around(temp, 3),