aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: add memsic MMC5883 driver

Browse Source
This commit is contained in:
Jacob Walser 2021-03-23 17:06:38 -04:00 committed by Peter Barker
parent 4cddb12508
commit 8defcb9169
5 changed files with 370 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Compass/AP_Compass.cpp
View File

@ -25,6 +25,7 @@
#include "AP_Compass_UAVCAN.h"
#endif
#include "AP_Compass_MMC3416.h"
#include "AP_Compass_MMC5xx3.h"
#include "AP_Compass_MAG3110.h"
#include "AP_Compass_RM3100.h"
#if HAL_MSP_COMPASS_ENABLED

1
libraries/AP_Compass/AP_Compass.h
View File

@ -431,6 +431,7 @@ private:
DRIVER_RM3100 =16,
DRIVER_MSP =17,
DRIVER_SERIAL =18,
DRIVER_MMC5XX3 =19,
};
bool _driver_enabled(enum DriverType driver_type);

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -63,6 +63,8 @@ public:
DEVTYPE_SITL = 0x0F,
DEVTYPE_IST8308 = 0x10,
DEVTYPE_RM3100 = 0x11,
DEVTYPE_RM3100_2 = 0x12, // unused, past mistake
DEVTYPE_MMC5883 = 0x13,
};
#if HAL_MSP_COMPASS_ENABLED

292
libraries/AP_Compass/AP_Compass_MMC5xx3.cpp Normal file
View File

@ -0,0 +1,292 @@
/*
* 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_MMC5xx3.h"
#include <AP_HAL/AP_HAL.h>
#include <stdio.h>
extern const AP_HAL::HAL &hal;
#define REG_PRODUCT_ID 0x2F
#define REG_XOUT_L 0x00
#define REG_STATUS 0x07
#define REG_CONTROL0 0x08
#define REG_CONTROL1 0x09
#define REG_CONTROL2 0x0A
// bits in REG_CONTROL0
#define REG_CONTROL0_RESET 0x10
#define REG_CONTROL0_SET 0x08
#define REG_CONTROL0_TM 0x01
// bits in REG_CONTROL1
#define REG_CONTROL1_SW_RST 0x80
#define REG_CONTROL1_BW0 0x01
#define REG_CONTROL1_BW1 0x02
#define MMC5883_ID 0x0C
AP_Compass_Backend *AP_Compass_MMC5XX3::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_MMC5XX3 *sensor = new AP_Compass_MMC5XX3(std::move(dev), force_external, rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_MMC5XX3::AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
bool _force_external,
enum Rotation _rotation)
: dev(std::move(_dev))
, force_external(_force_external)
, rotation(_rotation)
{
}
bool AP_Compass_MMC5XX3::init()
{
// take i2c bus sempahore
WITH_SEMAPHORE(dev->get_semaphore());
dev->set_retries(10);
uint8_t whoami;
if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) ||
whoami != MMC5883_ID) {
// not a MMC5883
return false;
}
// reset sensor
dev->write_register(REG_CONTROL1, REG_CONTROL1_SW_RST);
// 5ms minimum startup time
hal.scheduler->delay(10);
if (!dev->write_register(REG_CONTROL1, REG_CONTROL1_BW0 | REG_CONTROL1_BW1)) {
return false;
} // 16 bit operation, 1.6ms measurement time
/* register the compass instance in the frontend */
dev->set_device_type(DEVTYPE_MMC5883);
if (!register_compass(dev->get_bus_id(), compass_instance)) {
return false;
}
set_dev_id(compass_instance, dev->get_bus_id());
printf("Found a MMC5883 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
set_rotation(compass_instance, rotation);
if (force_external) {
set_external(compass_instance, true);
}
dev->set_retries(1);
// call timer() at 1kHz
dev->register_periodic_callback(1000,
FUNCTOR_BIND_MEMBER(&AP_Compass_MMC5XX3::timer, void));
return true;
}
void AP_Compass_MMC5XX3::timer()
{
// recalculate the offset with set/reset operation every measure_count_limit measurements
// sensor is read at about 500Hz, so about every 10 seconds
const uint16_t measure_count_limit = 5000;
const uint16_t zero_offset = 32768; // 16 bit mode
const uint16_t sensitivity = 4096; // counts per Gauss, 16 bit mode
constexpr float counts_to_milliGauss = 1.0e3f / sensitivity;
/*
we use the SET/RESET method to remove bridge offset every
measure_count_limit measurements. This involves a fairly complex
state machine, but means we are much less sensitive to
temperature changes
*/
switch (state) {
// perform a set operation
case MMCState::STATE_SET: {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET)) {
break;
}
// minimum time to wait after set/reset before take measurement request is 1ms
state = MMCState::STATE_SET_MEASURE;
break;
}
// request a measurement for field and offset calculation after set operation
case MMCState::STATE_SET_MEASURE: {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
break;
}
state = MMCState::STATE_SET_WAIT;
break;
}
// wait for measurement to be ready after set operation, then read the
// measurement data and request a reset operation
case MMCState::STATE_SET_WAIT: {
uint8_t status;
if (!dev->read_registers(REG_STATUS, &status, 1)) {
state = MMCState::STATE_SET;
break;
}
// check if measurement is ready
if (!(status & 1)) {
break;
}
// read measurement
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data0[0], 6)) {
state = MMCState::STATE_SET;
break;
}
// request set operation
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET)) {
break;
}
// minimum time to wait after set/reset before take measurement request is 1ms
state = MMCState::STATE_RESET_MEASURE;
break;
}
// request a measurement for field and offset calculation after reset operation
case MMCState::STATE_RESET_MEASURE: {
// take measurement request
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
state = MMCState::STATE_SET;
break;
}
state = MMCState::STATE_RESET_WAIT;
break;
}
// wait for measurement to be ready after reset operation,
// then read the measurement data, calculate the field and offset,
// and begin requesting field measurements
case MMCState::STATE_RESET_WAIT: {
uint8_t status;
if (!dev->read_registers(REG_STATUS, &status, 1)) {
state = MMCState::STATE_SET;
break;
}
// check if measurement is ready
if (!(status & 1)) {
break;
}
uint16_t data1[3];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
state = MMCState::STATE_SET;
break;
}
/*
calculate field and offset
*/
Vector3f f1 {float(data0[0]) - zero_offset,
float(data0[1]) - zero_offset,
float(data0[2]) - zero_offset};
Vector3f f2 {float(data1[0]) - zero_offset,
float(data1[1]) - zero_offset,
float(data1[2]) - zero_offset};
Vector3f field {(f1 - f2) * counts_to_milliGauss * 0.5f};
Vector3f new_offset {(f1 + f2) * counts_to_milliGauss * 0.5f};
if (!have_initial_offset) {
offset = new_offset;
have_initial_offset = true;
} else {
// low pass changes to the offset
offset = offset * 0.5f + new_offset * 0.5f;
}
accumulate_sample(field, compass_instance);
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
printf("failed to initiate measurement\n");
state = MMCState::STATE_SET;
} else {
state = MMCState::STATE_MEASURE;
}
break;
}
// take repeated field measurements, set/reset is performed again after
// measure_count_limit measurements
case MMCState::STATE_MEASURE: {
uint8_t status;
if (!dev->read_registers(REG_STATUS, &status, 1)) {
state = MMCState::STATE_SET;
break;
}
// check if measurement is ready
if (!(status & 1)) {
break;
}
uint16_t data1[3];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
printf("cant read data\n");
state = MMCState::STATE_SET;
break;
}
Vector3f field {float(data1[0]) - zero_offset,
float(data1[1]) - zero_offset,
float(data1[2]) - zero_offset};
field *= counts_to_milliGauss;
field += offset;
accumulate_sample(field, compass_instance);
// we stay in STATE_MEASURE for measure_count_limit cycles
if (measure_count++ >= measure_count_limit) {
measure_count = 0;
state = MMCState::STATE_SET;
} else {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = MMCState::STATE_SET;
}
}
break;
}
}
}
void AP_Compass_MMC5XX3::read()
{
drain_accumulated_samples(compass_instance);
}

74
libraries/AP_Compass/AP_Compass_MMC5xx3.h Normal file
View File

@ -0,0 +1,74 @@
/*
* 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/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_Math/AP_Math.h>
#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
#ifndef HAL_COMPASS_MMC5xx3_I2C_ADDR
# define HAL_COMPASS_MMC5xx3_I2C_ADDR 0x30
#endif
class AP_Compass_MMC5XX3 : public AP_Compass_Backend
{
public:
static AP_Compass_Backend *probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation);
void read() override;
static constexpr const char *name = "MMC5883";
private:
AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation);
AP_HAL::OwnPtr<AP_HAL::Device> dev;
enum class MMCState {
STATE_SET,
STATE_SET_MEASURE,
STATE_SET_WAIT,
STATE_RESET_MEASURE,
STATE_RESET_WAIT,
STATE_MEASURE,
} state;
/**
* Device periodic callback to read data from the sensor.
*/
bool init();
void timer();
void accumulate_field(Vector3f &field);
uint8_t compass_instance;
bool force_external;
Vector3f offset;
uint16_t measure_count;
bool have_initial_offset;
uint32_t refill_start_ms;
uint32_t last_sample_ms;
uint16_t data0[3];
enum Rotation rotation;
};