zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Compass: add memsic MMC5883 driver

zr-v5.1
Jacob Walser 4 years ago committed by Peter Barker
parent
4cddb12508
commit
8defcb9169
5 changed files with 370 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      libraries/AP_Compass/AP_Compass.cpp
  2. 1
      libraries/AP_Compass/AP_Compass.h
  3. 2
      libraries/AP_Compass/AP_Compass_Backend.h
  4. 292
      libraries/AP_Compass/AP_Compass_MMC5xx3.cpp
  5. 74
      libraries/AP_Compass/AP_Compass_MMC5xx3.h

1
libraries/AP_Compass/AP_Compass.cpp
Unescape View File

@ -25,6 +25,7 @@ @@ -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
Unescape View File

@ -431,6 +431,7 @@ private: @@ -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
Unescape View File

@ -63,6 +63,8 @@ public: @@ -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
Unescape View File

@ -0,0 +1,292 @@ @@ -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
Unescape View File

@ -0,0 +1,74 @@ @@ -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;
};
Write Preview
Loading…
Cancel
Save