You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
359 lines
10 KiB
359 lines
10 KiB
/* |
|
This program 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 program 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/>. |
|
*/ |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_HAL/I2CDevice.h> |
|
#include <utility> |
|
|
|
#include <AP_Common/AP_Common.h> |
|
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_Math/AP_Math.h> |
|
#include <AP_BoardConfig/AP_BoardConfig.h> |
|
#include "AP_Baro_ICM20789.h" |
|
|
|
#include <utility> |
|
#include <stdio.h> |
|
|
|
#include <AP_Math/AP_Math.h> |
|
#include <AP_Logger/AP_Logger.h> |
|
|
|
#include <AP_InertialSensor/AP_InertialSensor_Invensense_registers.h> |
|
|
|
extern const AP_HAL::HAL &hal; |
|
|
|
/* |
|
CMD_READ options. The draft datasheet doesn't specify, but it seems |
|
Mode_1 has a conversion interval of 2ms. Mode_3 has a conversion |
|
interval of 20ms. Both seem to produce equally as smooth results, so |
|
presumably Mode_3 is doing internal averaging |
|
*/ |
|
#define CMD_READ_PT_MODE_1 0x401A |
|
#define CMD_READ_PT_MODE_3 0x5059 |
|
#define CMD_READ_TP_MODE_1 0x609C |
|
#define CMD_READ_TP_MODE_3 0x70DF |
|
|
|
#define CONVERSION_INTERVAL_MODE_1 2000 |
|
#define CONVERSION_INTERVAL_MODE_3 20000 |
|
|
|
// setup for Mode_3 |
|
#define CMD_READ_PT CMD_READ_PT_MODE_3 |
|
#define CONVERSION_INTERVAL CONVERSION_INTERVAL_MODE_3 |
|
|
|
#define CMD_SOFT_RESET 0x805D |
|
#define CMD_READ_ID 0xEFC8 |
|
|
|
#define BARO_ICM20789_DEBUG 0 |
|
|
|
#if BARO_ICM20789_DEBUG |
|
#define debug(fmt, args...) hal.console->printf(fmt, ##args) |
|
#else |
|
#define debug(fmt, args...) |
|
#endif |
|
|
|
/* |
|
constructor |
|
*/ |
|
AP_Baro_ICM20789::AP_Baro_ICM20789(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev, AP_HAL::OwnPtr<AP_HAL::Device> _dev_imu) |
|
: AP_Baro_Backend(baro) |
|
, dev(std::move(_dev)) |
|
, dev_imu(std::move(_dev_imu)) |
|
{ |
|
} |
|
|
|
AP_Baro_Backend *AP_Baro_ICM20789::probe(AP_Baro &baro, |
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, |
|
AP_HAL::OwnPtr<AP_HAL::Device> dev_imu) |
|
{ |
|
debug("Probing for ICM20789 baro\n"); |
|
if (!dev || !dev_imu) { |
|
return nullptr; |
|
} |
|
AP_Baro_ICM20789 *sensor = new AP_Baro_ICM20789(baro, std::move(dev), std::move(dev_imu)); |
|
if (!sensor || !sensor->init()) { |
|
delete sensor; |
|
return nullptr; |
|
} |
|
return sensor; |
|
} |
|
|
|
|
|
/* |
|
setup ICM20789 to enable barometer, assuming IMU is on SPI and baro is on I2C |
|
*/ |
|
bool AP_Baro_ICM20789::imu_spi_init(void) |
|
{ |
|
if (!dev_imu->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { |
|
AP_HAL::panic("PANIC: AP_Baro_ICM20789: failed to take serial semaphore ICM"); |
|
} |
|
|
|
dev_imu->set_read_flag(0x80); |
|
|
|
dev_imu->set_speed(AP_HAL::Device::SPEED_LOW); |
|
uint8_t whoami = 0; |
|
uint8_t v; |
|
|
|
dev_imu->read_registers(MPUREG_USER_CTRL, &v, 1); |
|
dev_imu->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO); |
|
|
|
hal.scheduler->delay(1); |
|
dev_imu->write_register(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); |
|
dev_imu->write_register(MPUREG_PWR_MGMT_1, |
|
BIT_PWR_MGMT_1_SLEEP | BIT_PWR_MGMT_1_CLK_XGYRO); |
|
|
|
hal.scheduler->delay(1); |
|
dev_imu->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO); |
|
|
|
hal.scheduler->delay(1); |
|
dev_imu->write_register(MPUREG_FIFO_EN, 0x00); |
|
dev_imu->write_register(MPUREG_PWR_MGMT_1, |
|
BIT_PWR_MGMT_1_SLEEP | BIT_PWR_MGMT_1_CLK_XGYRO); |
|
|
|
dev_imu->read_registers(MPUREG_WHOAMI, &whoami, 1); |
|
|
|
// wait for sensor to settle |
|
hal.scheduler->delay(100); |
|
|
|
dev_imu->read_registers(MPUREG_WHOAMI, &whoami, 1); |
|
|
|
dev_imu->write_register(MPUREG_INT_PIN_CFG, 0x00); |
|
dev_imu->write_register(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); |
|
|
|
dev_imu->get_semaphore()->give(); |
|
|
|
return true; |
|
} |
|
|
|
/* |
|
setup ICM20789 to enable barometer, assuming both IMU and baro on the same i2c bus |
|
*/ |
|
bool AP_Baro_ICM20789::imu_i2c_init(void) |
|
{ |
|
// as the baro device is already locked we need to re-use it, |
|
// changing its address to match the IMU address |
|
uint8_t old_address = dev->get_bus_address(); |
|
dev->set_address(dev_imu->get_bus_address()); |
|
|
|
dev->set_retries(4); |
|
|
|
uint8_t whoami=0; |
|
dev->read_registers(MPUREG_WHOAMI, &whoami, 1); |
|
debug("ICM20789: whoami 0x%02x old_address=%02x\n", whoami, old_address); |
|
|
|
dev->write_register(MPUREG_FIFO_EN, 0x00); |
|
dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO); |
|
|
|
// wait for sensor to settle |
|
hal.scheduler->delay(10); |
|
|
|
dev->write_register(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); |
|
|
|
dev->set_address(old_address); |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Baro_ICM20789::init() |
|
{ |
|
if (!dev) { |
|
return false; |
|
} |
|
|
|
debug("Looking for 20789 baro\n"); |
|
|
|
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { |
|
AP_HAL::panic("PANIC: AP_Baro_ICM20789: failed to take serial semaphore for init"); |
|
} |
|
|
|
debug("Setting up IMU\n"); |
|
if (dev_imu->bus_type() != AP_HAL::Device::BUS_TYPE_I2C) { |
|
if (!imu_spi_init()) { |
|
debug("ICM20789: failed to initialise IMU SPI device\n"); |
|
return false; |
|
} |
|
} else if (!imu_i2c_init()) { |
|
debug("ICM20789: failed to initialise IMU I2C device\n"); |
|
return false; |
|
} |
|
|
|
if (!send_cmd16(CMD_SOFT_RESET)) { |
|
debug("ICM20789: reset failed\n"); |
|
goto failed; |
|
} |
|
|
|
// wait for sensor to settle |
|
hal.scheduler->delay(10); |
|
|
|
if (!read_calibration_data()) { |
|
debug("ICM20789: read_calibration_data failed\n"); |
|
goto failed; |
|
} |
|
|
|
// start a reading |
|
if (!send_cmd16(CMD_READ_PT)) { |
|
debug("ICM20789: start read failed\n"); |
|
goto failed; |
|
} |
|
|
|
dev->set_retries(0); |
|
|
|
instance = _frontend.register_sensor(); |
|
|
|
dev->get_semaphore()->give(); |
|
|
|
debug("ICM20789: startup OK\n"); |
|
|
|
// use 10ms to ensure we don't lose samples, with max lag of 10ms |
|
dev->register_periodic_callback(CONVERSION_INTERVAL/2, FUNCTOR_BIND_MEMBER(&AP_Baro_ICM20789::timer, void)); |
|
|
|
return true; |
|
|
|
failed: |
|
dev->get_semaphore()->give(); |
|
return false; |
|
} |
|
|
|
bool AP_Baro_ICM20789::send_cmd16(uint16_t cmd) |
|
{ |
|
uint8_t cmd_b[2] = { uint8_t(cmd >> 8), uint8_t(cmd & 0xFF) }; |
|
return dev->transfer(cmd_b, 2, nullptr, 0); |
|
} |
|
|
|
bool AP_Baro_ICM20789::read_calibration_data(void) |
|
{ |
|
// setup for OTP read |
|
const uint8_t cmd[5] = { 0xC5, 0x95, 0x00, 0x66, 0x9C }; |
|
if (!dev->transfer(cmd, sizeof(cmd), nullptr, 0)) { |
|
debug("ICM20789: read cal1 failed\n"); |
|
return false; |
|
} |
|
for (uint8_t i=0; i<4; i++) { |
|
if (!send_cmd16(0xC7F7)) { |
|
debug("ICM20789: read cal2[%u] failed\n", i); |
|
return false; |
|
} |
|
uint8_t d[3]; |
|
if (!dev->transfer(nullptr, 0, d, sizeof(d))) { |
|
debug("ICM20789: read cal3[%u] failed\n", i); |
|
return false; |
|
} |
|
sensor_constants[i] = int16_t((d[0]<<8) | d[1]); |
|
debug("sensor_constants[%u]=%d\n", i, sensor_constants[i]); |
|
} |
|
return true; |
|
} |
|
|
|
void AP_Baro_ICM20789::calculate_conversion_constants(const float p_Pa[3], const float p_LUT[3], |
|
float &A, float &B, float &C) |
|
{ |
|
C = (p_LUT[0] * p_LUT[1] * (p_Pa[0] - p_Pa[1]) + |
|
p_LUT[1] * p_LUT[2] * (p_Pa[1] - p_Pa[2]) + |
|
p_LUT[2] * p_LUT[0] * (p_Pa[2] - p_Pa[0])) / |
|
(p_LUT[2] * (p_Pa[0] - p_Pa[1]) + |
|
p_LUT[0] * (p_Pa[1] - p_Pa[2]) + |
|
p_LUT[1] * (p_Pa[2] - p_Pa[0])); |
|
A = (p_Pa[0] * p_LUT[0] - p_Pa[1] * p_LUT[1] - (p_Pa[1] - p_Pa[0]) * C) / (p_LUT[0] - p_LUT[1]); |
|
B = (p_Pa[0] - A) * (p_LUT[0] + C); |
|
} |
|
|
|
/* |
|
Convert an output from a calibrated sensor to a pressure in Pa. |
|
Arguments: |
|
p_LSB -- Raw pressure data from sensor |
|
T_LSB -- Raw temperature data from sensor |
|
*/ |
|
float AP_Baro_ICM20789::get_pressure(uint32_t p_LSB, uint32_t T_LSB) |
|
{ |
|
float t = T_LSB - 32768.0; |
|
float s[3]; |
|
s[0] = LUT_lower + float(sensor_constants[0] * t * t) * quadr_factor; |
|
s[1] = offst_factor * sensor_constants[3] + float(sensor_constants[1] * t * t) * quadr_factor; |
|
s[2] = LUT_upper + float(sensor_constants[2] * t * t) * quadr_factor; |
|
float A, B, C; |
|
calculate_conversion_constants(p_Pa_calib, s, A, B, C); |
|
return A + B / (C + p_LSB); |
|
} |
|
|
|
|
|
#if BARO_ICM20789_DEBUG |
|
static struct { |
|
uint32_t Praw, Traw; |
|
float T, P; |
|
} dd; |
|
#endif |
|
|
|
void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw) |
|
{ |
|
// temperature is easy |
|
float T = -45 + (175.0 / (1U<<16)) * Traw; |
|
|
|
// pressure involves a few more calculations |
|
float P = get_pressure(Praw, Traw); |
|
|
|
if (!pressure_ok(P)) { |
|
return; |
|
} |
|
|
|
WITH_SEMAPHORE(_sem); |
|
|
|
#if BARO_ICM20789_DEBUG |
|
dd.Praw = Praw; |
|
dd.Traw = Traw; |
|
dd.P = P; |
|
dd.T = T; |
|
#endif |
|
|
|
accum.psum += P; |
|
accum.tsum += T; |
|
accum.count++; |
|
} |
|
|
|
void AP_Baro_ICM20789::timer(void) |
|
{ |
|
uint8_t d[9] {}; |
|
if (dev->transfer(nullptr, 0, d, sizeof(d))) { |
|
// ignore CRC bytes for now |
|
uint32_t Praw = (uint32_t(d[0]) << 16) | (uint32_t(d[1]) << 8) | d[3]; |
|
uint32_t Traw = (uint32_t(d[6]) << 8) | d[7]; |
|
|
|
convert_data(Praw, Traw); |
|
send_cmd16(CMD_READ_PT); |
|
last_measure_us = AP_HAL::micros(); |
|
} else { |
|
if (AP_HAL::micros() - last_measure_us > CONVERSION_INTERVAL*3) { |
|
// lost a sample |
|
send_cmd16(CMD_READ_PT); |
|
last_measure_us = AP_HAL::micros(); |
|
} |
|
} |
|
} |
|
|
|
void AP_Baro_ICM20789::update() |
|
{ |
|
#if BARO_ICM20789_DEBUG |
|
// useful for debugging |
|
AP::logger().Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff", |
|
AP_HAL::micros64(), |
|
dd.Traw, dd.Praw, dd.P, dd.T); |
|
#endif |
|
|
|
WITH_SEMAPHORE(_sem); |
|
|
|
if (accum.count > 0) { |
|
_copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count); |
|
accum.psum = accum.tsum = 0; |
|
accum.count = 0; |
|
} |
|
} |
|
|
|
|