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

AP_Compass: Add in QMC5883P Driver

This commit is contained in:
Jonathan Loong 2023-11-10 20:00:55 -08:00 committed by Andrew Tridgell
parent 16c55a3bc1
commit 0db1719c8f
6 changed files with 324 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Compass/AP_Compass.cpp
View File

@ -27,6 +27,7 @@
#if AP_COMPASS_DRONECAN_ENABLED
#include "AP_Compass_DroneCAN.h"
#endif
#include "AP_Compass_QMC5883P.h"
#include "AP_Compass_MMC3416.h"
#include "AP_Compass_MMC5xx3.h"
#include "AP_Compass_MAG3110.h"
@ -1132,6 +1133,26 @@ void Compass::_probe_external_i2c_compasses(void)
#endif
#endif // AP_COMPASS_QMC5883L_ENABLED
#if AP_COMPASS_QMC5883P_ENABLED
//external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL));
}
// internal i2c bus
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
if (all_external) {
// only probe QMC5883P on internal if we are treating internals as externals
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
all_external,
all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL));
}
}
#endif
#endif // AP_COMPASS_QMC5883P_ENABLED
#ifndef HAL_BUILD_AP_PERIPH
// AK09916 on ICM20948
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED

5
libraries/AP_Compass/AP_Compass.h
View File

@ -481,7 +481,10 @@ private:
#if AP_COMPASS_MMC5XX3_ENABLED
DRIVER_MMC5XX3 =19,
#endif
};
#if AP_COMPASS_QMC5883P_ENABLED
DRIVER_QMC5883P =20,
#endif
};
bool _driver_enabled(enum DriverType driver_type);

1
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -73,6 +73,7 @@ public:
DEVTYPE_MMC5983 = 0x13,
DEVTYPE_AK09918 = 0x14,
DEVTYPE_AK09915 = 0x15,
DEVTYPE_QMC5883P = 0x16,
};
#if AP_COMPASS_MSP_ENABLED

221
libraries/AP_Compass/AP_Compass_QMC5883P.cpp Normal file
View File

@ -0,0 +1,221 @@
/*
* 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/>.
*
* Driver by Lokesh Ramina, Jan 2022
*/
#include "AP_Compass_QMC5883P.h"
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#if AP_COMPASS_QMC5883P_ENABLED
//Register Address
#define QMC5883P_REG_ID 0x00
#define QMC5883P_REG_DATA_OUTPUT_X 0x01
#define QMC5883P_REG_DATA_OUTPUT_Z_MSB 0x06
#define QMC5883P_REG_STATUS 0x09
#define QMC5883P_REG_CONF1 0x0A
#define QMC5883P_REG_CONF2 0x0B
#define QMC5883P_ID_VAL 0x80
//Register values
// Sensor operation modes
#define QMC5883P_MODE_SUSPEND 0x00
#define QMC5883P_MODE_NORMAL 0x01
#define QMC5883P_MODE_SINGLE 0x02
#define QMC5883P_MODE_CONTINUOUS 0x03
// ODR data output rates for 5883L
#define QMC5883P_ODR_10HZ (0x00 << 2)
#define QMC5883P_ODR_50HZ (0x01 << 2)
#define QMC5883P_ODR_100HZ (0x02 << 2)
#define QMC5883P_ODR_200HZ (0x03 << 2)
// Over sampling Ratio OSR1
#define QMC5883P_OSR1_8 (0x00 << 4)
#define QMC5883P_OSR1_4 (0x01 << 4)
#define QMC5883P_OSR1_2 (0x02 << 4)
#define QMC5883P_OSR1_1 (0x03 << 4)
// Down sampling Rate OSR2
#define QMC5883P_OSR2_8 0x08
//RNG
#define QMC5883P_RNG_30G (0x00 << 2)
#define QMC5883P_RNG_12G (0x01 << 2)
#define QMC5883P_RNG_8G (0x10 << 2)
#define QMC5883P_RNG_2G (0x11 << 2)
#define QMC5883P_SET_XYZ_SIGN 0x29
//Reset
#define QMC5883P_RST 0x80
//Status Val
#define QMC5883P_STATUS_DATA_READY 0x01
#ifndef DEBUG
#define DEBUG 0
#endif
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_QMC5883P *sensor = new AP_Compass_QMC5883P(std::move(dev),force_external,rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_QMC5883P::AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation)
: _dev(std::move(dev))
, _rotation(rotation)
, _force_external(force_external)
{
}
bool AP_Compass_QMC5883P::init()
{
_dev->get_semaphore()->take_blocking();
_dev->set_retries(10);
#if DEBUG
_dump_registers();
#endif
if (!_check_whoami()) {
goto fail;
}
//As mentioned in the Datasheet 7.2 to do continues mode 0x29 will set sign for X,Y,Z
if (!_dev->write_register(QMC5883P_REG_DATA_OUTPUT_Z_MSB, QMC5883P_SET_XYZ_SIGN)||
!_dev->write_register(QMC5883P_REG_CONF1,
QMC5883P_MODE_CONTINUOUS|
QMC5883P_ODR_100HZ|
QMC5883P_OSR1_8|
QMC5883P_OSR2_8)||
!_dev->write_register(QMC5883P_REG_CONF2,QMC5883P_OSR2_8)) {
goto fail;
}
// lower retries for run
_dev->set_retries(3);
_dev->get_semaphore()->give();
//register compass instance
_dev->set_device_type(DEVTYPE_QMC5883P);
if (!register_compass(_dev->get_bus_id(), _instance)) {
return false;
}
set_dev_id(_instance, _dev->get_bus_id());
printf("%s found on bus %u id %u address 0x%02x\n", name,
_dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
set_rotation(_instance, _rotation);
if (_force_external) {
set_external(_instance, true);
}
//Enable 100HZ
_dev->register_periodic_callback(10000,
FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883P::timer, void));
return true;
fail:
_dev->get_semaphore()->give();
return false;
}
bool AP_Compass_QMC5883P::_check_whoami()
{
uint8_t whoami;
if (!_dev->read_registers(QMC5883P_REG_ID, &whoami,1)||
whoami != QMC5883P_ID_VAL) {
return false;
}
return true;
}
void AP_Compass_QMC5883P::timer()
{
struct PACKED {
int16_t rx;
int16_t ry;
int16_t rz;
} buffer;
const float range_scale = 1000.0f / 3000.0f;
uint8_t status;
if (!_dev->read_registers(QMC5883P_REG_STATUS,&status,1)) {
return;
}
//new data is ready
if (!(status & QMC5883P_STATUS_DATA_READY)) {
return;
}
if (!_dev->read_registers(QMC5883P_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))) {
return ;
}
auto x = buffer.rx;
auto y = buffer.ry;
auto z = buffer.rz;
#if DEBUG
printf("mag.x:%d\n",x);
printf("mag.y:%d\n",y);
printf("mag.z:%d\n",z);
#endif
Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale };
accumulate_sample(field, _instance, 20);
}
void AP_Compass_QMC5883P::read()
{
drain_accumulated_samples(_instance);
}
void AP_Compass_QMC5883P::_dump_registers()
{
printf("QMC5883P registers dump\n");
for (uint8_t reg = QMC5883P_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) {
uint8_t v;
_dev->read_registers(reg,&v,1);
printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - ( QMC5883P_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
printf("\n");
}
}
}
#endif //AP_COMPASS_QMC5883P_ENABLED

73
libraries/AP_Compass/AP_Compass_QMC5883P.h Normal file
View File

@ -0,0 +1,73 @@
/*
* 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/>.
*
* Driver by Lokesh Ramina, Jan 2022
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_Math/AP_Math.h>
#include "AP_Compass_config.h"
#ifdef AP_COMPASS_QMC5883P_ENABLED
#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
#ifndef HAL_COMPASS_QMC5883P_I2C_ADDR
#define HAL_COMPASS_QMC5883P_I2C_ADDR 0x2C
#endif
/*
setup default orientations
*/
#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL
#define HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL ROTATION_ROLL_180
#endif
#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL
#define HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL ROTATION_ROLL_180_YAW_270
#endif
class AP_Compass_QMC5883P : 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 = "QMC5883P";
private:
AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation);
void _dump_registers();
bool _check_whoami();
void timer();
bool init();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external:1;
};
#endif // AP_COMPASS_QMC5883P_ENABLED

4
libraries/AP_Compass/AP_Compass_config.h
View File

@ -112,6 +112,10 @@
#define AP_COMPASS_MMC5XX3_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_COMPASS_QMC5883P_ENABLED
#define AP_COMPASS_QMC5883P_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED && (BOARD_FLASH_SIZE > 1024)
#endif
#ifndef AP_COMPASS_QMC5883L_ENABLED
#define AP_COMPASS_QMC5883L_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
#endif