Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/libraries/AP_Compass/AP_Compass_BMM150.cpp
Views: 1798
/*1* Copyright (C) 2016 Intel Corporation. All rights reserved.2*3* This file is free software: you can redistribute it and/or modify it4* under the terms of the GNU General Public License as published by the5* Free Software Foundation, either version 3 of the License, or6* (at your option) any later version.7*8* This file is distributed in the hope that it will be useful, but9* WITHOUT ANY WARRANTY; without even the implied warranty of10* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.11* See the GNU General Public License for more details.12*13* You should have received a copy of the GNU General Public License along14* with this program. If not, see <http://www.gnu.org/licenses/>.15*/16#include "AP_Compass_BMM150.h"1718#if AP_COMPASS_BMM150_ENABLED1920#include <AP_HAL/AP_HAL.h>2122#include <utility>2324#include <AP_HAL/utility/sparse-endian.h>25#include <AP_Math/AP_Math.h>26#include <stdio.h>2728#define CHIP_ID_REG 0x4029#define CHIP_ID_VAL 0x323031#define POWER_AND_OPERATIONS_REG 0x4B32#define POWER_CONTROL_VAL (1 << 0)33#define SOFT_RESET (1 << 7 | 1 << 1)3435#define OP_MODE_SELF_TEST_ODR_REG 0x4C36#define NORMAL_MODE (0 << 1)37#define ODR_30HZ (1 << 3 | 1 << 4 | 1 << 5)38#define ODR_20HZ (1 << 3 | 0 << 4 | 1 << 5)3940#define DATA_X_LSB_REG 0x424142#define REPETITIONS_XY_REG 0x5143#define REPETITIONS_Z_REG 0X524445/* Trim registers */46#define DIG_X1_REG 0x5D47#define DIG_Y1_REG 0x5E48#define DIG_Z4_LSB_REG 0x6249#define DIG_Z4_MSB_REG 0x6350#define DIG_X2_REG 0x6451#define DIG_Y2_REG 0x6552#define DIG_Z2_LSB_REG 0x6853#define DIG_Z2_MSB_REG 0x6954#define DIG_Z1_LSB_REG 0x6A55#define DIG_Z1_MSB_REG 0x6B56#define DIG_XYZ1_LSB_REG 0x6C57#define DIG_XYZ1_MSB_REG 0x6D58#define DIG_Z3_LSB_REG 0x6E59#define DIG_Z3_MSB_REG 0x6F60#define DIG_XY2_REG 0x7061#define DIG_XY1_REG 0x716263#define MEASURE_TIME_USEC 166676465extern const AP_HAL::HAL &hal;6667AP_Compass_Backend *AP_Compass_BMM150::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, bool force_external, enum Rotation rotation)68{69if (!dev) {70return nullptr;71}72AP_Compass_BMM150 *sensor = NEW_NOTHROW AP_Compass_BMM150(std::move(dev), force_external, rotation);73if (!sensor || !sensor->init()) {74delete sensor;75return nullptr;76}7778return sensor;79}8081AP_Compass_BMM150::AP_Compass_BMM150(AP_HAL::OwnPtr<AP_HAL::Device> dev, bool force_external, enum Rotation rotation)82: _dev(std::move(dev)), _rotation(rotation), _force_external(force_external)83{84}8586bool AP_Compass_BMM150::_load_trim_values()87{88struct {89int8_t dig_x1;90int8_t dig_y1;91uint8_t rsv[3];92le16_t dig_z4;93int8_t dig_x2;94int8_t dig_y2;95uint8_t rsv2[2];96le16_t dig_z2;97le16_t dig_z1;98le16_t dig_xyz1;99le16_t dig_z3;100int8_t dig_xy2;101uint8_t dig_xy1;102} PACKED trim_registers, trim_registers2;103104// read the registers twice to confirm we have the right105// values. There is no CRC in the registers and these values are106// vital to correct operation107int8_t tries = 4;108while (tries--) {109if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,110sizeof(trim_registers))) {111continue;112}113if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers2,114sizeof(trim_registers))) {115continue;116}117if (memcmp(&trim_registers, &trim_registers2, sizeof(trim_registers)) == 0) {118break;119}120}121if (-1 == tries) {122DEV_PRINTF("BMM150: Failed to load trim registers\n");123return false;124}125126_dig.x1 = trim_registers.dig_x1;127_dig.x2 = trim_registers.dig_x2;128_dig.xy1 = trim_registers.dig_xy1;129_dig.xy2 = trim_registers.dig_xy2;130_dig.xyz1 = le16toh(trim_registers.dig_xyz1);131_dig.y1 = trim_registers.dig_y1;132_dig.y2 = trim_registers.dig_y2;133_dig.z1 = le16toh(trim_registers.dig_z1);134_dig.z2 = le16toh(trim_registers.dig_z2);135_dig.z3 = le16toh(trim_registers.dig_z3);136_dig.z4 = le16toh(trim_registers.dig_z4);137138return true;139}140141bool AP_Compass_BMM150::init()142{143uint8_t val = 0;144bool ret;145146_dev->get_semaphore()->take_blocking();147148// 10 retries for init149_dev->set_retries(10);150151// use checked registers to cope with bus errors152_dev->setup_checked_registers(4);153154int8_t boot_tries = 4;155while (boot_tries--) {156/* Do a soft reset */157ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);158hal.scheduler->delay(2);159if (!ret) {160continue;161}162163/* Change power state from suspend mode to sleep mode */164ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);165hal.scheduler->delay(2);166if (!ret) {167continue;168}169170ret = _dev->read_registers(CHIP_ID_REG, &val, 1);171if (!ret) {172continue;173}174if (val == CHIP_ID_VAL) {175// found it176break;177}178if (boot_tries == 0) {179DEV_PRINTF("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL);180}181}182if (-1 == boot_tries) {183goto bus_error;184}185186ret = _load_trim_values();187if (!ret) {188goto bus_error;189}190191/*192* Recommended preset for high accuracy:193* - Rep X/Y = 47194* - Rep Z = 83195* - ODR = 20196* But we are going to use 30Hz of ODR197*/198ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2, true);199if (!ret) {200goto bus_error;201}202ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1, true);203if (!ret) {204goto bus_error;205}206/* Change operation mode from sleep to normal and set ODR */207ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ, true);208if (!ret) {209goto bus_error;210}211212_dev->get_semaphore()->give();213214/* register the compass instance in the frontend */215_dev->set_device_type(DEVTYPE_BMM150);216if (!register_compass(_dev->get_bus_id(), _compass_instance)) {217return false;218}219set_dev_id(_compass_instance, _dev->get_bus_id());220221set_rotation(_compass_instance, _rotation);222223if (_force_external) {224set_external(_compass_instance, true);225}226227// 2 retries for run228_dev->set_retries(2);229230_dev->register_periodic_callback(MEASURE_TIME_USEC,231FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));232233_last_read_ms = AP_HAL::millis();234235return true;236237bus_error:238_dev->get_semaphore()->give();239return false;240}241242/*243* Compensation algorithm got from https://github.com/BoschSensortec/BMM050_driver244* this is not explained in datasheet.245*/246int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2) const247{248int32_t inter = ((int32_t)_dig.xyz1) << 14;249inter /= rhall;250inter -= 0x4000;251252int32_t val = _dig.xy2 * ((inter * inter) >> 7);253val += (inter * (((uint32_t)_dig.xy1) << 7));254val >>= 9;255val += 0x100000;256val *= (txy2 + 0xA0);257val >>= 12;258val *= xy;259val >>= 13;260val += (txy1 << 3);261262return val;263}264265int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall) const266{267int32_t dividend = int32_t(z - _dig.z4) << 15;268int32_t dividend2 = dividend - ((_dig.z3 * (int32_t(rhall) - int32_t(_dig.xyz1))) >> 2);269270int32_t divisor = int32_t(_dig.z1) * (rhall << 1);271divisor += 0x8000;272divisor >>= 16;273divisor += _dig.z2;274275int16_t ret = constrain_int32(dividend2 / divisor, -0x8000, 0x8000);276#if 0277static uint8_t counter;278if (counter++ == 0) {279printf("ret=%d z=%d rhall=%u z1=%d z2=%d z3=%d z4=%d xyz1=%d dividend=%d dividend2=%d divisor=%d\n",280ret, z, rhall, _dig.z1, _dig.z2, _dig.z3, _dig.z4, _dig.xyz1, dividend, dividend2, divisor);281}282#endif283return ret;284}285286void AP_Compass_BMM150::_update()287{288le16_t data[4];289bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));290291/* Checking data ready status */292if (!ret || !(data[3] & 0x1)) {293_dev->check_next_register();294uint32_t now = AP_HAL::millis();295if (now - _last_read_ms > 250) {296// cope with power cycle to sensor297_last_read_ms = now;298_dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);299_dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);300}301return;302}303304const uint16_t rhall = le16toh(data[3]) >> 2;305306Vector3f raw_field = Vector3f{307(float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,308rhall, _dig.x1, _dig.x2),309(float) _compensate_xy(((int16_t)le16toh(data[1])) >> 3,310rhall, _dig.y1, _dig.y2),311(float) _compensate_z(((int16_t)le16toh(data[2])) >> 1, rhall)};312313/* apply sensitivity scale 16 LSB/uT */314raw_field /= 16;315/* convert uT to milligauss */316raw_field *= 10;317318_last_read_ms = AP_HAL::millis();319320accumulate_sample(raw_field, _compass_instance);321_dev->check_next_register();322}323324void AP_Compass_BMM150::read()325{326drain_accumulated_samples(_compass_instance);327}328329330#endif // AP_COMPASS_BMM150_ENABLED331332333