Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Compass/AP_Compass_BMM150.cpp

306 lines
7.9 KiB
C++
Raw Blame History

/*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Compass_BMM150.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
#define CHIP_ID_REG 0x40
#define CHIP_ID_VAL 0x32
#define POWER_AND_OPERATIONS_REG 0x4B
#define POWER_CONTROL_VAL (1 << 0)
#define SOFT_RESET (1 << 7 | 1 << 1)
#define OP_MODE_SELF_TEST_ODR_REG 0x4C
#define NORMAL_MODE (0 << 1)
#define ODR_30HZ (1 << 3 | 1 << 4 | 1 << 5)
#define ODR_20HZ (1 << 3 | 0 << 4 | 1 << 5)
#define DATA_X_LSB_REG 0x42
#define REPETITIONS_XY_REG 0x51
#define REPETITIONS_Z_REG 0X52
/* Trim registers */
#define DIG_X1_REG 0x5D
#define DIG_Y1_REG 0x5E
#define DIG_Z4_LSB_REG 0x62
#define DIG_Z4_MSB_REG 0x63
#define DIG_X2_REG 0x64
#define DIG_Y2_REG 0x65
#define DIG_Z2_LSB_REG 0x68
#define DIG_Z2_MSB_REG 0x69
#define DIG_Z1_LSB_REG 0x6A
#define DIG_Z1_MSB_REG 0x6B
#define DIG_XYZ1_LSB_REG 0x6C
#define DIG_XYZ1_MSB_REG 0x6D
#define DIG_Z3_LSB_REG 0x6E
#define DIG_Z3_MSB_REG 0x6F
#define DIG_XY2_REG 0x70
#define DIG_XY1_REG 0x71
#define MEASURE_TIME_USEC 16667
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_BMM150::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
if (!dev) {
return nullptr;
}
AP_Compass_BMM150 *sensor = new AP_Compass_BMM150(compass, std::move(dev));
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_BMM150::AP_Compass_BMM150(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
{
}
bool AP_Compass_BMM150::_load_trim_values()
{
struct {
int8_t dig_x1;
int8_t dig_y1;
uint8_t rsv[3];
le16_t dig_z4;
int8_t dig_x2;
int8_t dig_y2;
uint8_t rsv2[2];
le16_t dig_z2;
le16_t dig_z1;
le16_t dig_xyz1;
le16_t dig_z3;
int8_t dig_xy2;
uint8_t dig_xy1;
} PACKED trim_registers;
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
sizeof(trim_registers))) {
return false;
}
_dig.x1 = trim_registers.dig_x1;
_dig.x2 = trim_registers.dig_x2;
_dig.xy1 = trim_registers.dig_xy1;
_dig.xy2 = trim_registers.dig_xy2;
_dig.xyz1 = le16toh(trim_registers.dig_xyz1);
_dig.y1 = trim_registers.dig_y1;
_dig.y2 = trim_registers.dig_y2;
_dig.z1 = le16toh(trim_registers.dig_z1);
_dig.z2 = le16toh(trim_registers.dig_z2);
_dig.z3 = le16toh(trim_registers.dig_z3);
_dig.z4 = le16toh(trim_registers.dig_z4);
return true;
}
bool AP_Compass_BMM150::init()
{
uint8_t val = 0;
bool ret;
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("BMM150: Unable to get bus semaphore\n");
return false;
}
/* Do a soft reset */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
if (!ret) {
goto bus_error;
}
hal.scheduler->delay(2);
/* Change power state from suspend mode to sleep mode */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL);
if (!ret) {
goto bus_error;
}
hal.scheduler->delay(2);
ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
if (!ret) {
goto bus_error;
}
if (val != CHIP_ID_VAL) {
hal.console->printf("BMM150: Wrong id\n");
goto fail;
}
ret = _load_trim_values();
if (!ret) {
goto bus_error;
}
/*
* Recommended preset for high accuracy:
* - Rep X/Y = 47
* - Rep Z = 83
* - ODR = 20
* But we are going to use 30Hz of ODR
*/
ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2);
if (!ret) {
goto bus_error;
}
ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1);
if (!ret) {
goto bus_error;
}
/* Change operation mode from sleep to normal and set ODR */
ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ);
if (!ret) {
goto bus_error;
}
_dev->get_semaphore()->give();
/* register the compass instance in the frontend */
_compass_instance = register_compass();
_dev->set_device_type(DEVTYPE_BMM150);
set_dev_id(_compass_instance, _dev->get_bus_id());
_dev->register_periodic_callback(MEASURE_TIME_USEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
return true;
bus_error:
hal.console->printf("BMM150: Bus communication error\n");
fail:
_dev->get_semaphore()->give();
return false;
}
/*
* Compensation algorithm got from https://github.com/BoschSensortec/BMM050_driver
* this is not explained in datasheet.
*/
int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2)
{
int32_t inter = ((int32_t)_dig.xyz1) << 14;
inter /= rhall;
inter -= 0x4000;
int32_t val = _dig.xy2 * ((inter * inter) >> 7);
val += (inter * (((uint32_t)_dig.xy1) << 7));
val >>= 9;
val += 0x100000;
val *= (txy2 + 0xA0);
val >>= 12;
val *= xy;
val >>= 13;
val += (txy1 << 3);
return val;
}
int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall)
{
int32_t dividend = ((int32_t)(z - _dig.z4)) << 15;
dividend -= (_dig.z3 * (rhall - _dig.xyz1)) >> 2;
int32_t divisor = ((int32_t)_dig.z1) * (rhall << 1);
divisor += 0x8000;
divisor >>= 16;
divisor += _dig.z2;
return constrain_int32(dividend / divisor, -0x8000, 0x8000);
}
void AP_Compass_BMM150::_update()
{
const uint32_t time_usec = AP_HAL::micros();
le16_t data[4];
bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
/* Checking data ready status */
if (!ret || !(data[3] & 0x1)) {
return;
}
const uint16_t rhall = le16toh(data[3] >> 2);
Vector3f raw_field = Vector3f{
(float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,
rhall, _dig.x1, _dig.x2),
(float) _compensate_xy(((int16_t)le16toh(data[1])) >> 3,
rhall, _dig.y1, _dig.y2),
(float) _compensate_z(((int16_t)le16toh(data[2])) >> 1, rhall)};
/* apply sensitivity scale 16 LSB/uT */
raw_field /= 16;
/* convert uT to milligauss */
raw_field *= 10;
/* rotate raw_field from sensor frame to body frame */
rotate_field(raw_field, _compass_instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(raw_field, time_usec, _compass_instance);
/* correct raw_field for known errors */
correct_field(raw_field, _compass_instance);
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
_mag_accum += raw_field;
_accum_count++;
if (_accum_count == 10) {
_mag_accum /= 2;
_accum_count = 5;
}
_sem->give();
}
void AP_Compass_BMM150::read()
{
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
_sem->give();
return;
}
Vector3f field(_mag_accum);
field /= _accum_count;
_mag_accum.zero();
_accum_count = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
}
Reference in New Issue View Git Blame Copy Permalink