Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: add support for AK09916 connected over fourth IMU over I2C

Browse Source
This commit is contained in:
Siddharth Purohit 2019-03-05 17:52:03 +05:30 committed by Andrew Tridgell
parent b76ec8cc11
commit 6ff8f52957
3 changed files with 80 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass.cpp
View File

@ -622,14 +622,16 @@ void Compass::_probe_external_i2c_compasses(void)
// AK09916 on ICM20948
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90));
}
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
all_external, ROTATION_PITCH_180_YAW_90));
}
// lis3mdl on bus 0 with default address
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),

81
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -22,7 +22,8 @@
#include <utility>
#include <AP_Math/AP_Math.h>
#include <stdio.h>
#include <AP_InertialSensor/AP_InertialSensor_Invensense.h>
#include <AP_InertialSensor/AP_InertialSensor_Invensensev2.h>
#include <GCS_MAVLink/GCS.h>
#define REG_COMPANY_ID 0x00
#define REG_DEVICE_ID 0x01
@ -43,6 +44,8 @@
#define REG_ICM_PWR_MGMT_1 0x06
#define REG_ICM_INT_PIN_CFG 0x0f
#define ICM_WHOAMI_VAL 0xEA
#define AK09916_Device_ID 0x09
#define AK09916_MILLIGAUSS_SCALE 10.0f
@ -91,18 +94,76 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice>
}
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
if (!dev || !dev_icm) {
return nullptr;
}
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return nullptr;
}
AP_InertialSensor &ins = AP::ins();
/* Allow ICM20x48 to shortcut auxiliary bus and host bus */
ins.detect_backends();
uint8_t rval;
uint16_t whoami;
uint8_t retries = 5;
if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) ||
rval != ICM_WHOAMI_VAL) {
// not an ICM_WHOAMI
goto fail;
}
do {
// reset then bring sensor out of sleep mode
if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x80)) {
goto fail;
}
hal.scheduler->delay(10);
if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00)) {
goto fail;
}
hal.scheduler->delay(10);
// see if ICM20948 is sleeping
if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) {
goto fail;
}
if ((rval & 0x40) == 0) {
break;
}
} while (retries--);
if (rval & 0x40) {
// it didn't come out of sleep
goto fail;
}
// initially force i2c bypass off
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x00);
hal.scheduler->delay(1);
// now check if a AK09916 shows up on the bus. If it does then
// we have both a real AK09916 and a ICM20948 with an embedded
// AK09916. In that case we will fail the driver load and use
// the main AK09916 driver
if (dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) {
// a device is replying on the AK09916 I2C address, don't
// load the ICM20948
gcs().send_text(MAV_SEVERITY_INFO, "ICM20948: AK09916 bus conflict\n");
goto fail;
}
// now force bypass on
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02);
hal.scheduler->delay(1);
dev->get_semaphore()->give();
return probe(std::move(dev), force_external, rotation);
fail:
dev->get_semaphore()->give();
return nullptr;
}
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(uint8_t inv2_instance,
@ -130,32 +191,32 @@ bool AP_Compass_AK09916::init()
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("AK09916: Unable to get bus semaphore\n");
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Unable to get bus semaphore\n");
return false;
}
if (!_bus->configure()) {
hal.console->printf("AK09916: Could not configure the bus\n");
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not configure the bus\n");
goto fail;
}
if (!_reset()) {
hal.console->printf("AK09916: Reset Failed\n");
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Reset Failed\n");
goto fail;
}
if (!_check_id()) {
hal.console->printf("AK09916: Wrong id\n");
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Wrong id\n");
goto fail;
}
if (!_setup_mode()) {
hal.console->printf("AK09916: Could not setup mode\n");
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not setup mode\n");
goto fail;
}
if (!_bus->start_measurements()) {
hal.console->printf("AK09916: Could not start measurements\n");
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not start measurements\n");
goto fail;
}

6
libraries/AP_Compass/AP_Compass_AK09916.h
View File

@ -26,6 +26,11 @@
# define HAL_COMPASS_AK09916_I2C_ADDR 0x0C
#endif
#ifndef HAL_COMPASS_ICM20948_I2C_ADDR
# define HAL_COMPASS_ICM20948_I2C_ADDR 0x69
#endif
class AuxiliaryBus;
class AuxiliaryBusSlave;
class AP_InertialSensor;
@ -41,6 +46,7 @@ public:
/* Probe for AK09916 on auxiliary bus of ICM20948, connected through I2C */
static AP_Compass_Backend *probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
bool force_external,
enum Rotation rotation = ROTATION_NONE);