[modules] add bmm150
This commit is contained in:
vamoosebbf
parent
d733b4710e
commit
0a66a1cd66
|
|
@ -0,0 +1,957 @@
|
|||
from machine import I2C
|
||||
import time
|
||||
import ustruct
|
||||
|
||||
I2C_MODE = 1
|
||||
SPI_MODE = 2
|
||||
ENABLE_POWER = 1
|
||||
DISABLE_POWER = 0
|
||||
POKARITY_HIGH = 1
|
||||
POKARITY_LOW = 0
|
||||
ERROR = -1
|
||||
SELF_TEST_XYZ_FALL = 0
|
||||
SELF_TEST_YZ_FAIL = 1
|
||||
SELF_TEST_XZ_FAIL = 2
|
||||
SELF_TEST_Z_FAIL = 3
|
||||
SELF_TEST_XY_FAIL = 4
|
||||
SELF_TEST_Y_FAIL = 5
|
||||
SELF_TEST_X_FAIL = 6
|
||||
SELF_TEST_XYZ_OK = 7
|
||||
ENABLE_DRDY = 1
|
||||
DISABLE_DRDY = 0
|
||||
INTERRUPUT_LATCH_ENABLE = 1
|
||||
INTERRUPUT_LATCH_DISABLE = 0
|
||||
MEASUREMENT_X_ENABLE = 0
|
||||
MEASUREMENT_Y_ENABLE = 0
|
||||
MEASUREMENT_Z_ENABLE = 0
|
||||
MEASUREMENT_X_DISABLE = 1
|
||||
MEASUREMENT_Y_DISABLE = 1
|
||||
MEASUREMENT_Z_DISABLE = 1
|
||||
DATA_OVERRUN_ENABLE = 1
|
||||
DATA_OVERRUN_DISABLE = 0
|
||||
OVERFLOW_INT_ENABLE = 1
|
||||
OVERFLOW_INT_DISABLE = 0
|
||||
LOW_INTERRUPT_X_ENABLE = 0
|
||||
LOW_INTERRUPT_Y_ENABLE = 0
|
||||
LOW_INTERRUPT_Z_ENABLE = 0
|
||||
LOW_INTERRUPT_X_DISABLE = 1
|
||||
LOW_INTERRUPT_Y_DISABLE = 1
|
||||
LOW_INTERRUPT_Z_DISABLE = 1
|
||||
HIGH_INTERRUPT_X_ENABLE = 0
|
||||
HIGH_INTERRUPT_Y_ENABLE = 0
|
||||
HIGH_INTERRUPT_Z_ENABLE = 0
|
||||
HIGH_INTERRUPT_X_DISABLE = 1
|
||||
HIGH_INTERRUPT_Y_DISABLE = 1
|
||||
HIGH_INTERRUPT_Z_DISABLE = 1
|
||||
CHANNEL_X = 1
|
||||
CHANNEL_Y = 2
|
||||
CHANNEL_Z = 3
|
||||
ENABLE_INTERRUPT_PIN = 1
|
||||
DISABLE_INTERRUPT_PIN = 0
|
||||
POWERMODE_NORMAL = 0x00
|
||||
POWERMODE_FORCED = 0x01
|
||||
POWERMODE_SLEEP = 0x03
|
||||
POWERMODE_SUSPEND = 0x04
|
||||
PRESETMODE_LOWPOWER = 0x01
|
||||
PRESETMODE_REGULAR = 0x02
|
||||
PRESETMODE_HIGHACCURACY = 0x03
|
||||
PRESETMODE_ENHANCED = 0x04
|
||||
REPXY_LOWPOWER = 0x01
|
||||
REPXY_REGULAR = 0x04
|
||||
REPXY_ENHANCED = 0x07
|
||||
REPXY_HIGHACCURACY = 0x17
|
||||
REPZ_LOWPOWER = 0x01
|
||||
REPZ_REGULAR = 0x07
|
||||
REPZ_ENHANCED = 0x0D
|
||||
REPZ_HIGHACCURACY = 0x29
|
||||
CHIP_ID_VALUE = 0x32
|
||||
CHIP_ID_REGISTER = 0x40
|
||||
REG_DATA_X_LSB = 0x42
|
||||
REG_DATA_READY_STATUS = 0x48
|
||||
REG_INTERRUPT_STATUS = 0x4a
|
||||
CTRL_POWER_REGISTER = 0x4b
|
||||
MODE_RATE_REGISTER = 0x4c
|
||||
REG_INT_CONFIG = 0x4D
|
||||
REG_AXES_ENABLE = 0x4E
|
||||
REG_LOW_THRESHOLD = 0x4F
|
||||
REG_HIGH_THRESHOLD = 0x50
|
||||
REG_REP_XY = 0x51
|
||||
REG_REP_Z = 0x52
|
||||
RATE_10HZ = 0x00 # (default rate)
|
||||
RATE_02HZ = 0x01
|
||||
RATE_06HZ = 0x02
|
||||
RATE_08HZ = 0x03
|
||||
RATE_15HZ = 0x04
|
||||
RATE_20HZ = 0x05
|
||||
RATE_25HZ = 0x06
|
||||
RATE_30HZ = 0x07
|
||||
DIG_X1 = 0x5D
|
||||
DIG_Y1 = 0x5E
|
||||
DIG_Z4_LSB = 0x62
|
||||
DIG_Z4_MSB = 0x63
|
||||
DIG_X2 = 0x64
|
||||
DIG_Y2 = 0x65
|
||||
DIG_Z2_LSB = 0x68
|
||||
DIG_Z2_MSB = 0x69
|
||||
DIG_Z1_LSB = 0x6A
|
||||
DIG_Z1_MSB = 0x6B
|
||||
DIG_XYZ1_LSB = 0x6C
|
||||
DIG_XYZ1_MSB = 0x6D
|
||||
DIG_Z3_LSB = 0x6E
|
||||
DIG_Z3_MSB = 0x6F
|
||||
DIG_XY2 = 0x70
|
||||
DIG_XY1 = 0x71
|
||||
|
||||
BMM150_ADDR = 19
|
||||
|
||||
|
||||
class trim_register:
|
||||
def __init__(self):
|
||||
self.dig_x1 = 0;
|
||||
self.dig_y1 = 0;
|
||||
self.dig_x2 = 0;
|
||||
self.dig_y2 = 0;
|
||||
self.dig_z1 = 0;
|
||||
self.dig_z2 = 0;
|
||||
self.dig_z3 = 0;
|
||||
self.dig_z4 = 0;
|
||||
self.dig_xy1 = 0;
|
||||
self.dig_xy2 = 0;
|
||||
self.dig_xyz1 = 0;
|
||||
|
||||
|
||||
_trim_data = trim_register()
|
||||
|
||||
|
||||
class geomagnetic_data:
|
||||
def __init__(self):
|
||||
self.x = 0;
|
||||
self.y = 0;
|
||||
self.z = 0;
|
||||
self.r = 0;
|
||||
|
||||
|
||||
_geomagnetic = geomagnetic_data()
|
||||
|
||||
|
||||
class BMM150:
|
||||
__txbuf = [0]
|
||||
def __init__(self, i2c_dev, i2c_addr=BMM150_ADDR):
|
||||
self._offset = (0, 0, 0)
|
||||
self._scale = (1, 1, 1)
|
||||
self.i2cDev = i2c_dev
|
||||
self.bmm150Addr = i2c_addr
|
||||
scan_list = self.i2cDev.scan()
|
||||
print(scan_list)
|
||||
if self.bmm150Addr not in scan_list:
|
||||
raise Exception("Error: Unable connect pmu_bmm150!")
|
||||
|
||||
'''
|
||||
@brief get chip id
|
||||
@return chip id
|
||||
'''
|
||||
|
||||
def get_chip_id(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, CHIP_ID_REGISTER, 1)
|
||||
return rslt[0]
|
||||
|
||||
'''
|
||||
init sensor
|
||||
return 0 is init success
|
||||
-1 is init faild
|
||||
'''
|
||||
|
||||
def sensor_init(self):
|
||||
self.set_power_bit(ENABLE_POWER)
|
||||
chip_id = self.get_chip_id()
|
||||
print("chip_id:", hex(chip_id))
|
||||
if chip_id == CHIP_ID_VALUE:
|
||||
self.get_trim_data()
|
||||
return 0
|
||||
else:
|
||||
return -1
|
||||
|
||||
# soft reset
|
||||
def soft_reset(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, CTRL_POWER_REGISTER, 1)
|
||||
self.__txbuf[0] = rslt[0] | 0x82
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, CTRL_POWER_REGISTER, self.__txbuf[0])
|
||||
|
||||
'''
|
||||
@brief set bmm150 self test
|
||||
@retval
|
||||
SELF_TEST_XYZ_FALL = 0
|
||||
SELF_TEST_YZ_FAIL = 1
|
||||
SELF_TEST_XZ_FAIL = 2
|
||||
SELF_TEST_Z_FAIL = 3
|
||||
SELF_TEST_XY_FAIL = 4
|
||||
SELF_TEST_Y_FAIL = 5
|
||||
SELF_TEST_X_FAIL = 6
|
||||
SELF_TEST_XYZ_OK = 7
|
||||
'''
|
||||
|
||||
def self_test(self):
|
||||
self.set_operation_mode(POWERMODE_SLEEP)
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
rslt1 = []
|
||||
rslt1 = self.change_date(rslt, 1)
|
||||
self.__txbuf[0] == rslt1[0] | 0x01
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, self.__txbuf[0])
|
||||
time.sleep(1)
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
rslt1 = []
|
||||
rslt1 = self.change_date(rslt, 1)
|
||||
if (rslt1[0] & 0x01) == 0:
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, REG_DATA_X_LSB, 5)
|
||||
rslt1 = []
|
||||
rslt1 = self.change_date(rslt, 5)
|
||||
number = (rslt1[0] & 0x01) | (rslt1[2] & 0x01) << 1 | (rslt1[4] & 0x01) << 2
|
||||
return number
|
||||
else:
|
||||
return -1
|
||||
|
||||
'''
|
||||
@brief set power bit
|
||||
@param ctrl is enable/disable power
|
||||
DISABLE_POWER is disable power
|
||||
ENABLE_POWER is enable power
|
||||
'''
|
||||
|
||||
def set_power_bit(self, ctrl):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, CTRL_POWER_REGISTER, 1)
|
||||
rslt1 = self.change_date(rslt, 1)
|
||||
if ctrl == DISABLE_POWER:
|
||||
self.__txbuf[0] = rslt1[0] & 0xFE
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, CTRL_POWER_REGISTER, self.__txbuf[0])
|
||||
else:
|
||||
self.__txbuf[0] = rslt1[0] | 0x01
|
||||
print("power enable", hex(self.__txbuf[0]))
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, CTRL_POWER_REGISTER, self.__txbuf[0])
|
||||
|
||||
'''
|
||||
@brief get power bit
|
||||
@return power bit
|
||||
DISABLE_POWER is disable power
|
||||
ENABLE_POWER is enable power
|
||||
'''
|
||||
|
||||
def get_power_bit(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,CTRL_POWER_REGISTER, 1)
|
||||
return rslt[0] & 0x01
|
||||
|
||||
'''
|
||||
@brief set opration mode
|
||||
@param modes is operation mode
|
||||
POWERMODE_NORMAL
|
||||
POWERMODE_FORCED
|
||||
POWERMODE_SLEEP
|
||||
POWERMODE_SUSPEND
|
||||
'''
|
||||
|
||||
def set_operation_mode(self, modes):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
rslt1 = []
|
||||
rslt1 = self.change_date(rslt, 1)
|
||||
if modes == POWERMODE_NORMAL:
|
||||
self.set_power_bit(ENABLE_POWER)
|
||||
rslt1[0] = rslt1[0] & 0xf9
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif modes == POWERMODE_FORCED:
|
||||
rslt1[0] = (rslt1[0] & 0xf9) | 0x02
|
||||
self.set_power_bit(ENABLE_POWER)
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif modes == POWERMODE_SLEEP:
|
||||
self.set_power_bit(ENABLE_POWER)
|
||||
rslt1[0] = (rslt1[0] & 0xf9) | 0x04
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
else:
|
||||
self.set_power_bit(DISABLE_POWER)
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
|
||||
'''
|
||||
@brief get opration mode
|
||||
@return modes is operation mode
|
||||
POWERMODE_NORMAL = 0x00
|
||||
POWERMODE_FORCED = 0x01
|
||||
POWERMODE_SLEEP = 0x03
|
||||
POWERMODE_SUSPEND = 0x04
|
||||
'''
|
||||
|
||||
def get_operation_mode(self):
|
||||
if self.get_power_bit() == 0:
|
||||
return POWERMODE_SUSPEND
|
||||
else:
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
rslt1 = self.change_date(rslt, 1)
|
||||
return hex((rslt1[0] & 0x03))
|
||||
|
||||
'''
|
||||
@brief set rate
|
||||
@param rate
|
||||
RATE_10HZ #(default rate)
|
||||
RATE_02HZ
|
||||
RATE_06HZ
|
||||
RATE_08HZ
|
||||
RATE_15HZ
|
||||
RATE_20HZ
|
||||
RATE_25HZ
|
||||
RATE_30HZ
|
||||
'''
|
||||
|
||||
def set_rate(self, rates):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
rslt1 = []
|
||||
rslt1 = self.change_date(rslt, 1)
|
||||
if rates == RATE_10HZ:
|
||||
rslt1[0] = rslt1[0] & 0xc7
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_02HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x08
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_06HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x10
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_08HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x18
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_15HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x20
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_20HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x28
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_25HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x30
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
elif rates == RATE_30HZ:
|
||||
rslt1[0] = (rslt1[0] & 0xc7) | 0x38
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
else:
|
||||
rslt1[0] = rslt1[0] & 0xc7
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, MODE_RATE_REGISTER, rslt1[0])
|
||||
|
||||
'''
|
||||
@brief get rates
|
||||
@return rates
|
||||
RATE_10HZ #(default rate)
|
||||
RATE_02HZ
|
||||
RATE_06HZ
|
||||
RATE_08HZ
|
||||
RATE_15HZ
|
||||
RATE_20HZ
|
||||
RATE_25HZ
|
||||
RATE_30HZ
|
||||
'''
|
||||
|
||||
def get_rate(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, MODE_RATE_REGISTER, 1)
|
||||
return (rslt[0] & 0x38) >> 3
|
||||
|
||||
|
||||
'''
|
||||
@brief set preset mode
|
||||
@param modes
|
||||
PRESETMODE_LOWPOWER
|
||||
PRESETMODE_REGULAR
|
||||
PRESETMODE_HIGHACCURACY
|
||||
PRESETMODE_ENHANCED
|
||||
'''
|
||||
|
||||
def set_preset_mode(self, modes):
|
||||
if modes == PRESETMODE_LOWPOWER:
|
||||
self.set_rate(RATE_10HZ)
|
||||
self.set_xy_rep(REPXY_LOWPOWER)
|
||||
self.set_z_rep(REPZ_LOWPOWER)
|
||||
elif modes == PRESETMODE_REGULAR:
|
||||
self.set_rate(RATE_10HZ)
|
||||
self.set_xy_rep(REPXY_REGULAR)
|
||||
self.set_z_rep(REPZ_REGULAR)
|
||||
elif modes == PRESETMODE_HIGHACCURACY:
|
||||
self.set_rate(RATE_20HZ)
|
||||
self.set_xy_rep(REPXY_HIGHACCURACY)
|
||||
self.set_z_rep(REPZ_HIGHACCURACY)
|
||||
elif modes == PRESETMODE_ENHANCED:
|
||||
self.set_rate(RATE_10HZ)
|
||||
self.set_xy_rep(REPXY_ENHANCED)
|
||||
self.set_z_rep(REPZ_ENHANCED)
|
||||
else:
|
||||
self.set_rate(RATE_10HZ)
|
||||
self.set_xy_rep(REPXY_LOWPOWER)
|
||||
self.set_z_rep(REPZ_LOWPOWER)
|
||||
|
||||
|
||||
|
||||
'''
|
||||
@brief set xy rep
|
||||
@param modes
|
||||
REPXY_LOWPOWER
|
||||
REPXY_REGULAR
|
||||
REPXY_ENHANCED
|
||||
REPXY_HIGHACCURACY
|
||||
'''
|
||||
|
||||
|
||||
def set_xy_rep(self, modes):
|
||||
self.__txbuf[0] = modes
|
||||
if modes == REPXY_LOWPOWER:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_XY, self.__txbuf[0])
|
||||
elif modes == REPXY_REGULAR:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_XY, self.__txbuf[0])
|
||||
elif modes == REPXY_ENHANCED:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_XY, self.__txbuf[0])
|
||||
elif modes == REPXY_HIGHACCURACY:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_XY, self.__txbuf[0])
|
||||
else:
|
||||
__txbuf[0] = REPXY_LOWPOWER####################################
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_XY, self.__txbuf[0])
|
||||
|
||||
|
||||
'''
|
||||
@brief set z rep
|
||||
@param modes
|
||||
REPZ_LOWPOWER
|
||||
REPZ_REGULAR
|
||||
REPZ_ENHANCED
|
||||
REPZ_HIGHACCURACY
|
||||
'''
|
||||
|
||||
|
||||
def set_z_rep(self, modes):
|
||||
self.__txbuf[0] = modes
|
||||
if modes == REPZ_LOWPOWER:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_Z, self.__txbuf[0])
|
||||
elif modes == REPZ_REGULAR:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_Z, self.__txbuf[0])
|
||||
elif modes == REPZ_ENHANCED:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_Z, self.__txbuf[0])
|
||||
elif modes == REPZ_HIGHACCURACY:
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_Z, self.__txbuf[0])
|
||||
else:
|
||||
__txbuf[0] = REPZ_LOWPOWER ####################################
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_REP_Z, self.__txbuf[0])
|
||||
|
||||
|
||||
'''
|
||||
@brief get trim data
|
||||
'''
|
||||
|
||||
|
||||
def get_trim_data(self):
|
||||
trim_x1_y1 = self.i2cDev.readfrom_mem( self.bmm150Addr,DIG_X1, 2)
|
||||
trim_xyz_data = self.i2cDev.readfrom_mem( self.bmm150Addr,DIG_Z4_LSB, 4)
|
||||
trim_xy1_xy2 = self.i2cDev.readfrom_mem( self.bmm150Addr,DIG_Z2_LSB, 10)
|
||||
_trim_data.dig_x1 = self.uint8_to_int8(trim_x1_y1[0])
|
||||
_trim_data.dig_y1 = self.uint8_to_int8(trim_x1_y1[1])
|
||||
_trim_data.dig_x2 = self.uint8_to_int8(trim_xyz_data[2])
|
||||
_trim_data.dig_y2 = self.uint8_to_int8(trim_xyz_data[3])
|
||||
temp_msb = int(trim_xy1_xy2[3]) << 8
|
||||
_trim_data.dig_z1 = int(temp_msb | trim_xy1_xy2[2])
|
||||
temp_msb = int(trim_xy1_xy2[1] << 8)
|
||||
_trim_data.dig_z2 = int(temp_msb | trim_xy1_xy2[0])
|
||||
temp_msb = int(trim_xy1_xy2[7] << 8)
|
||||
_trim_data.dig_z3 = temp_msb | trim_xy1_xy2[6]
|
||||
temp_msb = int(trim_xyz_data[1] << 8)
|
||||
_trim_data.dig_z4 = int(temp_msb | trim_xyz_data[0])
|
||||
_trim_data.dig_xy1 = trim_xy1_xy2[9]
|
||||
_trim_data.dig_xy2 = self.uint8_to_int8(trim_xy1_xy2[8])
|
||||
temp_msb = int((trim_xy1_xy2[5] & 0x7F) << 8)
|
||||
_trim_data.dig_xyz1 = int(temp_msb | trim_xy1_xy2[4])
|
||||
|
||||
|
||||
'''
|
||||
@brief get geomagnetic
|
||||
'''
|
||||
|
||||
|
||||
def get_geomagnetic(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_DATA_X_LSB, 8)
|
||||
# rslt = self.change_date(rslt, 8)
|
||||
rslt1 = ustruct.unpack("bbbbbbbb", rslt)
|
||||
_geomagnetic.x = ((rslt1[0] & 0xF8) >> 3) | (rslt1[1] << 5)
|
||||
_geomagnetic.y = ((rslt1[2] & 0xF8) >> 3) | (rslt1[3] << 5)
|
||||
_geomagnetic.z = ((rslt1[4] & 0xFE) >> 1) | (rslt1[5] << 7)
|
||||
_geomagnetic.r = ((rslt1[6] & 0xFC) >> 2) | (rslt1[7] << 6)
|
||||
rslt2 = [rslt1[0],rslt1[1],rslt1[2]]
|
||||
rslt2[0] = self.compenstate_x(_geomagnetic.x, _geomagnetic.r)
|
||||
rslt2[1] = self.compenstate_y(_geomagnetic.y, _geomagnetic.r)
|
||||
rslt2[2] = self.compenstate_z(_geomagnetic.z, _geomagnetic.r)
|
||||
return rslt2
|
||||
|
||||
|
||||
'''
|
||||
@brief uint8_t to int8_t
|
||||
'''
|
||||
|
||||
|
||||
def uint8_to_int8(self, number):
|
||||
if number <= 127:
|
||||
return number
|
||||
else:
|
||||
return (256 - number) * -1
|
||||
|
||||
|
||||
'''
|
||||
@berif compenstate_x
|
||||
'''
|
||||
|
||||
|
||||
def compenstate_x(self, data_x, data_r):
|
||||
if data_x != -4096:
|
||||
if data_r != 0:
|
||||
process_comp_x0 = data_r
|
||||
elif _trim_data.dig_xyz1 != 0:
|
||||
process_comp_x0 = _trim_data.dig_xyz1
|
||||
else:
|
||||
process_comp_x0 = 0
|
||||
if process_comp_x0 != 0:
|
||||
process_comp_x1 = int(_trim_data.dig_xyz1 * 16384)
|
||||
process_comp_x2 = int(process_comp_x1 / process_comp_x0 - 0x4000)
|
||||
retval = process_comp_x2
|
||||
process_comp_x3 = retval * retval
|
||||
process_comp_x4 = _trim_data.dig_xy2 * (process_comp_x3 / 128)
|
||||
process_comp_x5 = _trim_data.dig_xy1 * 128
|
||||
process_comp_x6 = retval * process_comp_x5
|
||||
process_comp_x7 = (process_comp_x4 + process_comp_x6) / 512 + 0x100000
|
||||
process_comp_x8 = _trim_data.dig_x2 + 0xA0
|
||||
process_comp_x9 = (process_comp_x8 * process_comp_x7) / 4096
|
||||
process_comp_x10 = data_x * process_comp_x9
|
||||
retval = process_comp_x10 / 8192
|
||||
retval = (retval + _trim_data.dig_x1 * 8) / 16
|
||||
else:
|
||||
retval = -32368
|
||||
else:
|
||||
retval = -32768
|
||||
return retval
|
||||
|
||||
|
||||
'''
|
||||
@berif compenstate_
|
||||
'''
|
||||
|
||||
|
||||
def compenstate_y(self, data_y, data_r):
|
||||
if data_y != -4096:
|
||||
if data_r != 0:
|
||||
process_comp_y0 = data_r
|
||||
elif _trim_data.dig_xyz1 != 0:
|
||||
process_comp_y0 = _trim_data.dig_xyz1
|
||||
else:
|
||||
process_comp_y0 = 0
|
||||
if process_comp_y0 != 0:
|
||||
process_comp_y1 = int(_trim_data.dig_xyz1 * 16384 / process_comp_y0)
|
||||
process_comp_y2 = int(process_comp_y1 - 0x4000)
|
||||
retval = process_comp_y2
|
||||
process_comp_y3 = retval * retval
|
||||
process_comp_y4 = _trim_data.dig_xy2 * (process_comp_y3 / 128)
|
||||
process_comp_y5 = _trim_data.dig_xy1 * 128
|
||||
process_comp_y6 = (process_comp_y4 + process_comp_y5 * retval) / 512
|
||||
process_comp_y7 = _trim_data.dig_y2 + 0xA0
|
||||
process_comp_y8 = ((process_comp_y6 + 0x100000) * process_comp_y7) / 4096
|
||||
process_comp_y9 = data_y * process_comp_y8
|
||||
retval = process_comp_y9 / 8192
|
||||
retval = (retval + _trim_data.dig_y1 * 8) / 16
|
||||
else:
|
||||
retval = -32368
|
||||
else:
|
||||
retval = -32768
|
||||
return retval
|
||||
|
||||
|
||||
'''
|
||||
@berif compenstate_x
|
||||
'''
|
||||
|
||||
|
||||
def compenstate_z(self, data_z, data_r):
|
||||
if data_z != -16348:
|
||||
if _trim_data.dig_z2 != 0 and _trim_data.dig_z1 != 0 and _trim_data.dig_xyz1 != 0 and data_r != 0:
|
||||
process_comp_z0 = data_r - _trim_data.dig_xyz1
|
||||
process_comp_z1 = (_trim_data.dig_z3 * process_comp_z0) / 4
|
||||
process_comp_z2 = (data_z - _trim_data.dig_z4) * 32768
|
||||
process_comp_z3 = _trim_data.dig_z1 * data_r * 2
|
||||
process_comp_z4 = (process_comp_z3 + 32768) / 65536
|
||||
retval = (process_comp_z2 - process_comp_z1) / (_trim_data.dig_z2 + process_comp_z4)
|
||||
if retval > 32767:
|
||||
retval = 32367
|
||||
elif retval < -32367:
|
||||
retval = -32367
|
||||
retval = retval / 16
|
||||
else:
|
||||
retval = -32768
|
||||
else:
|
||||
retval = -32768
|
||||
return retval
|
||||
|
||||
|
||||
'''
|
||||
@brief Enable or disable the data readly mode pin, configure the polarity of the data ready mode pin
|
||||
@param modes Enable or disable the pin :
|
||||
enable : ENABLE_DRDY
|
||||
disable : DISABLE_DRDY (default mode)
|
||||
@param polarity Active level
|
||||
high : POKARITY_HIGH (default active high level )
|
||||
low : POKARITY_LOW
|
||||
'''
|
||||
|
||||
|
||||
def set_data_readly_interrupt_pin(self, modes, polarity):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_INT_CONFIG, 1)
|
||||
if modes == DISABLE_DRDY:
|
||||
self.__txbuf[0] = rslt[0] & 0x7F
|
||||
else:
|
||||
self.__txbuf[0] = rslt[0] | 0x80
|
||||
if polarity == POKARITY_LOW:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xFB
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x04
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_INT_CONFIG, self.__txbuf[0])
|
||||
|
||||
|
||||
'''
|
||||
@brief Get data ready status
|
||||
@return status data readly status
|
||||
1 is data is ready
|
||||
0 is data is not ready
|
||||
'''
|
||||
|
||||
|
||||
def get_data_readly_state(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, REG_DATA_READY_STATUS, 1)
|
||||
return (rslt[0] & 0x01)
|
||||
|
||||
|
||||
'''
|
||||
@brief set measurement xyz
|
||||
@param channel_x channel x selection:
|
||||
MEASUREMENT_X_ENABLE (Default x-axis channel enabled)
|
||||
MEASUREMENT_X_DISABLE
|
||||
@param channel_y channel y selection:
|
||||
MEASUREMENT_Y_ENABLE (Default y-axis channel enabled)
|
||||
MEASUREMENT_Y_DISABLE
|
||||
@param channel_z channel z selection:
|
||||
MEASUREMENT_Z_ENABLE (Default z-axis channel enabled)
|
||||
MEASUREMENT_Z_DISABLE
|
||||
'''
|
||||
|
||||
|
||||
def set_measurement_xyz(self, channel_x, channel_y, channel_z):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, REG_AXES_ENABLE, 1)
|
||||
|
||||
if channel_x == MEASUREMENT_X_DISABLE:
|
||||
self.__txbuf[0] = rslt[0] | 0x08
|
||||
else:
|
||||
self.__txbuf[0] = rslt[0] & 0xF7
|
||||
|
||||
if channel_y == MEASUREMENT_Y_DISABLE:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x10
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xEF
|
||||
|
||||
if channel_z == MEASUREMENT_Z_DISABLE:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x20
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xDF
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr, REG_AXES_ENABLE, self.__txbuf[0])
|
||||
|
||||
|
||||
'''
|
||||
@brief get measurement xyz
|
||||
@param channel channel ? selection:
|
||||
CHANNEL_X
|
||||
CHANNEL_Y
|
||||
CHANNEL_Z
|
||||
@return
|
||||
1 is enable measurement
|
||||
0 is disable measurement
|
||||
'''
|
||||
|
||||
|
||||
def get_measurement_state_xyz(self, channel):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_AXES_ENABLE, 1)
|
||||
if channel == CHANNEL_X:
|
||||
if (rslt[0] & 0x08) == 0:
|
||||
return 1
|
||||
elif channel == CHANNEL_Y:
|
||||
if (rslt[0] & 0x10) == 0:
|
||||
return 1
|
||||
elif channel == CHANNEL_Z:
|
||||
if (rslt[0] & 0x20) == 0:
|
||||
return 1
|
||||
else:
|
||||
return 0
|
||||
return 0
|
||||
|
||||
|
||||
'''
|
||||
@brief Enable or disable the interrupt pin, configure the polarity of the interrupt pin
|
||||
@param modes Enable or disable the pin :
|
||||
enable : ENABLE_INTERRUPT_PIN
|
||||
disable : DISABLE_INTERRUPT_PIN (default mode)
|
||||
@param polarity Active level
|
||||
high : POKARITY_HIGH (default active high level )
|
||||
low : POKARITY_LOW
|
||||
'''
|
||||
|
||||
|
||||
def set_interrupt_pin(self, modes, polarity):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_AXES_ENABLE, 1)
|
||||
if modes == DISABLE_INTERRUPT_PIN:
|
||||
self.__txbuf[0] = rslt[0] & 0xBF
|
||||
else:
|
||||
self.__txbuf[0] = rslt[0] | 0x40
|
||||
if polarity == POKARITY_LOW:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xFE
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x01
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_AXES_ENABLE, self.__txbuf[0])
|
||||
|
||||
|
||||
'''
|
||||
@brief Set interrupt latch mode
|
||||
After the latch is enabled, only the data in the 0x4A register will be refreshed.
|
||||
No latch, data is refreshed in real time.
|
||||
@param modes Latched or not latched
|
||||
latch : INTERRUPUT_LATCH_ENABLE (dafault interrupt latch)
|
||||
no latch : INTERRUPUT_LATCH_DISABLE
|
||||
'''
|
||||
|
||||
|
||||
def set_interruput_latch(self, modes):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_AXES_ENABLE, 1)
|
||||
if modes == INTERRUPUT_LATCH_DISABLE:
|
||||
self.__txbuf[0] = rslt[0] & 0xFD
|
||||
else:
|
||||
self.__txbuf[0] = rslt[0] | 0x02
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_AXES_ENABLE, self.__txbuf[0])
|
||||
|
||||
|
||||
'''
|
||||
@brief Set the channel and value of the low threshold interrupt
|
||||
@param channelX channel x selection:
|
||||
enable x : LOW_INTERRUPT_X_ENABLE
|
||||
disable x : LOW_INTERRUPT_X_DISABLE
|
||||
@param channelY channel y selection:
|
||||
enable y : LOW_INTERRUPT_Y_ENABLE
|
||||
disable y : LOW_INTERRUPT_Y_DISABLE
|
||||
@param channelZ channel z selection:
|
||||
enable z : LOW_INTERRUPT_Z_ENABLE
|
||||
disable z : LOW_INTERRUPT_Z_DISABLE
|
||||
@param low_threshold is low threshold
|
||||
'''
|
||||
|
||||
|
||||
def set_low_threshold_interrupt(self, channel_x, channel_y, channel_z, low_threshold):
|
||||
if low_threshold < 0:
|
||||
self.__txbuf[0] = (low_threshold * -1) | 0x80
|
||||
else:
|
||||
self.__txbuf[0] = low_threshold
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_LOW_THRESHOLD, self.__txbuf[0])
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_INT_CONFIG, 1)
|
||||
if channel_x == LOW_INTERRUPT_X_DISABLE:
|
||||
self.__txbuf[0] = rslt[0] | 0x01
|
||||
#print(self.__txbuf[0])
|
||||
else:
|
||||
self.__txbuf[0] = rslt[0] & 0xFE
|
||||
# print(self.__txbuf[0])
|
||||
if channel_y == LOW_INTERRUPT_Y_DISABLE:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x02
|
||||
#print(self.__txbuf[0])
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xFC
|
||||
# print(self.__txbuf[0])
|
||||
if channel_x == LOW_INTERRUPT_X_DISABLE:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x04
|
||||
#print(self.__txbuf[0])
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xFB
|
||||
# print(self.__txbuf[0])
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_INT_CONFIG, self.__txbuf[0])
|
||||
# print(self.__txbuf[0])
|
||||
self.set_interrupt_pin(ENABLE_INTERRUPT_PIN, POKARITY_HIGH)
|
||||
self.get_geomagnetic()
|
||||
|
||||
|
||||
|
||||
'''
|
||||
@brief Get the channel low threshold Interrupt status
|
||||
@return status interrupt status
|
||||
1-7 is interrupt
|
||||
0 is no interrupt
|
||||
'''
|
||||
|
||||
|
||||
def get_low_threshold_interrupt_state(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_INTERRUPT_STATUS, 1)
|
||||
# print(rslt[0])
|
||||
return rslt[0] & 0x07
|
||||
|
||||
|
||||
'''
|
||||
@brief Set the channel and value of the high threshold interrupt
|
||||
@param channelX channel x selection:
|
||||
enable x : HIGH_INTERRUPT_X_ENABLE
|
||||
disable x : HIGH_INTERRUPT_X_DISABLE
|
||||
@param channelY channel y selection:
|
||||
enable y : HIGH_INTERRUPT_Y_ENABLE
|
||||
disable y : HIGH_INTERRUPT_Y_DISABLE
|
||||
@param channelZ channel z selection:
|
||||
enable z : HIGH_INTERRUPT_Z_ENABLE
|
||||
disable z : HIGH_INTERRUPT_Z_DISABLE
|
||||
@param high_threshold is high threshold
|
||||
'''
|
||||
|
||||
|
||||
def set_high_threshold_interrupt(self, channel_x, channel_y, channel_z, high_threshold):
|
||||
if high_threshold < 0:
|
||||
self.__txbuf[0] = (high_threshold * -1) | 0x80
|
||||
else:
|
||||
self.__txbuf[0] = high_threshold
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_HIGH_THRESHOLD, self.__txbuf[0])
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr, REG_INT_CONFIG, 1)
|
||||
if channel_x == HIGH_INTERRUPT_X_DISABLE:
|
||||
self.__txbuf[0] = rslt[0] | 0x08
|
||||
else:
|
||||
self.__txbuf[0] = rslt[0] & 0xF7
|
||||
if channel_y == HIGH_INTERRUPT_Y_DISABLE:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x10
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xEF
|
||||
if channel_x == HIGH_INTERRUPT_X_DISABLE:
|
||||
self.__txbuf[0] = self.__txbuf[0] | 0x20
|
||||
else:
|
||||
self.__txbuf[0] = self.__txbuf[0] & 0xDf
|
||||
|
||||
self.i2cDev.writeto_mem(self.bmm150Addr,REG_INT_CONFIG, self.__txbuf)
|
||||
self.set_interrupt_pin(ENABLE_INTERRUPT_PIN, POKARITY_HIGH)
|
||||
self.get_geomagnetic()
|
||||
|
||||
|
||||
'''
|
||||
@brief Get the channel low threshold Interrupt status
|
||||
@return status interrupt status
|
||||
1-7 is interrupt
|
||||
0 is no interrupt
|
||||
'''
|
||||
|
||||
|
||||
def get_high_threshold_interrupt_state(self):
|
||||
rslt = self.i2cDev.readfrom_mem(self.bmm150Addr,REG_INTERRUPT_STATUS, 1)
|
||||
return (rslt[0] & 0x38) >> 3
|
||||
|
||||
def change_date(self, rslt, num):
|
||||
rslt_change = []
|
||||
for i in range(num):
|
||||
rslt_change.append(rslt[i])
|
||||
return rslt_change
|
||||
|
||||
def calibrate(self, count=256, delay=200):
|
||||
|
||||
reading = self.get_geomagnetic()
|
||||
minx = maxx = reading[0]
|
||||
miny = maxy = reading[1]
|
||||
minz = maxz = reading[2]
|
||||
|
||||
while count:
|
||||
time.sleep_ms(delay)
|
||||
reading = self.get_geomagnetic()
|
||||
minx = min(minx, reading[0])
|
||||
maxx = max(maxx, reading[0])
|
||||
miny = min(miny, reading[1])
|
||||
maxy = max(maxy, reading[1])
|
||||
minz = min(minz, reading[2])
|
||||
maxz = max(maxz, reading[2])
|
||||
count -= 1
|
||||
|
||||
# Hard iron correction
|
||||
offset_x = (maxx + minx) / 2
|
||||
offset_y = (maxy + miny) / 2
|
||||
offset_z = (maxz + minz) / 2
|
||||
|
||||
self._offset = (offset_x, offset_y, offset_z)
|
||||
|
||||
# Soft iron correction
|
||||
avg_delta_x = (maxx - minx) / 2
|
||||
avg_delta_y = (maxy - miny) / 2
|
||||
avg_delta_z = (maxz - minz) / 2
|
||||
|
||||
avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3
|
||||
|
||||
scale_x = avg_delta / avg_delta_x
|
||||
scale_y = avg_delta / avg_delta_y
|
||||
if avg_delta_z == 0:
|
||||
avg_delta_z=1
|
||||
scale_z = avg_delta / avg_delta_z
|
||||
|
||||
self._scale = (scale_x, scale_y, scale_z)
|
||||
|
||||
return self._offset, self._scale
|
||||
|
||||
def setup():
|
||||
global bmm150
|
||||
while ERROR == bmm150.sensor_init():
|
||||
print("sensor init error ,please check connect")
|
||||
'''
|
||||
POWERMODE_NORMAL
|
||||
POWERMODE_FORCED
|
||||
POWERMODE_SLEEP
|
||||
POWERMODE_SUSPEND
|
||||
'''
|
||||
bmm150.set_operation_mode(POWERMODE_NORMAL)
|
||||
|
||||
'''
|
||||
PRESETMODE_LOWPOWER
|
||||
PRESETMODE_REGULAR
|
||||
PRESETMODE_HIGHACCURACY
|
||||
PRESETMODE_ENHANCED
|
||||
'''
|
||||
bmm150.set_preset_mode(PRESETMODE_LOWPOWER)
|
||||
|
||||
'''
|
||||
Enable or disable the pin :
|
||||
ENABLE_DRDY
|
||||
DISABLE_DRDY (default mode)
|
||||
|
||||
polarity Active level
|
||||
POKARITY_HIGH (default active high level )
|
||||
POKARITY_LOW
|
||||
'''
|
||||
bmm150.set_data_readly_interrupt_pin(ENABLE_DRDY ,POKARITY_HIGH)
|
||||
|
||||
# bmm150.calibrate(200, 200)
|
||||
|
||||
def loop():
|
||||
global bmm150, img
|
||||
if bmm150.get_data_readly_state() == 1:
|
||||
rslt = bmm150.get_geomagnetic()
|
||||
print("mag x = %d ut"%rslt[0])
|
||||
print("mag y = %d ut"%rslt[1])
|
||||
print("mag z = %d ut"%rslt[2])
|
||||
print("")
|
||||
color = (0, 255, 0)
|
||||
if rslt[0] > 100 or rslt[1] > 100 or rslt[2] > 100:
|
||||
color = (255, 0, 0)
|
||||
img.clear()
|
||||
img.draw_rectangle(0, 0 , img.width(), img.height(), color, fill=True)
|
||||
img.draw_string(10, 10, "mag x = %d ut"%rslt[0], scale=2)
|
||||
img.draw_string(10, 40, "mag y = %d ut"%rslt[1], scale=2)
|
||||
img.draw_string(10, 70, "mag z = %d ut"%rslt[2], scale=2)
|
||||
lcd.display(img)
|
||||
else:
|
||||
time.sleep(0.2)
|
||||
time.sleep(0.2)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import lcd, image
|
||||
lcd.init()
|
||||
img = image.Image(size=(320, 240))
|
||||
tmp = I2C(I2C.I2C0, freq = 100*1000, scl = 28, sda = 22)
|
||||
print(tmp.scan())
|
||||
bmm150 = BMM150(tmp, 0x13)
|
||||
|
||||
setup()
|
||||
while True:
|
||||
loop()
|
||||
|
||||
|
||||
Loading…
Reference in New Issue