Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-15 11:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: merged compass changes from plane3.8

Browse Source
This commit is contained in:
Andrew Tridgell 2017-11-04 18:28:07 +11:00 committed by Randy Mackay
parent 51e9091ca3
commit 2e11bef518
18 changed files with 1321 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

364
libraries/AP_Compass/AP_Compass.cpp
View File

@ -5,6 +5,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "AP_Compass_SITL.h"
#include "AP_Compass_AK8963.h"
#include "AP_Compass_Backend.h"
#include "AP_Compass_BMM150.h"
@ -17,9 +18,11 @@
#include "AP_Compass_qflight.h"
#include "AP_Compass_LIS3MDL.h"
#include "AP_Compass_AK09916.h"
#include "AP_Compass_QMC5883L.h"
#if HAL_WITH_UAVCAN
#include "AP_Compass_UAVCAN.h"
#endif
#include "AP_Compass_MMC3416.h"
#include "AP_Compass.h"
extern AP_HAL::HAL& hal;
@ -31,7 +34,7 @@ extern AP_HAL::HAL& hal;
#endif
#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 600
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 850
#endif
const AP_Param::GroupInfo Compass::var_info[] = {
@ -41,7 +44,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass offsets in milligauss on the X axis
// @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
@ -49,7 +52,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass offsets in milligauss on the Y axis
// @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
@ -57,7 +60,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass offsets in milligauss on the Z axis
// @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("OFS", 1, Compass, _state[0].offset, 0),
@ -66,7 +69,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass declination
// @Description: An angle to compensate between the true north and magnetic north
// @Range: -3.142 3.142
// @Units: Radians
// @Units: rad
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("DEC", 2, Compass, _declination, 0),
@ -101,25 +104,25 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Param: MOT_X
// @DisplayName: Motor interference compensation for body frame X axis
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Param: MOT_Y
// @DisplayName: Motor interference compensation for body frame Y axis
// @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Param: MOT_Z
// @DisplayName: Motor interference compensation for body frame Z axis
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("MOT", 7, Compass, _state[0].motor_compensation, 0),
@ -142,7 +145,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass2 offsets in milligauss on the X axis
// @Description: Offset to be added to compass2's x-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
@ -150,7 +153,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass2 offsets in milligauss on the Y axis
// @Description: Offset to be added to compass2's y-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
@ -158,32 +161,32 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass2 offsets in milligauss on the Z axis
// @Description: Offset to be added to compass2's z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("OFS2", 10, Compass, _state[1].offset, 0),
// @Param: MOT2_X
// @DisplayName: Motor interference compensation to compass2 for body frame X axis
// @Description: Multiplied by the current throttle and added to compass2's x-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to compass2's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Param: MOT2_Y
// @DisplayName: Motor interference compensation to compass2 for body frame Y axis
// @Description: Multiplied by the current throttle and added to compass2's y-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to compass2's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Param: MOT2_Z
// @DisplayName: Motor interference compensation to compass2 for body frame Z axis
// @Description: Multiplied by the current throttle and added to compass2's z-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to compass2's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("MOT2", 11, Compass, _state[1].motor_compensation, 0),
@ -199,7 +202,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass3 offsets in milligauss on the X axis
// @Description: Offset to be added to compass3's x-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
@ -207,7 +210,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass3 offsets in milligauss on the Y axis
// @Description: Offset to be added to compass3's y-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
@ -215,32 +218,32 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @DisplayName: Compass3 offsets in milligauss on the Z axis
// @Description: Offset to be added to compass3's z-axis values to compensate for metal in the frame
// @Range: -400 400
// @Units: milligauss
// @Units: mGauss
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("OFS3", 13, Compass, _state[2].offset, 0),
// @Param: MOT3_X
// @DisplayName: Motor interference compensation to compass3 for body frame X axis
// @Description: Multiplied by the current throttle and added to compass3's x-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to compass3's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Param: MOT3_Y
// @DisplayName: Motor interference compensation to compass3 for body frame Y axis
// @Description: Multiplied by the current throttle and added to compass3's y-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to compass3's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Param: MOT3_Z
// @DisplayName: Motor interference compensation to compass3 for body frame Z axis
// @Description: Multiplied by the current throttle and added to compass3's z-axis values to compensate for motor interference
// @Description: Multiplied by the current throttle and added to compass3's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("MOT3", 14, Compass, _state[2].motor_compensation, 0),
@ -417,6 +420,13 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
// @Param: TYPEMASK
// @DisplayName: Compass disable driver type mask
// @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup
// @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,10:QFLIGHT,11:UAVCAN,12:QMC5883
// @User: Advanced
AP_GROUPINFO("TYPEMASK", 33, Compass, _driver_type_mask, 0),
AP_GROUPEND
};
@ -490,6 +500,29 @@ bool Compass::_add_backend(AP_Compass_Backend *backend, const char *name, bool e
return true;
}
/*
return true if a driver type is enabled
*/
bool Compass::_driver_enabled(enum DriverType driver_type)
{
uint32_t mask = (1U<<uint8_t(driver_type));
return (mask & uint32_t(_driver_type_mask.get())) == 0;
}
/*
see if we already have probed a driver by bus type
*/
bool Compass::_have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const
{
uint32_t id = AP_HAL::Device::make_bus_id(bus_type, bus_num, address, devtype);
for (uint8_t i=0; i<_compass_count; i++) {
if (id == uint32_t(_state[i].dev_id.get())) {
return true;
}
}
return false;
}
/*
detect available backends for this board
*/
@ -500,20 +533,32 @@ void Compass::_detect_backends(void)
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
// default to disabling LIS3MDL on pixhawk2 due to hardware issue
_driver_type_mask.set_default(1U<<DRIVER_LIS3MDL);
}
#endif
/*
macro to add a backend with check for too many backends or compass
instances. We don't try to start more than the maximum allowed
*/
#define ADD_BACKEND(backend, name, external) \
do { _add_backend(backend, name, external); \
#define ADD_BACKEND(driver_type, backend, name, external) \
do { if (_driver_enabled(driver_type)) { _add_backend(backend, name, external); } \
if (_backend_count == COMPASS_MAX_BACKEND || \
_compass_count == COMPASS_MAX_INSTANCES) { \
return; \
} \
} while (0)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL(*this), nullptr, false);
return;
#endif
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
ADD_BACKEND(AP_Compass_HIL::detect(*this), nullptr, false);
ADD_BACKEND(DRIVER_SITL, AP_Compass_HIL::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
@ -522,42 +567,77 @@ void Compass::_detect_backends(void)
case AP_BoardConfig::PX4_BOARD_AUAV21:
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
case AP_BoardConfig::PX4_BOARD_PIXRACER: {
#if 0
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
#endif
case AP_BoardConfig::PX4_BOARD_PIXRACER:{
bool both_i2c_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
// external i2c bus
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
// internal i2c bus
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
AP_Compass_HMC5843::name, both_i2c_external);
#if !HAL_MINIMIZE_FEATURES
#if 0
// lis3mdl - this is disabled for now due to an errata on pixhawk2 GPS unit, pending investigation
ADD_BACKEND(AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_LIS3MDL_I2C_ADDR),
true, ROTATION_YAW_90),
AP_Compass_LIS3MDL::name, true);
ADD_BACKEND(AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
#endif
// AK09916
ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
true, ROTATION_YAW_270),
//external i2c bus
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR),
true,ROTATION_ROLL_180),
AP_Compass_QMC5883L::name, true);
//internal i2c bus
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
AP_Compass_QMC5883L::name,both_i2c_external);
#if !HAL_MINIMIZE_FEATURES
// AK09916 on ICM20948
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
hal.i2c_mgr->get_device(1, HAL_COMPASS_ICM20948_I2C_ADDR),
true, ROTATION_NONE),
AP_Compass_AK09916::name, true);
ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
AP_Compass_AK09916::name, both_i2c_external);
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
hal.i2c_mgr->get_device(0, HAL_COMPASS_ICM20948_I2C_ADDR),
both_i2c_external, ROTATION_NONE),
AP_Compass_AK09916::name, true);
// lis3mdl
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_LIS3MDL_I2C_ADDR),
true, ROTATION_YAW_90),
AP_Compass_LIS3MDL::name, true);
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
// AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 1, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) {
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
true, ROTATION_YAW_270),
AP_Compass_AK09916::name, true);
}
if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 0, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) {
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
AP_Compass_AK09916::name, both_i2c_external);
}
// IST8310 on external and internal bus
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180), AP_Compass_IST8310::name, true);
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR),
both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external);
#endif // HAL_MINIMIZE_FEATURES
}
break;
case AP_BoardConfig::PX4_BOARD_AEROFC:
#ifdef HAL_COMPASS_IST8310_I2C_BUS
ADD_BACKEND(AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#endif
break;
default:
@ -565,43 +645,53 @@ void Compass::_detect_backends(void)
}
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)),
AP_Compass_LSM303D::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)),
AP_Compass_LSM303D::name, false);
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270),
AP_Compass_LSM303D::name, false);
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270),
AP_Compass_LSM303D::name, false);
// we run the AK8963 only on the 2nd MPU9250, which leaves the
// first MPU9250 to run without disturbance at high rate
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270),
AP_Compass_AK8963::name, false);
break;
case AP_BoardConfig::PX4_BOARD_PIXRACER:
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false);
break;
#if 0
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME),
false, ROTATION_NONE),
AP_Compass_LIS3MDL::name, false);
break;
#endif
case AP_BoardConfig::PX4_BOARD_PHMINI:
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180),
AP_Compass_AK8963::name, false);
break;
case AP_BoardConfig::PX4_BOARD_AUAV21:
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false);
break;
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270),
AP_Compass_AK8963::name, false);
break;
default:
@ -609,29 +699,29 @@ void Compass::_detect_backends(void)
}
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT
ADD_BACKEND(AP_Compass_QURT::detect(*this), nullptr, false);
ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QURT::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")),
AP_Compass_LSM303D::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")),
AP_Compass_LSM303D::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT
ADD_BACKEND(AP_Compass_QFLIGHT::detect(*this));
ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QFLIGHT::detect(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1),
AP_Compass_AK8963::name, true);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1),
AP_Compass_AK8963::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
ADD_BACKEND(AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(AP_Compass_HMC5843::probe(*this,
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this,
Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device(
{ /* UEFI with lpss set to ACPI */
"platform/80860F41:05",
@ -641,67 +731,71 @@ void Compass::_detect_backends(void)
true),
AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180),
AP_Compass_LSM9DS1::name, false);
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180),
AP_Compass_LSM9DS1::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
ADD_BACKEND(AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
ADD_BACKEND(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AERO
ADD_BACKEND(AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
AP_Compass_BMM150::name, false);
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
AP_Compass_BMM150::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
AP_Compass_LSM9DS1::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
AP_Compass_LSM9DS1::name, false);
#else
#error Unrecognised HAL_COMPASS_TYPE setting
#endif
#if HAL_WITH_UAVCAN
if ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr))
{
if((_backend_count < COMPASS_MAX_BACKEND) && (_compass_count < COMPASS_MAX_INSTANCES))
{
printf("Creating AP_Compass_UAVCAN\n\r");
_backends[_backend_count] = new AP_Compass_UAVCAN(*this);
_backend_count++;
}
if (_driver_enabled(DRIVER_UAVCAN)) {
bool added;
do {
added = _add_backend(new AP_Compass_UAVCAN(*this), "UAVCAN", true);
if (_backend_count == COMPASS_MAX_BACKEND || _compass_count == COMPASS_MAX_INSTANCES) {
return;
}
} while (added);
}
#endif

35
libraries/AP_Compass/AP_Compass.h
View File

@ -122,7 +122,7 @@ public:
/*
handle an incoming MAG_CAL command
*/
uint8_t handle_mag_cal_command(const mavlink_command_long_t &packet);
MAV_RESULT handle_mag_cal_command(const mavlink_command_long_t &packet);
void send_mag_cal_progress(mavlink_channel_t chan);
void send_mag_cal_report(mavlink_channel_t chan);
@ -261,6 +261,9 @@ public:
uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; }
uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; }
uint32_t last_update_ms(void) const { return _state[get_primary()].last_update_ms; }
uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; }
static const struct AP_Param::GroupInfo var_info[];
// HIL variables
@ -286,7 +289,7 @@ public:
uint16_t get_offsets_max(void) const {
return (uint16_t)_offset_max.get();
}
private:
/// Register a new compas driver, allocating an instance number
///
@ -307,6 +310,9 @@ private:
bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
bool _auto_reboot() { return _compass_cal_autoreboot; }
// see if we already have probed a driver by bus type
bool _have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const;
//keep track of which calibrators have been saved
bool _cal_saved[COMPASS_MAX_INSTANCES];
@ -317,6 +323,26 @@ private:
bool _cal_complete_requires_reboot;
bool _cal_has_run;
// enum of drivers for COMPASS_TYPEMASK
enum DriverType {
DRIVER_HMC5883 =0,
DRIVER_LSM303D =1,
DRIVER_AK8963 =2,
DRIVER_BMM150 =3,
DRIVER_LSM9DS1 =4,
DRIVER_LIS3MDL =5,
DRIVER_AK09916 =6,
DRIVER_IST8310 =7,
DRIVER_ICM20948 =8,
DRIVER_MMC3416 =9,
DRIVER_QFLIGHT =10,
DRIVER_UAVCAN =11,
DRIVER_QMC5883 =12,
DRIVER_SITL =13,
};
bool _driver_enabled(enum DriverType driver_type);
// backend objects
AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND];
uint8_t _backend_count;
@ -388,11 +414,14 @@ private:
} _state[COMPASS_MAX_INSTANCES];
AP_Int16 _offset_max;
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
// if we want HIL only
bool _hil_mode:1;
AP_Float _calibration_threshold;
// mask of driver types to not load. Bit positions match DEVTYPE_ in backend
AP_Int32 _driver_type_mask;
};

65
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -37,8 +37,17 @@
#define REG_CNTL2 0x31
#define REG_CNTL3 0x32
#define REG_ICM_WHOAMI 0x00
#define REG_ICM_PWR_MGMT_1 0x06
#define REG_ICM_INT_PIN_CFG 0x0f
#define ICM_WHOAMI_VAL 0xEA
extern const AP_HAL::HAL &hal;
/*
probe for AK09916 directly on I2C
*/
AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
@ -47,7 +56,33 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
if (!dev) {
return nullptr;
}
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), force_external, rotation);
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), nullptr,
force_external,
rotation, AK09916_I2C);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
/*
probe for AK09916 connected via an ICM20948
*/
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), std::move(dev_icm),
force_external,
rotation, AK09916_ICM20948_I2C);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
@ -58,10 +93,14 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
AP_Compass_AK09916::AP_Compass_AK09916(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
AP_HAL::OwnPtr<AP_HAL::Device> _dev_icm,
bool _force_external,
enum Rotation _rotation)
enum Rotation _rotation,
enum bus_type _bus_type)
: AP_Compass_Backend(compass)
, bus_type(_bus_type)
, dev(std::move(_dev))
, dev_icm(std::move(_dev_icm))
, force_external(_force_external)
, rotation(_rotation)
{
@ -73,6 +112,26 @@ bool AP_Compass_AK09916::init()
return false;
}
if (bus_type == AK09916_ICM20948_I2C && dev_icm) {
uint8_t rval;
if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) ||
rval != ICM_WHOAMI_VAL) {
// not an ICM_WHOAMI
goto fail;
}
// see if ICM20948 is sleeping
if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) {
goto fail;
}
if (rval & 0x40) {
// bring out of sleep mode, use internal oscillator
dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00);
hal.scheduler->delay(10);
}
// enable i2c bypass
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02);
}
uint16_t whoami;
if (!dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2) ||
whoami != 0x0948) {
@ -98,7 +157,7 @@ bool AP_Compass_AK09916::init()
set_external(compass_instance, true);
}
dev->set_device_type(DEVTYPE_AK09916);
dev->set_device_type(bus_type == AK09916_ICM20948_I2C?DEVTYPE_ICM20948:DEVTYPE_AK09916);
set_dev_id(compass_instance, dev->get_bus_id());
// call timer() at 100Hz

25
libraries/AP_Compass/AP_Compass_AK09916.h
View File

@ -26,6 +26,11 @@
# define HAL_COMPASS_AK09916_I2C_ADDR 0x0C
#endif
// the AK09916 can be connected via an ICM20948
#ifndef HAL_COMPASS_ICM20948_I2C_ADDR
# define HAL_COMPASS_ICM20948_I2C_ADDR 0x69
#endif
class AP_Compass_AK09916 : public AP_Compass_Backend
{
public:
@ -34,16 +39,32 @@ public:
bool force_external = false,
enum Rotation rotation = ROTATION_NONE);
// separate probe function for when behind a ICM20948 IMU
static AP_Compass_Backend *probe_ICM20948(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
bool force_external = false,
enum Rotation rotation = ROTATION_NONE);
void read() override;
static constexpr const char *name = "AK09916";
private:
AP_Compass_AK09916(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum bus_type {
AK09916_I2C=0,
AK09916_ICM20948_I2C,
} bus_type;
AP_Compass_AK09916(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm,
bool force_external,
enum Rotation rotation);
enum Rotation rotation,
enum bus_type bus_type);
AP_HAL::OwnPtr<AP_HAL::Device> dev;
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm;
/**
* Device periodic callback to read data from the sensor.

120
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -22,6 +22,7 @@
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
#include <stdio.h>
#define CHIP_ID_REG 0x40
#define CHIP_ID_VAL 0x32
@ -100,10 +101,27 @@ bool AP_Compass_BMM150::_load_trim_values()
le16_t dig_z3;
int8_t dig_xy2;
uint8_t dig_xy1;
} PACKED trim_registers;
} PACKED trim_registers, trim_registers2;
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
sizeof(trim_registers))) {
// read the registers twice to confirm we have the right
// values. There is no CRC in the registers and these values are
// vital to correct operation
int8_t tries = 4;
while (tries--) {
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
sizeof(trim_registers))) {
continue;
}
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers2,
sizeof(trim_registers))) {
continue;
}
if (memcmp(&trim_registers, &trim_registers2, sizeof(trim_registers)) == 0) {
break;
}
}
if (-1 == tries) {
hal.console->printf("BMM150: Failed to load trim registers\n");
return false;
}
@ -132,27 +150,42 @@ bool AP_Compass_BMM150::init()
return false;
}
/* Do a soft reset */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
if (!ret) {
goto bus_error;
}
hal.scheduler->delay(2);
// 10 retries for init
_dev->set_retries(10);
/* Change power state from suspend mode to sleep mode */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL);
if (!ret) {
goto bus_error;
}
hal.scheduler->delay(2);
// use checked registers to cope with bus errors
_dev->setup_checked_registers(4);
int8_t boot_tries = 4;
while (boot_tries--) {
/* Do a soft reset */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
hal.scheduler->delay(2);
if (!ret) {
continue;
}
ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
if (!ret) {
goto bus_error;
/* Change power state from suspend mode to sleep mode */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
hal.scheduler->delay(2);
if (!ret) {
continue;
}
ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
if (!ret) {
continue;
}
if (val == CHIP_ID_VAL) {
// found it
break;
}
if (boot_tries == 0) {
hal.console->printf("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL);
}
}
if (val != CHIP_ID_VAL) {
hal.console->printf("BMM150: Wrong id\n");
goto fail;
if (-1 == boot_tries) {
goto bus_error;
}
ret = _load_trim_values();
@ -167,16 +200,16 @@ bool AP_Compass_BMM150::init()
* - ODR = 20
* But we are going to use 30Hz of ODR
*/
ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2);
ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2, true);
if (!ret) {
goto bus_error;
}
ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1);
ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1, true);
if (!ret) {
goto bus_error;
}
/* Change operation mode from sleep to normal and set ODR */
ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ);
ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ, true);
if (!ret) {
goto bus_error;
}
@ -189,14 +222,20 @@ bool AP_Compass_BMM150::init()
_dev->set_device_type(DEVTYPE_BMM150);
set_dev_id(_compass_instance, _dev->get_bus_id());
_perf_err = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "BMM150_err");
// 2 retries for run
_dev->set_retries(2);
_dev->register_periodic_callback(MEASURE_TIME_USEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
_last_read_ms = AP_HAL::millis();
return true;
bus_error:
hal.console->printf("BMM150: Bus communication error\n");
fail:
_dev->get_semaphore()->give();
return false;
}
@ -226,15 +265,23 @@ int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t tx
int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall)
{
int32_t dividend = ((int32_t)(z - _dig.z4)) << 15;
dividend -= (_dig.z3 * (rhall - _dig.xyz1)) >> 2;
int32_t dividend = int32_t(z - _dig.z4) << 15;
int32_t dividend2 = dividend - ((_dig.z3 * (int32_t(rhall) - int32_t(_dig.xyz1))) >> 2);
int32_t divisor = ((int32_t)_dig.z1) * (rhall << 1);
int32_t divisor = int32_t(_dig.z1) * (rhall << 1);
divisor += 0x8000;
divisor >>= 16;
divisor += _dig.z2;
return constrain_int32(dividend / divisor, -0x8000, 0x8000);
int16_t ret = constrain_int32(dividend2 / divisor, -0x8000, 0x8000);
#if 0
static uint8_t counter;
if (counter++ == 0) {
printf("ret=%d z=%d rhall=%u z1=%d z2=%d z3=%d z4=%d xyz1=%d dividend=%d dividend2=%d divisor=%d\n",
ret, z, rhall, _dig.z1, _dig.z2, _dig.z3, _dig.z4, _dig.xyz1, dividend, dividend2, divisor);
}
#endif
return ret;
}
void AP_Compass_BMM150::_update()
@ -246,10 +293,19 @@ void AP_Compass_BMM150::_update()
/* Checking data ready status */
if (!ret || !(data[3] & 0x1)) {
_dev->check_next_register();
uint32_t now = AP_HAL::millis();
if (now - _last_read_ms > 250) {
// cope with power cycle to sensor
_last_read_ms = now;
_dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
_dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
hal.util->perf_count(_perf_err);
}
return;
}
const uint16_t rhall = le16toh(data[3] >> 2);
const uint16_t rhall = le16toh(data[3]) >> 2;
Vector3f raw_field = Vector3f{
(float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,
@ -272,6 +328,8 @@ void AP_Compass_BMM150::_update()
/* correct raw_field for known errors */
correct_field(raw_field, _compass_instance);
_last_read_ms = AP_HAL::millis();
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
@ -282,6 +340,8 @@ void AP_Compass_BMM150::_update()
_accum_count = 5;
}
_sem->give();
_dev->check_next_register();
}
void AP_Compass_BMM150::read()

3
libraries/AP_Compass/AP_Compass_BMM150.h
View File

@ -66,4 +66,7 @@ private:
int8_t xy2;
uint16_t xyz1;
} _dig;
uint32_t _last_read_ms;
AP_HAL::Util::perf_counter_t _perf_err;
};

3
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -59,6 +59,9 @@ public:
DEVTYPE_LIS3MDL = 0x08,
DEVTYPE_AK09916 = 0x09,
DEVTYPE_IST8310 = 0x0A,
DEVTYPE_ICM20948 = 0x0B,
DEVTYPE_MMC3416 = 0x0C,
DEVTYPE_QMC5883L = 0x0D,
};

4
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -267,9 +267,9 @@ Compass::_get_cal_mask() const
/*
handle an incoming MAG_CAL command
*/
uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
{
uint8_t result = MAV_RESULT_FAILED;
MAV_RESULT result = MAV_RESULT_FAILED;
switch (packet.command) {
case MAV_CMD_DO_START_MAG_CAL: {

9
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -77,13 +77,14 @@ extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), rotation);
AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), force_external, rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
@ -94,10 +95,12 @@ AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
AP_Compass_IST8310::AP_Compass_IST8310(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
, _rotation(rotation)
, _force_external(force_external)
{
}
@ -163,6 +166,10 @@ bool AP_Compass_IST8310::init()
_dev->set_device_type(DEVTYPE_IST8310);
set_dev_id(_instance, _dev->get_bus_id());
if (_force_external) {
set_external(_instance, true);
}
_periodic_handle = _dev->register_periodic_callback(SAMPLING_PERIOD_USEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, void));

8
libraries/AP_Compass/AP_Compass_IST8310.h
View File

@ -1,4 +1,3 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* Copyright (C) 2016 Emlid Ltd. All rights reserved.
*
@ -25,11 +24,16 @@
#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
#ifndef HAL_COMPASS_IST8310_I2C_ADDR
#define HAL_COMPASS_IST8310_I2C_ADDR 0x0E
#endif
class AP_Compass_IST8310 : public AP_Compass_Backend
{
public:
static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external = false,
enum Rotation rotation = ROTATION_NONE);
void read() override;
@ -39,6 +43,7 @@ public:
private:
AP_Compass_IST8310(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation);
void timer();
@ -56,4 +61,5 @@ private:
enum Rotation _rotation;
uint8_t _instance;
bool _ignore_next_sample;
bool _force_external;
};

6
libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
View File

@ -49,7 +49,7 @@
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_LIS3MDL::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation)
{
@ -98,7 +98,7 @@ bool AP_Compass_LIS3MDL::init()
dev->setup_checked_registers(5);
dev->write_register(ADDR_CTRL_REG1, 0x62, true); // 155Hz, UHP
dev->write_register(ADDR_CTRL_REG1, 0xFC, true); // 80Hz, UHP
dev->write_register(ADDR_CTRL_REG2, 0, true); // 4Ga range
dev->write_register(ADDR_CTRL_REG3, 0, true); // continuous
dev->write_register(ADDR_CTRL_REG4, 0x0C, true); // z-axis ultra high perf
@ -124,7 +124,7 @@ bool AP_Compass_LIS3MDL::init()
set_dev_id(compass_instance, dev->get_bus_id());
// call timer() at 155Hz
dev->register_periodic_callback(1000000U/155U,
dev->register_periodic_callback(1000000U/80U,
FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, void));
return true;

4
libraries/AP_Compass/AP_Compass_LIS3MDL.h
View File

@ -16,7 +16,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/Device.h>
#include <AP_Math/AP_Math.h>
#include "AP_Compass.h"
@ -31,7 +31,7 @@ class AP_Compass_LIS3MDL : public AP_Compass_Backend
{
public:
static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external = false,
enum Rotation rotation = ROTATION_NONE);

333
libraries/AP_Compass/AP_Compass_MMC3416.cpp Normal file
View File

@ -0,0 +1,333 @@
/*
* 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 Andrew Tridgell, Nov 2016
*/
#include "AP_Compass_MMC3416.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
#include <AP_Math/AP_Math.h>
#include <stdio.h>
#include <DataFlash/DataFlash.h>
extern const AP_HAL::HAL &hal;
#define REG_PRODUCT_ID 0x20
#define REG_XOUT_L 0x00
#define REG_STATUS 0x06
#define REG_CONTROL0 0x07
#define REG_CONTROL1 0x08
// bits in REG_CONTROL0
#define REG_CONTROL0_REFILL 0x80
#define REG_CONTROL0_RESET 0x40
#define REG_CONTROL0_SET 0x20
#define REG_CONTROL0_NB 0x10
#define REG_CONTROL0_TM 0x01
// datasheet says 50ms min for refill
#define MIN_DELAY_SET_RESET 50
AP_Compass_Backend *AP_Compass_MMC3416::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_MMC3416 *sensor = new AP_Compass_MMC3416(compass, std::move(dev), force_external, rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_MMC3416::AP_Compass_MMC3416(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
bool _force_external,
enum Rotation _rotation)
: AP_Compass_Backend(compass)
, dev(std::move(_dev))
, force_external(_force_external)
, rotation(_rotation)
{
}
bool AP_Compass_MMC3416::init()
{
if (!dev->get_semaphore()->take(0)) {
return false;
}
dev->set_retries(10);
uint8_t whoami;
if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) ||
whoami != 0x06) {
// not a MMC3416
dev->get_semaphore()->give();
return false;
}
// reset sensor
dev->write_register(REG_CONTROL1, 0x80);
hal.scheduler->delay(10);
dev->write_register(REG_CONTROL0, 0x00); // single shot
dev->write_register(REG_CONTROL1, 0x00); // 16 bit, 7.92ms
dev->get_semaphore()->give();
/* register the compass instance in the frontend */
compass_instance = register_compass();
printf("Found a MMC3416 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_device_type(DEVTYPE_MMC3416);
set_dev_id(compass_instance, dev->get_bus_id());
dev->set_retries(1);
// call timer() at 100Hz
dev->register_periodic_callback(10000,
FUNCTOR_BIND_MEMBER(&AP_Compass_MMC3416::timer, void));
// wait 250ms for the compass to make it's initial readings
hal.scheduler->delay(250);
return true;
}
void AP_Compass_MMC3416::timer()
{
const uint16_t measure_count_limit = 50;
const uint16_t zero_offset = 32768; // 16 bit mode
const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
const float counts_to_milliGauss = 1.0e3f / sensitivity;
uint32_t now = AP_HAL::millis();
if (now - last_sample_ms > 500) {
// seems to be stuck or on first sample, reset state machine
state = STATE_REFILL1;
last_sample_ms = now;
}
/*
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) {
case STATE_REFILL1:
if (dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
state = STATE_REFILL1_WAIT;
refill_start_ms = AP_HAL::millis();
}
break;
case STATE_REFILL1_WAIT: {
uint8_t status;
if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET &&
dev->read_registers(REG_STATUS, &status, 1) &&
(status & 0x02) == 0) {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET) ||
!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
} else {
state = STATE_MEASURE_WAIT1;
}
}
break;
}
case STATE_MEASURE_WAIT1: {
uint8_t status;
if (dev->read_registers(REG_STATUS, &status, 1) && (status & 1)) {
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data0[0], 6)) {
state = STATE_REFILL1;
break;
}
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
state = STATE_REFILL1;
} else {
state = STATE_REFILL2_WAIT;
refill_start_ms = AP_HAL::millis();
}
}
break;
}
case STATE_REFILL2_WAIT: {
uint8_t status;
if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET &&
dev->read_registers(REG_STATUS, &status, 1) &&
(status & 0x02) == 0) {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET) ||
!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
} else {
state = STATE_MEASURE_WAIT2;
}
}
break;
}
case STATE_MEASURE_WAIT2: {
uint8_t status;
if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
break;
}
uint16_t data1[3];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
state = STATE_REFILL1;
break;
}
Vector3f field;
/*
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);
field = (f1 - f2) * (counts_to_milliGauss / 2);
Vector3f new_offset = (f1 + f2) * (counts_to_milliGauss / 2);
if (!have_initial_offset) {
offset = new_offset;
have_initial_offset = true;
} else {
// low pass changes to the offset
offset = offset * 0.95 + new_offset * 0.05;
}
#if 0
DataFlash_Class::instance()->Log_Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff",
AP_HAL::micros64(),
(double)new_offset.x,
(double)new_offset.y,
(double)new_offset.z,
(double)offset.x,
(double)offset.y,
(double)offset.z);
printf("F(%.1f %.1f %.1f) O(%.1f %.1f %.1f)\n",
field.x, field.y, field.z,
offset.x, offset.y, offset.z);
#endif
accumulate_field(field);
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
state = STATE_REFILL1;
} else {
state = STATE_MEASURE_WAIT3;
}
break;
}
case STATE_MEASURE_WAIT3: {
uint8_t status;
if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
break;
}
uint16_t data1[3];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
state = STATE_REFILL1;
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_field(field);
// we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles
if (measure_count++ >= measure_count_limit) {
measure_count = 0;
state = STATE_REFILL1;
} else {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
}
}
break;
}
}
}
/*
accumulate a field
*/
void AP_Compass_MMC3416::accumulate_field(Vector3f &field)
{
#if 0
DataFlash_Class::instance()->Log_Write("MMC", "TimeUS,X,Y,Z", "Qfff",
AP_HAL::micros64(),
(double)field.x,
(double)field.y,
(double)field.z);
#endif
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, compass_instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, AP_HAL::micros(), compass_instance);
/* correct raw_field for known errors */
correct_field(field, compass_instance);
if (_sem->take(0)) {
accum += field;
accum_count++;
_sem->give();
}
last_sample_ms = AP_HAL::millis();
}
void AP_Compass_MMC3416::read()
{
if (!_sem->take(0)) {
return;
}
if (accum_count == 0) {
_sem->give();
return;
}
accum /= accum_count;
publish_filtered_field(accum, compass_instance);
accum.zero();
accum_count = 0;
_sem->give();
}

77
libraries/AP_Compass/AP_Compass_MMC3416.h Normal file
View File

@ -0,0 +1,77 @@
/*
* 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_MMC3416_I2C_ADDR
# define HAL_COMPASS_MMC3416_I2C_ADDR 0x30
#endif
class AP_Compass_MMC3416 : public AP_Compass_Backend
{
public:
static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external = false,
enum Rotation rotation = ROTATION_NONE);
void read() override;
static constexpr const char *name = "MMC3416";
private:
AP_Compass_MMC3416(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation);
AP_HAL::OwnPtr<AP_HAL::Device> dev;
enum {
STATE_REFILL1,
STATE_REFILL1_WAIT,
STATE_MEASURE_WAIT1,
STATE_REFILL2_WAIT,
STATE_MEASURE_WAIT2,
STATE_MEASURE_WAIT3,
} state;
/**
* Device periodic callback to read data from the sensor.
*/
bool init();
void timer();
void accumulate_field(Vector3f &field);
uint8_t compass_instance;
Vector3f accum;
uint16_t accum_count;
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;
};

231
libraries/AP_Compass/AP_Compass_QMC5883L.cpp Normal file
View File

@ -0,0 +1,231 @@
/*
* Copyright (C) 2016 Emlid Ltd. All rights reserved.
*
* 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 RadioLink LjWang, Jun 2017
* GPS compass module See<http://www.radiolink.com>
*/
#include "AP_Compass_QMC5883L.h"
#include <stdio.h>
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
#define QMC5883L_REG_CONF1 0x09
#define QMC5883L_REG_CONF2 0x0A
// data output rates for 5883L
#define QMC5883L_ODR_10HZ (0x00 << 2)
#define QMC5883L_ODR_50HZ (0x01 << 2)
#define QMC5883L_ODR_100HZ (0x02 << 2)
#define QMC5883L_ODR_200HZ (0x03 << 2)
// Sensor operation modes
#define QMC5883L_MODE_STANDBY 0x00
#define QMC5883L_MODE_CONTINUOUS 0x01
#define QMC5883L_RNG_2G (0x00 << 4)
#define QMC5883L_RNG_8G (0x01 << 4)
#define QMC5883L_OSR_512 (0x00 << 6)
#define QMC5883L_OSR_256 (0x01 << 6)
#define QMC5883L_OSR_128 (0x10 << 6)
#define QMC5883L_OSR_64 (0x11 << 6)
#define QMC5883L_RST 0x80
#define QMC5883L_REG_DATA_OUTPUT_X 0x00
#define QMC5883L_REG_STATUS 0x06
#define QMC5883L_REG_ID 0x0D
#define QMC5883_ID_VAL 0xFF
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),force_external,rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_QMC5883L::AP_Compass_QMC5883L(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
, _rotation(rotation)
, _force_external(force_external)
{
}
bool AP_Compass_QMC5883L::init()
{
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
//must reset first
_dev->write_register(QMC5883L_REG_CONF2,QMC5883L_RST);
_dev->set_retries(10);
uint8_t whoami;
if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)||
whoami != QMC5883_ID_VAL){
// not an QMC5883L
goto fail;
}
if (!_dev->write_register(0x0B, 0x01)||
!_dev->write_register(0x20, 0x40)||
!_dev->write_register(0x21, 0x01)||
!_dev->write_register(QMC5883L_REG_CONF1,
QMC5883L_MODE_CONTINUOUS|
QMC5883L_ODR_100HZ|
QMC5883L_OSR_512|
QMC5883L_RNG_8G)) {
goto fail;
}
// lower retries for run
_dev->set_retries(3);
_dev->get_semaphore()->give();
_instance = register_compass();
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);
_dev->set_device_type(DEVTYPE_QMC5883L);
set_dev_id(_instance, _dev->get_bus_id());
if (_force_external) {
set_external(_instance, true);
}
//Enable 100HZ
_dev->register_periodic_callback(10000,
FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883L::timer, void));
return true;
fail:
_dev->get_semaphore()->give();
return false;
}
void AP_Compass_QMC5883L::timer()
{
struct PACKED {
le16_t rx;
le16_t ry;
le16_t rz;
} buffer;
const float range_scale = 1000.0f / 3000.0f;
uint8_t status;
if(!_dev->read_registers(QMC5883L_REG_STATUS,&status,1)){
return;
}
//new data is ready
if (!(status & 0x04)) {
return;
}
if(!_dev->read_registers(QMC5883L_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))){
return ;
}
uint32_t now = AP_HAL::micros();
auto x = -static_cast<int16_t>(le16toh(buffer.rx));
auto y = static_cast<int16_t>(le16toh(buffer.ry));
auto z = -static_cast<int16_t>(le16toh(buffer.rz));
#if 0
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 };
// rotate to the desired orientation
if (is_external(_instance)) {
field.rotate(ROTATION_YAW_90);
}
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, _instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, now, _instance);
/* correct raw_field for known errors */
correct_field(field, _instance);
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_accum += field;
_accum_count++;
if(_accum_count == 20){
_accum.x /= 2;
_accum.y /= 2;
_accum.z /= 2;
_accum_count = 10;
}
_sem->give();
}
}
void AP_Compass_QMC5883L::read()
{
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
_sem->give();
return;
}
Vector3f field(_accum);
field /= _accum_count;
publish_filtered_field(field, _instance);
_accum.zero();
_accum_count = 0;
_sem->give();
}

60
libraries/AP_Compass/AP_Compass_QMC5883L.h Normal file
View File

@ -0,0 +1,60 @@
/*
* Copyright (C) 2016 Emlid Ltd. All rights reserved.
*
* 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_QMC5883L_I2C_ADDR
#define HAL_COMPASS_QMC5883L_I2C_ADDR 0x0D
#endif
class AP_Compass_QMC5883L : public AP_Compass_Backend
{
public:
static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation = ROTATION_NONE);
void read() override;
static constexpr const char *name = "QMC5883L";
private:
AP_Compass_QMC5883L(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation);
void timer();
bool init();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
Vector3f _accum = Vector3f();
uint32_t _accum_count = 0;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external:1;
};

113
libraries/AP_Compass/AP_Compass_SITL.cpp Normal file
View File

@ -0,0 +1,113 @@
#include "AP_Compass_SITL.h"
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
extern const AP_HAL::HAL& hal;
AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
_has_sample(false),
AP_Compass_Backend(compass)
{
_sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
if (_sitl != nullptr) {
_compass._setup_earth_field();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
_compass_instance[i] = register_compass();
}
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
}
}
void AP_Compass_SITL::_timer()
{
// TODO: Refactor delay buffer with AP_Baro_SITL.
// Sampled at 100Hz
uint32_t now = AP_HAL::millis();
if ((now - _last_sample_time) < 10) {
return;
}
_last_sample_time = now;
// calculate sensor noise and add to 'truth' field in body frame
// units are milli-Gauss
Vector3f noise = rand_vec3f() * _sitl->mag_noise;
Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
// add delay
uint32_t best_time_delta = 1000; // initialise large time representing buffer entry closest to current time - delay.
uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay.
// storing data from sensor to buffer
if (now - last_store_time >= 10) { // store data every 10 ms.
last_store_time = now;
if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer
store_index = 0;
}
buffer[store_index].data = new_mag_data; // add data to current index
buffer[store_index].time = last_store_time; // add time to current index
store_index = store_index + 1; // increment index
}
// return delayed measurement
uint32_t delayed_time = now - _sitl->mag_delay; // get time corresponding to delay
// find data corresponding to delayed time in buffer
for (uint8_t i=0; i<=buffer_length-1; i++) {
// find difference between delayed time and time stamp in buffer
uint32_t time_delta = abs((int32_t)(delayed_time - buffer[i].time));
// if this difference is smaller than last delta, store this time
if (time_delta < best_time_delta) {
best_index= i;
best_time_delta = time_delta;
}
}
if (best_time_delta < 1000) { // only output stored state if < 1 sec retrieval error
new_mag_data = buffer[best_index].data;
}
new_mag_data -= _sitl->mag_ofs.get();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
rotate_field(new_mag_data, _compass_instance[i]);
publish_raw_field(new_mag_data, AP_HAL::micros(), _compass_instance[i]);
correct_field(new_mag_data, _compass_instance[i]);
}
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
_mag_accum += new_mag_data;
_accum_count++;
if (_accum_count == 10) {
_mag_accum /= 2;
_accum_count = 5;
_has_sample = true;
}
_sem->give();
}
void AP_Compass_SITL::read()
{
if (_sem->take_nonblocking()) {
if (!_has_sample) {
_sem->give();
return;
}
Vector3f field(_mag_accum);
field /= _accum_count;
_mag_accum.zero();
_accum_count = 0;
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
publish_filtered_field(field, _compass_instance[i]);
}
_has_sample = false;
_sem->give();
}
}
#endif

43
libraries/AP_Compass/AP_Compass_SITL.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#include <AP_Math/vectorN.h>
#include <AP_Math/AP_Math.h>
#include <AP_Declination/AP_Declination.h>
#define SITL_NUM_COMPASSES 2
class AP_Compass_SITL : public AP_Compass_Backend {
public:
AP_Compass_SITL(Compass &);
void read(void);
private:
uint8_t _compass_instance[SITL_NUM_COMPASSES];
SITL::SITL *_sitl;
// delay buffer variables
struct readings_compass {
uint32_t time;
Vector3f data;
};
uint8_t store_index;
uint32_t last_store_time;
static const uint8_t buffer_length = 50;
VectorN<readings_compass,buffer_length> buffer;
void _timer();
bool _has_sample;
uint32_t _last_sample_time;
Vector3f _mag_accum;
uint32_t _accum_count;
};
#endif // CONFIG_HAL_BOARD