From 6ff8f52957cc966891680a9175fe80175e01c68d Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Tue, 5 Mar 2019 17:52:03 +0530 Subject: [PATCH] AP_Compass: add support for AK09916 connected over fourth IMU over I2C --- libraries/AP_Compass/AP_Compass.cpp | 4 +- libraries/AP_Compass/AP_Compass_AK09916.cpp | 81 ++++++++++++++++++--- libraries/AP_Compass/AP_Compass_AK09916.h | 6 ++ 3 files changed, 80 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index cbe0630e48..6f2185d3b7 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -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), diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 0f6f94a4f8..8457a00e16 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -22,7 +22,8 @@ #include #include #include -#include +#include +#include #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_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr dev, + AP_HAL::OwnPtr 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; } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index b91805039f..01d67d2f30 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -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 dev, + AP_HAL::OwnPtr dev_icm, bool force_external, enum Rotation rotation = ROTATION_NONE);