From f64a6d290170f6d1f7a03d97e7abb64f9ba28372 Mon Sep 17 00:00:00 2001 From: David Vo Date: Fri, 9 Mar 2018 11:38:07 +1100 Subject: [PATCH 1/2] bno055: Clean up and fix Apparently our BNO055.getHeadingRate() has been broken this entire time. We also had roll and pitch the wrong way around according to the datasheet, but at least we weren't reading those anyway. --- utilities/bno055.py | 178 +++++++++++++++++++++++----------------- utilities/bno055_sim.py | 6 +- 2 files changed, 104 insertions(+), 80 deletions(-) diff --git a/utilities/bno055.py b/utilities/bno055.py index 318b8a8..2a19eb2 100644 --- a/utilities/bno055.py +++ b/utilities/bno055.py @@ -58,12 +58,12 @@ class Register(enum.IntEnum): GYRO_DATA_Z_LSB = 0x18 GYRO_DATA_Z_MSB = 0x19 - EULER_H_LSB = 0x1A # euler heading, least significant bit - EULER_H_MSB = 0x1B # euler heading, most significant bit - EULER_P_LSB = 0x1C # euler pitch, least significant bit - EULER_P_MSB = 0x1D # euler pitch, most significant bit - EULER_R_LSB = 0x1E # euler roll, least significant bit - EULER_R_MSB = 0x1F # euler roll, most significant bit + EULER_HEADING_LSB = 0x1A # yaw + EULER_HEADING_MSB = 0x1B + EULER_ROLL_LSB = 0x1C + EULER_ROLL_MSB = 0x1D + EULER_PITCH_LSB = 0x1E + EULER_PITCH_MSB = 0x1F QUATERNION_DATA_W_LSB = 0x20 QUATERNION_DATA_W_MSB = 0x21 @@ -94,16 +94,13 @@ class Register(enum.IntEnum): SELFTEST_RESULT = 0x36 INTR_STAT = 0x37 - SYS_CLK_STAT = 0x38 - SYS_STAT = 0x39 + SYS_CLK_STATUS = 0x38 + SYS_STATUS = 0x39 SYS_ERR = 0x3A UNIT_SEL = 0x3B # address used to select the unit outputs - - DATA_SELECT = 0x3C - + #DATA_SELECT = 0x3C OPR_MODE = 0x3D # the address used to select what sensors to use for the gyro outputs - PWR_MODE = 0x3E SYS_TRIGGER = 0x3F @@ -157,6 +154,37 @@ class Register(enum.IntEnum): MAG_RADIUS_LSB = 0x69 MAG_RADIUS_MSB = 0x6A + class Page1Register(enum.IntEnum): + """Page 1 register addresses. Set Register.PAGE_ID to access these.""" + + # Configuration registers + ACCEL_CONFIG = 0x08 + MAG_CONFIG = 0x09 + GYRO_CONFIG = 0x0A + GYRO_MODE_CONFIG = 0x0B + ACCEL_SLEEP_CONFIG = 0x0C + GYRO_SLEEP_CONFIG = 0x0D + MAG_SLEEP_CONFIG = 0x0E + + # Interrupt registers + INT_MASK = 0x0F + INT = 0x10 + ACCEL_ANY_MOTION_THRES = 0x11 + ACCEL_INTR_SETTINGS = 0x12 + ACCEL_HIGH_G_DURN = 0x13 + ACCEL_HIGH_G_THRES = 0x14 + ACCEL_NO_MOTION_THRES = 0x15 + ACCEL_NO_MOTION_SET = 0x16 + GYRO_INTR_SETING = 0x17 + GYRO_HIGHRATE_X_SET = 0x18 + GYRO_DURN_X_SET = 0x19 + GYRO_HIGHRATE_Y_SET = 0x1A + GYRO_DURN_Y_SET = 0x1B + GYRO_HIGHRATE_Z_SET = 0x1C + GYRO_DURN_Z_SET = 0x1D + GYRO_ANY_MOTION_THRES = 0x1E + GYRO_ANY_MOTION_SET = 0x1F + class OperationMode(enum.IntEnum): """Operation modes. Changes what sensors it uses and how it fuses them.""" @@ -168,9 +196,9 @@ class OperationMode(enum.IntEnum): ACCGYRO = 0x05 MAGGYRO = 0x06 AMG = 0x07 - IMUPLUS = 0x08 + IMU = 0x08 # Inertial Measurement Unit COMPASS = 0x09 - M4G = 0x0A + M4G = 0x0A # Magnet for Gyroscope NDOF_FMC_OFF = 0x0B NDOF = 0x0C @@ -179,46 +207,47 @@ class PowerMode(enum.IntEnum): LOWPOWER = 0x01 SUSPEND = 0x02 - class AxisMapConfigMode(enum.IntEnum): - """Axis map configuration modes.""" - - P0 = 0x21 - P1 = 0x22 - P2 = 0x23 - P3 = 0x24 - P4 = 0x25 - P5 = 0x26 - P6 = 0x27 - P7 = 0x28 - - class AxisMapSign(enum.IntEnum): - P0 = 0x04 - P1 = 0x00 - P2 = 0x06 - P3 = 0x02 - P4 = 0x03 - P5 = 0x01 - P6 = 0x07 - P7 = 0x05 - - # the units that we want and the index in the unit select register that corresponds to it - UNIT_SEL_ACC_UNIT = 0 # m/s - UNIT_SEL_ACC_UNIT_INDEX = 0 - UNIT_SEL_GYR_UNIT = 1 # rad/s - UNIT_SEL_GYR_UNIT_INDEX = 1 - UNIT_SEL_EUL_UNIT = 1 # rad - UNIT_SEL_EUL_UNIT_INDEX = 2 - UNIT_SEL_TEMP_UNIT = 0 # celcius - UNIT_SEL_TEMP_UNIT_INDEX = 4 - UNIT_SEL_ORI_UNIT = 1 # android orientation mode, pitch turning clockwise decreases values - UNIT_SEL_ORI_UNIT_INDEX = 7 - UNIT_SEL_LIST = [ - (UNIT_SEL_ACC_UNIT, UNIT_SEL_ACC_UNIT_INDEX), - (UNIT_SEL_GYR_UNIT, UNIT_SEL_GYR_UNIT_INDEX), - (UNIT_SEL_EUL_UNIT, UNIT_SEL_EUL_UNIT_INDEX), - (UNIT_SEL_TEMP_UNIT, UNIT_SEL_TEMP_UNIT_INDEX), - (UNIT_SEL_ORI_UNIT, UNIT_SEL_ORI_UNIT_INDEX), - ] + class AxisMapConfig(enum.IntFlag): + """Axis remapping configuration flags.""" + + X_AXIS_IS_X = 0 << 0 + X_AXIS_IS_Y = 1 << 0 + X_AXIS_IS_Z = 2 << 0 + + Y_AXIS_IS_X = 0 << 2 + Y_AXIS_IS_Y = 1 << 2 + Y_AXIS_IS_Z = 2 << 2 + + Z_AXIS_IS_X = 0 << 4 + Z_AXIS_IS_Y = 1 << 4 + Z_AXIS_IS_Z = 2 << 4 + + class AxisMapSign(enum.IntFlag): + """Axis sign remapping configuration flags.""" + + ALL_POSITIVE = 0 + Z_NEGATIVE = 1 << 0 + Y_NEGATIVE = 1 << 1 + X_NEGATIVE = 1 << 2 + + class UnitSel(enum.IntFlag): + """Flags for Register.UNIT_SEL to select units/data format for data outputs.""" + + # Acceleration + ACC_M_S = 0 << 0 # m/s^2 + ACC_MG = 1 << 0 # mg + # Gyro angular rate + GYR_DPS = 0 << 1 # deg/s + GYR_RPS = 1 << 1 # rad/s + # Euler angles + EUL_DEGREES = 0 << 2 + EUL_RADIANS = 1 << 2 + # Temperature + TEMP_CELSIUS = 0 << 4 + TEMP_FAHRENHEIT = 1 << 4 + # Orientation output format + ORI_WINDOWS = 0 << 7 # pitch turning clockwise increases values + ORI_ANDROID = 1 << 7 # pitch turning clockwise decreases values def __init__(self, port: wpilib.I2C.Port = None, address: int = None) -> None: super().__init__() @@ -234,17 +263,15 @@ def __init__(self, port: wpilib.I2C.Port = None, address: int = None) -> None: self.i2c = wpilib.I2C(port, address, sim_port) - # set the units that we want self.offset = 0.0 - current_units = self.i2c.read(self.Register.UNIT_SEL, 1)[0] - for wanted, index in self.UNIT_SEL_LIST: - if wanted == 1: - current_units = current_units | (1 << index) - elif wanted == 0: - current_units = current_units & ~(1 << index) - self.i2c.write(self.Register.UNIT_SEL, current_units) - self.setOperationMode(self.OperationMode.IMUPLUS) # accelerometer and gyro + self.select_units( + self.UnitSel.ACC_M_S + | self.UnitSel.GYR_RPS | self.UnitSel.EUL_RADIANS + | self.UnitSel.TEMP_CELSIUS + | self.UnitSel.ORI_ANDROID + ) self.reverse_axis(False, False, False) + self.setOperationMode(self.OperationMode.IMU) # accelerometer and gyro self.cache_heading() def cache_heading(self): @@ -254,25 +281,22 @@ def cache_heading(self): def reverse_axis(self, x: bool, y: bool, z: bool) -> None: """Reverse the specified axis directions.""" - current_directions = self.i2c.read(self.Register.AXIS_MAP_SIGN, 1)[0] + current_directions = self.AxisMapSign.ALL_POSITIVE if x: - current_directions |= (1 << 2) - else: - current_directions &= ~(1 << 2) + current_directions |= self.AxisMapSign.X_NEGATIVE if y: - current_directions |= (1 << 1) - else: - current_directions &= ~(1 << 1) + current_directions |= self.AxisMapSign.Y_NEGATIVE if z: - current_directions |= (1 << 0) - else: - current_directions &= ~(1 << 0) + current_directions |= self.AxisMapSign.Z_NEGATIVE self.i2c.write(self.Register.AXIS_MAP_SIGN, current_directions) def setOperationMode(self, mode: OperationMode) -> None: mode = self.OperationMode(mode) self.i2c.write(self.Register.OPR_MODE, mode.value) + def select_units(self, units: UnitSel) -> None: + self.i2c.write(self.Register.UNIT_SEL, units) + def getAngle(self) -> float: """Function called by the GyroBase's PID Source to get the current measurement.""" return self.getHeading() @@ -292,13 +316,13 @@ def getHeadingTime(self) -> float: return self.timestep_cached_heading_tm def getRawHeading(self) -> float: - return -self.getEuler(self.Register.EULER_H_LSB) + return -self.getEuler(self.Register.EULER_HEADING_LSB) def getPitch(self) -> float: - return self.getEuler(self.Register.EULER_P_LSB) + return self.getEuler(self.Register.EULER_PITCH_LSB) def getRoll(self) -> float: - return self.getEuler(self.Register.EULER_R_LSB) + return self.getEuler(self.Register.EULER_ROLL_LSB) def getEuler(self, start_register: int) -> float: euler_bytes = self.i2c.read(start_register, 2) @@ -306,7 +330,7 @@ def getEuler(self, start_register: int) -> float: return euler_int / 900.0 def getHeadingRate(self) -> float: - return -self.getEuler(self.Register.GYRO_DATA_Z_LSB) + return self.getEuler(self.Register.GYRO_DATA_Z_LSB) def resetHeading(self, heading: float = 0) -> None: self.offset = self.getRawHeading() - heading diff --git a/utilities/bno055_sim.py b/utilities/bno055_sim.py index 431ca59..73c3f9c 100644 --- a/utilities/bno055_sim.py +++ b/utilities/bno055_sim.py @@ -24,11 +24,11 @@ def transactionI2C(self, port, device_address, data_to_send, send_size, data_rec if 'bno055' in hal_data['robot']: self.heading = math.radians(-hal_data['robot']['bno055']) - if data_to_send[0] == BNO055.Register.EULER_H_LSB: + if data_to_send[0] == BNO055.Register.EULER_HEADING_LSB: struct.pack_into(' Date: Sat, 15 Sep 2018 20:44:37 +1000 Subject: [PATCH 2/2] bno055_sim: Use more meaningful variable names --- utilities/bno055_sim.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/utilities/bno055_sim.py b/utilities/bno055_sim.py index 73c3f9c..24952f7 100644 --- a/utilities/bno055_sim.py +++ b/utilities/bno055_sim.py @@ -24,11 +24,12 @@ def transactionI2C(self, port, device_address, data_to_send, send_size, data_rec if 'bno055' in hal_data['robot']: self.heading = math.radians(-hal_data['robot']['bno055']) - if data_to_send[0] == BNO055.Register.EULER_HEADING_LSB: + register = data_to_send[0] + if register == BNO055.Register.EULER_HEADING_LSB: struct.pack_into('