Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 14:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: update MMMC5XX3 driver to support only mmc5983

Browse Source 
Registers changed
Product ID changed
Data is now Big endian
Results are now 18 bits instead of 16, but we only consume 16.
Added SPI support

Co-authored-by: Patrick Pereira <patrickelectric@gmail.com>
Co-authored-by: Jacob Walser <jwalser90@gmail.com>
This commit is contained in:
Willian Galvani 2021-11-03 17:16:18 -03:00 committed by Andrew Tridgell
parent 31fede4337
commit 4c756fd7ef
3 changed files with 46 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -64,7 +64,7 @@ public:
DEVTYPE_IST8308 = 0x10, DEVTYPE_IST8308 = 0x10,
DEVTYPE_RM3100 = 0x11, DEVTYPE_RM3100 = 0x11,
DEVTYPE_RM3100_2 = 0x12, // unused, past mistake DEVTYPE_RM3100_2 = 0x12, // unused, past mistake
DEVTYPE_MMC5883 = 0x13, DEVTYPE_MMC5983 = 0x13,
DEVTYPE_AK09918 = 0x14, DEVTYPE_AK09918 = 0x14,
}; };

77
libraries/AP_Compass/AP_Compass_MMC5xx3.cpp
View File

@ -22,24 +22,25 @@ extern const AP_HAL::HAL &hal;
#define REG_PRODUCT_ID 0x2F #define REG_PRODUCT_ID 0x2F
#define REG_XOUT_L 0x00 #define REG_XOUT_L 0x00
#define REG_STATUS 0x07 #define REG_STATUS 0x08
#define REG_CONTROL0 0x08 #define REG_CONTROL0 0x09
#define REG_CONTROL1 0x09 #define REG_CONTROL1 0x0A
#define REG_CONTROL2 0x0A #define REG_CONTROL2 0x0B
// bits in REG_CONTROL0 // bits in REG_CONTROL0
#define REG_CONTROL0_RESET 0x10 #define REG_CONTROL0_RESET 0x10 // Set coil for measuring offset
#define REG_CONTROL0_SET 0x08 #define REG_CONTROL0_SET 0x08 // Reset coil for measuring offset
#define REG_CONTROL0_TM 0x01 #define REG_CONTROL0_TMM 0x01 // Take Measurement for Magnetic field
#define REG_CONTROL0_TMT 0x02 // Take Measurement for Temperature
// bits in REG_CONTROL1 // bits in REG_CONTROL1
#define REG_CONTROL1_SW_RST 0x80 #define REG_CONTROL1_SW_RST 0x80 // Software reset
#define REG_CONTROL1_BW0 0x01 #define REG_CONTROL1_BW0 0x01
#define REG_CONTROL1_BW1 0x02 #define REG_CONTROL1_BW1 0x02
#define MMC5883_ID 0x0C #define MMC5983_ID 0x30
AP_Compass_Backend *AP_Compass_MMC5XX3::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, AP_Compass_Backend *AP_Compass_MMC5XX3::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external, bool force_external,
enum Rotation rotation) enum Rotation rotation)
{ {
@ -61,6 +62,7 @@ AP_Compass_MMC5XX3::AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
: dev(std::move(_dev)) : dev(std::move(_dev))
, force_external(_force_external) , force_external(_force_external)
, rotation(_rotation) , rotation(_rotation)
, have_initial_offset(false)
{ {
} }
@ -71,32 +73,38 @@ bool AP_Compass_MMC5XX3::init()
dev->set_retries(10); dev->set_retries(10);
// setup to allow reads on SPI
if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
dev->set_read_flag(0x80);
}
uint8_t whoami; uint8_t whoami;
if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) || if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) ||
whoami != MMC5883_ID) { whoami != MMC5983_ID) {
// not a MMC5883 // not a MMC5983
return false; return false;
} }
// reset sensor // reset sensor
dev->write_register(REG_CONTROL1, REG_CONTROL1_SW_RST); dev->write_register(REG_CONTROL1, REG_CONTROL1_SW_RST);
// 5ms minimum startup time // 10ms minimum startup time
hal.scheduler->delay(10); hal.scheduler->delay(15);
if (!dev->write_register(REG_CONTROL1, REG_CONTROL1_BW0 | REG_CONTROL1_BW1)) { if (!dev->write_register(REG_CONTROL1, REG_CONTROL1_BW0 | REG_CONTROL1_BW1)) {
return false; return false;
} // 16 bit operation, 1.6ms measurement time } // // This BW config sets the sensor measurement time to 0.5ms and filter bandwidth to 800Hz
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
dev->set_device_type(DEVTYPE_MMC5883); dev->set_device_type(DEVTYPE_MMC5983);
if (!register_compass(dev->get_bus_id(), compass_instance)) { if (!register_compass(dev->get_bus_id(), compass_instance)) {
return false; return false;
} }
set_dev_id(compass_instance, dev->get_bus_id()); 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); printf("Found a MMC5983 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
set_rotation(compass_instance, rotation); set_rotation(compass_instance, rotation);
@ -142,7 +150,7 @@ void AP_Compass_MMC5XX3::timer()
// request a measurement for field and offset calculation after set operation // request a measurement for field and offset calculation after set operation
case MMCState::STATE_SET_MEASURE: { case MMCState::STATE_SET_MEASURE: {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
break; break;
} }
state = MMCState::STATE_SET_WAIT; state = MMCState::STATE_SET_WAIT;
@ -181,7 +189,7 @@ void AP_Compass_MMC5XX3::timer()
// request a measurement for field and offset calculation after reset operation // request a measurement for field and offset calculation after reset operation
case MMCState::STATE_RESET_MEASURE: { case MMCState::STATE_RESET_MEASURE: {
// take measurement request // take measurement request
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
break; break;
} }
@ -199,13 +207,12 @@ void AP_Compass_MMC5XX3::timer()
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
break; break;
} }
// check if measurement is ready // check if measurement is ready
if (!(status & 1)) { if (!(status & 1)) {
break; break;
} }
uint16_t data1[3]; uint8_t data1[6];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) { if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
break; break;
@ -214,14 +221,14 @@ void AP_Compass_MMC5XX3::timer()
/* /*
calculate field and offset calculate field and offset
*/ */
Vector3f f1 {float(data0[0]) - zero_offset, Vector3f f1 {float((data0[0] << 8) + data0[1]) - zero_offset,
float(data0[1]) - zero_offset, float((data0[2] << 8) + data0[3]) - zero_offset,
float(data0[2]) - zero_offset}; float((data0[4] << 8) + data0[5]) - zero_offset};
Vector3f f2 {float(data1[0]) - zero_offset, Vector3f f2 {float((data1[0] << 8) + data1[1]) - zero_offset,
float(data1[1]) - zero_offset, float((data1[2] << 8) + data1[3]) - zero_offset,
float(data1[2]) - zero_offset}; float((data1[4] << 8) + data1[5]) - zero_offset};
Vector3f field {(f1 - f2) * counts_to_milliGauss * 0.5f}; Vector3f field {(f2 - f1) * counts_to_milliGauss * 0.5f};
Vector3f new_offset {(f1 + f2) * counts_to_milliGauss * 0.5f}; Vector3f new_offset {(f1 + f2) * counts_to_milliGauss * 0.5f};
if (!have_initial_offset) { if (!have_initial_offset) {
@ -234,7 +241,7 @@ void AP_Compass_MMC5XX3::timer()
accumulate_sample(field, compass_instance); accumulate_sample(field, compass_instance);
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
printf("failed to initiate measurement\n"); printf("failed to initiate measurement\n");
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
} else { } else {
@ -258,18 +265,18 @@ void AP_Compass_MMC5XX3::timer()
break; break;
} }
uint16_t data1[3]; uint8_t data1[6];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) { if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
printf("cant read data\n"); printf("cant read data\n");
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
break; break;
} }
Vector3f field {float(data1[0]) - zero_offset, Vector3f field {float((data1[0] << 8) + data1[1]) - zero_offset,
float(data1[1]) - zero_offset, float((data1[2] << 8) + data1[3]) - zero_offset,
float(data1[2]) - zero_offset}; float((data1[4] << 8) + data1[5]) - zero_offset};
field *= counts_to_milliGauss; field *= counts_to_milliGauss;
field += offset; field -= offset;
accumulate_sample(field, compass_instance); accumulate_sample(field, compass_instance);
// we stay in STATE_MEASURE for measure_count_limit cycles // we stay in STATE_MEASURE for measure_count_limit cycles
@ -277,7 +284,7 @@ void AP_Compass_MMC5XX3::timer()
measure_count = 0; measure_count = 0;
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
} else { } else {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) { // Take Measurement
state = MMCState::STATE_SET; state = MMCState::STATE_SET;
} }
} }

6
libraries/AP_Compass/AP_Compass_MMC5xx3.h
View File

@ -29,13 +29,13 @@
class AP_Compass_MMC5XX3 : public AP_Compass_Backend class AP_Compass_MMC5XX3 : public AP_Compass_Backend
{ {
public: public:
static AP_Compass_Backend *probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, static AP_Compass_Backend *probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external, bool force_external,
enum Rotation rotation); enum Rotation rotation);
void read() override; void read() override;
static constexpr const char *name = "MMC5883"; static constexpr const char *name = "MMC5983";
private: private:
AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> dev, AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> dev,
@ -68,7 +68,7 @@ private:
uint32_t refill_start_ms; uint32_t refill_start_ms;
uint32_t last_sample_ms; uint32_t last_sample_ms;
uint16_t data0[3]; uint8_t data0[6];
enum Rotation rotation; enum Rotation rotation;
}; };