Path: blob/master/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp
9402 views
/*1* This file is free software: you can redistribute it and/or modify it2* under the terms of the GNU General Public License as published by the3* Free Software Foundation, either version 3 of the License, or4* (at your option) any later version.5*6* This file is distributed in the hope that it will be useful, but7* WITHOUT ANY WARRANTY; without even the implied warranty of8* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.9* See the GNU General Public License for more details.10*11* You should have received a copy of the GNU General Public License along12* with this program. If not, see <http://www.gnu.org/licenses/>.13*14* https://www.st.com/resource/en/datasheet/iis2mdc.pdf15*16*/17#include "AP_Compass_config.h"1819#if AP_COMPASS_IIS2MDC_ENABLED2021#include "AP_Compass_IIS2MDC.h"2223// IIS2MDC Registers24#define IIS2MDC_ADDR_CFG_REG_A 0x6025#define IIS2MDC_ADDR_CFG_REG_B 0x6126#define IIS2MDC_ADDR_CFG_REG_C 0x6227#define IIS2MDC_ADDR_STATUS_REG 0x6728#define IIS2MDC_ADDR_OUTX_L_REG 0x6829#define IIS2MDC_ADDR_WHO_AM_I 0x4F3031// IIS2MDC Definitions32#define IIS2MDC_WHO_AM_I 0b0100000033#define IIS2MDC_STATUS_REG_READY 0b0000111134// CFG_REG_A35#define COMP_TEMP_EN (1 << 7)36#define MD_CONTINUOUS (0 << 0)37#define ODR_100 ((1 << 3) | (1 << 2))38// CFG_REG_B39#define OFF_CANC (1 << 1)40// CFG_REG_C41#define BDU (1 << 4)4243extern const AP_HAL::HAL &hal;4445AP_Compass_Backend *AP_Compass_IIS2MDC::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,46bool force_external,47enum Rotation rotation)48{49if (!dev) {50return nullptr;51}5253AP_Compass_IIS2MDC *sensor = NEW_NOTHROW AP_Compass_IIS2MDC(std::move(dev),force_external,rotation);5455if (!sensor || !sensor->init()) {56delete sensor;57return nullptr;58}5960return sensor;61}6263AP_Compass_IIS2MDC::AP_Compass_IIS2MDC(AP_HAL::OwnPtr<AP_HAL::Device> dev,64bool force_external,65enum Rotation rotation)66: _dev(std::move(dev))67, _rotation(rotation)68, _force_external(force_external)69{70}7172bool AP_Compass_IIS2MDC::init()73{74WITH_SEMAPHORE(_dev->get_semaphore());7576_dev->set_retries(10);7778if (!check_whoami()) {79return false;80}8182if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN)) {83return false;84}8586if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC)) {87return false;88}8990if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_C, BDU)) {91return false;92}9394// lower retries for run95_dev->set_retries(3);9697// register compass instance98_dev->set_device_type(DEVTYPE_IIS2MDC);99100if (!register_compass(_dev->get_bus_id())) {101return false;102}103104set_rotation(_rotation);105106if (_force_external) {107set_external(true);108}109110// Enable 100HZ111_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_IIS2MDC::timer, void));112113return true;114}115116bool AP_Compass_IIS2MDC::check_whoami()117{118uint8_t whoami = 0;119if (!_dev->read_registers(IIS2MDC_ADDR_WHO_AM_I, &whoami, 1)){120return false;121}122123return whoami == IIS2MDC_WHO_AM_I;124}125126void AP_Compass_IIS2MDC::timer()127{128struct PACKED {129uint8_t xout0;130uint8_t xout1;131uint8_t yout0;132uint8_t yout1;133uint8_t zout0;134uint8_t zout1;135uint8_t tout0;136uint8_t tout1;137} buffer;138139const float range_scale = 100.f / 65.535f; // +/- 50,000 milligauss, 16bit140141uint8_t status = 0;142if (!_dev->read_registers(IIS2MDC_ADDR_STATUS_REG, &status, 1)) {143return;144}145146if (!(status & IIS2MDC_STATUS_REG_READY)) {147return;148}149150if (!_dev->read_registers(IIS2MDC_ADDR_OUTX_L_REG, (uint8_t *) &buffer, sizeof(buffer))) {151return;152}153154const int16_t x = ((buffer.xout1 << 8) | buffer.xout0);155const int16_t y = ((buffer.yout1 << 8) | buffer.yout0);156const int16_t z = -1 * ((buffer.zout1 << 8) | buffer.zout0);157158Vector3f field{ x * range_scale, y * range_scale, z * range_scale };159160accumulate_sample(field);161}162163void AP_Compass_IIS2MDC::read()164{165drain_accumulated_samples();166}167168#endif //AP_COMPASS_IIS2MDC_ENABLED169170171