From 2822c3030711136409125bdacf7d105d2e16193f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E5=AD=9F=E6=99=93?= Date: Fri, 24 Jun 2022 06:17:22 +0000 Subject: [PATCH] AP_Baro: added ICP201XX support --- libraries/AP_Baro/AP_Baro.cpp | 1 + libraries/AP_Baro/AP_Baro_Backend.h | 1 + libraries/AP_Baro/AP_Baro_ICP201XX.cpp | 496 +++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_ICP201XX.h | 85 +++++ 4 files changed, 583 insertions(+) create mode 100755 libraries/AP_Baro/AP_Baro_ICP201XX.cpp create mode 100755 libraries/AP_Baro/AP_Baro_ICP201XX.h diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index b87a5f85b4..47d366bd70 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -47,6 +47,7 @@ #include "AP_Baro_MSP.h" #include "AP_Baro_ExternalAHRS.h" #include "AP_Baro_ICP101XX.h" +#include "AP_Baro_ICP201XX.h" #include #include diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 3fcba57299..d7a402059f 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -57,6 +57,7 @@ public: DEVTYPE_BARO_UAVCAN = 0x0D, DEVTYPE_BARO_MSP = 0x0E, DEVTYPE_BARO_ICP101XX = 0x0F, + DEVTYPE_BARO_ICP201XX = 0x10, }; protected: diff --git a/libraries/AP_Baro/AP_Baro_ICP201XX.cpp b/libraries/AP_Baro/AP_Baro_ICP201XX.cpp new file mode 100755 index 0000000000..b7b5c6ec89 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_ICP201XX.cpp @@ -0,0 +1,496 @@ +/* + 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 . + */ + +#include "AP_Baro_ICP201XX.h" + +#if AP_BARO_ICP201XX_ENABLED + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +extern const AP_HAL::HAL &hal; + +#define ICP201XX_ID 0x63 + +#define CONVERSION_INTERVAL 25000 + +#define REG_EMPTY 0x00 +#define REG_TRIM1_MSB 0x05 +#define REG_TRIM2_LSB 0x06 +#define REG_TRIM2_MSB 0x07 +#define REG_DEVICE_ID 0x0C +#define REG_OTP_MTP_OTP_CFG1 0xAC +#define REG_OTP_MTP_MR_LSB 0xAD +#define REG_OTP_MTP_MR_MSB 0xAE +#define REG_OTP_MTP_MRA_LSB 0xAF +#define REG_OTP_MTP_MRA_MSB 0xB0 +#define REG_OTP_MTP_MRB_LSB 0xB1 +#define REG_OTP_MTP_MRB_MSB 0xB2 +#define REG_OTP_MTP_OTP_ADDR 0xB5 +#define REG_OTP_MTP_OTP_CMD 0xB6 +#define REG_OTP_MTP_RD_DATA 0xB8 +#define REG_OTP_MTP_OTP_STATUS 0xB9 +#define REG_OTP_DEBUG2 0xBC +#define REG_MASTER_LOCK 0xBE +#define REG_OTP_MTP_OTP_STATUS2 0xBF +#define REG_MODE_SELECT 0xC0 +#define REG_INTERRUPT_STATUS 0xC1 +#define REG_INTERRUPT_MASK 0xC2 +#define REG_FIFO_CONFIG 0xC3 +#define REG_FIFO_FILL 0xC4 +#define REG_SPI_MODE 0xC5 +#define REG_PRESS_ABS_LSB 0xC7 +#define REG_PRESS_ABS_MSB 0xC8 +#define REG_PRESS_DELTA_LSB 0xC9 +#define REG_PRESS_DELTA_MSB 0xCA +#define REG_DEVICE_STATUS 0xCD +#define REG_I3C_INFO 0xCE +#define REG_VERSION 0xD3 +#define REG_FIFO_BASE 0xFA + +/* + constructor + */ +AP_Baro_ICP201XX::AP_Baro_ICP201XX(AP_Baro &baro, AP_HAL::OwnPtr _dev) + : AP_Baro_Backend(baro) + , dev(std::move(_dev)) +{ +} + +AP_Baro_Backend *AP_Baro_ICP201XX::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + AP_Baro_ICP201XX *sensor = new AP_Baro_ICP201XX(baro, std::move(dev)); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_ICP201XX::init() +{ + if (!dev) { + return false; + } + + dev->get_semaphore()->take_blocking(); + + uint8_t id = 0xFF; + uint8_t ver = 0xFF; + read_reg(REG_DEVICE_ID, &id); + read_reg(REG_VERSION, &ver); + + if (id != ICP201XX_ID) { + goto failed; + } + + if (ver != 0x00 && ver != 0xB2) { + goto failed; + } + + hal.scheduler->delay(10); + + soft_reset(); + + if (!boot_sequence()) { + goto failed; + } + + if (!configure()) { + goto failed; + } + + wait_read(); + + dev->set_retries(0); + + instance = _frontend.register_sensor(); + + dev->set_device_type(DEVTYPE_BARO_ICP201XX); + set_bus_id(instance, dev->get_bus_id()); + + dev->get_semaphore()->give(); + + dev->register_periodic_callback(CONVERSION_INTERVAL/2, FUNCTOR_BIND_MEMBER(&AP_Baro_ICP201XX::timer, void)); + return true; + + failed: + dev->get_semaphore()->give(); + return false; +} + + +void AP_Baro_ICP201XX::dummy_reg() +{ + do { + uint8_t reg = REG_EMPTY; + uint8_t val = 0; + dev->transfer(®, 1, &val, 1); + } while (0); +} + +bool AP_Baro_ICP201XX::read_reg(uint8_t reg, uint8_t *buf, uint8_t len) +{ + bool ret; + ret = dev->transfer(®, 1, buf, len); + dummy_reg(); + return ret; +} + +bool AP_Baro_ICP201XX::read_reg(uint8_t reg, uint8_t *val) +{ + return read_reg(reg, val, 1); +} + +bool AP_Baro_ICP201XX::write_reg(uint8_t reg, uint8_t val) +{ + bool ret; + uint8_t data[2] = { reg, val }; + ret = dev->transfer(data, sizeof(data), nullptr, 0); + dummy_reg(); + return ret; +} + +void AP_Baro_ICP201XX::soft_reset() +{ + /* Stop the measurement */ + mode_select(0x00); + + hal.scheduler->delay(2); + + /* Flush FIFO */ + flush_fifo(); + + /* Mask all interrupts */ + write_reg(REG_FIFO_CONFIG, 0x00); + write_reg(REG_INTERRUPT_MASK, 0xFF); +} + +bool AP_Baro_ICP201XX::mode_select(uint8_t mode) +{ + uint8_t mode_sync_status = 0; + + do { + read_reg(REG_DEVICE_STATUS, &mode_sync_status, 1); + + if (mode_sync_status & 0x01) { + break; + } + + hal.scheduler->delay(1); + } while (1); + + return write_reg(REG_MODE_SELECT, mode); +} + +bool AP_Baro_ICP201XX::read_otp_data(uint8_t addr, uint8_t cmd, uint8_t *val) +{ + uint8_t otp_status = 0xFF; + + /* Write the address content and read command */ + if (!write_reg(REG_OTP_MTP_OTP_ADDR, addr)) { + return false; + } + + if (!write_reg(REG_OTP_MTP_OTP_CMD, cmd)) { + return false; + } + + /* Wait for the OTP read to finish Monitor otp_status */ + do { + read_reg(REG_OTP_MTP_OTP_STATUS, &otp_status); + + if (otp_status == 0) { + break; + } + + hal.scheduler->delay_microseconds(1); + } while (1); + + /* Read the data from register */ + if (!read_reg(REG_OTP_MTP_RD_DATA, val)) { + return false; + } + + return true; +} + +bool AP_Baro_ICP201XX::get_sensor_data(float *pressure, float *temperature) +{ + uint8_t fifo_data[96] {0}; + uint8_t fifo_packets = 0; + int32_t data_temp = 0; + int32_t data_press = 0; + *pressure = 0; + *temperature = 0; + + if (read_reg(REG_FIFO_FILL, &fifo_packets)) { + fifo_packets = (uint8_t)(fifo_packets & 0x1F); + if (fifo_packets > 16) { + flush_fifo(); + return false; + } + if (fifo_packets > 0 && fifo_packets <= 16 && read_reg(REG_FIFO_BASE, fifo_data, fifo_packets * 2 * 3)) { + uint8_t offset = 0; + + for (uint8_t i = 0; i < fifo_packets; i++) { + data_press = (int32_t)(((fifo_data[offset + 2] & 0x0f) << 16) | (fifo_data[offset + 1] << 8) | fifo_data[offset]); + if (data_press & 0x080000) { + data_press |= 0xFFF00000; + } + /* P = (POUT/2^17)*40kPa + 70kPa */ + *pressure += ((float)(data_press) * 40 / 131072) + 70; + offset += 3; + + data_temp = (int32_t)(((fifo_data[offset + 2] & 0x0f) << 16) | (fifo_data[offset + 1] << 8) | fifo_data[offset]); + if (data_temp & 0x080000) { + data_temp |= 0xFFF00000; + } + /* T = (TOUT/2^18)*65C + 25C */ + *temperature += ((float)(data_temp) * 65 / 262144) + 25; + offset += 3; + } + + *pressure = *pressure * 1000 / fifo_packets; + *temperature = *temperature / fifo_packets; + return true; + } + } + + return false; +} + +bool AP_Baro_ICP201XX::boot_sequence() +{ + uint8_t reg_value = 0; + uint8_t offset = 0, gain = 0, Hfosc = 0; + uint8_t version = 0; + uint8_t bootup_status = 0; + int ret = 1; + + /* read version register */ + if (!read_reg(REG_VERSION, &version)) { + return false; + } + + if (version == 0xB2) { + /* B2 version Asic is detected. Boot up sequence is not required for B2 Asic, so returning */ + return true; + } + + /* Read boot up status and avoid re running boot up sequence if it is already done */ + if (!read_reg(REG_OTP_MTP_OTP_STATUS2, &bootup_status)) { + return false; + } + + if (bootup_status & 0x01) { + /* Boot up sequence is already done, not required to repeat boot up sequence */ + return true; + } + + /* Bring the ASIC in power mode to activate the OTP power domain and get access to the main registers */ + mode_select(0x04); + hal.scheduler->delay(4); + + /* Unlock the main registers */ + write_reg(REG_MASTER_LOCK, 0x1F); + + /* Enable the OTP and the write switch */ + read_reg(REG_OTP_MTP_OTP_CFG1, ®_value); + reg_value |= 0x03; + write_reg(REG_OTP_MTP_OTP_CFG1, reg_value); + hal.scheduler->delay_microseconds(10); + + /* Toggle the OTP reset pin */ + read_reg(REG_OTP_DEBUG2, ®_value); + reg_value |= 1 << 7; + write_reg(REG_OTP_DEBUG2, reg_value); + hal.scheduler->delay_microseconds(10); + + read_reg(REG_OTP_DEBUG2, ®_value); + reg_value &= ~(1 << 7); + write_reg(REG_OTP_DEBUG2, reg_value); + hal.scheduler->delay_microseconds(10); + + /* Program redundant read */ + write_reg(REG_OTP_MTP_MRA_LSB, 0x04); + write_reg(REG_OTP_MTP_MRA_MSB, 0x04); + write_reg(REG_OTP_MTP_MRB_LSB, 0x21); + write_reg(REG_OTP_MTP_MRB_MSB, 0x20); + write_reg(REG_OTP_MTP_MR_LSB, 0x10); + write_reg(REG_OTP_MTP_MR_MSB, 0x80); + + /* Read the data from register */ + ret &= read_otp_data(0xF8, 0x10, &offset); + ret &= read_otp_data(0xF9, 0x10, &gain); + ret &= read_otp_data(0xFA, 0x10, &Hfosc); + hal.scheduler->delay_microseconds(10); + + /* Write OTP values to main registers */ + ret &= read_reg(REG_TRIM1_MSB, ®_value); + if (ret) { + reg_value = (reg_value & (~0x3F)) | (offset & 0x3F); + ret &= write_reg(REG_TRIM1_MSB, reg_value); + } + + ret &= read_reg(REG_TRIM2_MSB, ®_value); + if (ret) { + reg_value = (reg_value & (~0x70)) | ((gain & 0x07) << 4); + ret &= write_reg(REG_TRIM2_MSB, reg_value); + } + + ret &= read_reg(REG_TRIM2_LSB, ®_value); + if (ret) { + reg_value = (reg_value & (~0x7F)) | (Hfosc & 0x7F); + ret &= write_reg(REG_TRIM2_LSB, reg_value); + } + + hal.scheduler->delay_microseconds(10); + + /* Update boot up status to 1 */ + if (ret) { + ret &= read_reg(REG_OTP_MTP_OTP_STATUS2, ®_value); + if (!ret) { + reg_value |= 0x01; + ret &= write_reg(REG_OTP_MTP_OTP_STATUS2, reg_value); + } + } + + /* Disable OTP and write switch */ + read_reg(REG_OTP_MTP_OTP_CFG1, ®_value); + reg_value &= ~0x03; + write_reg(REG_OTP_MTP_OTP_CFG1, reg_value); + + /* Lock the main register */ + write_reg(REG_MASTER_LOCK, 0x00); + + /* Move to standby */ + mode_select(0x00); + + return ret; +} + +bool AP_Baro_ICP201XX::configure() +{ + uint8_t reg_value = 0; + + /* Initiate Triggered Operation: Stay in Standby mode */ + reg_value |= (reg_value & (~0x10)) | ((uint8_t)_forced_meas_trigger << 4); + + /* Power Mode Selection: Normal Mode */ + reg_value |= (reg_value & (~0x04)) | ((uint8_t)_power_mode << 2); + + /* FIFO Readout Mode Selection: Pressure first. */ + reg_value |= (reg_value & (~0x03)) | ((uint8_t)(_fifo_readout_mode)); + + /* Measurement Configuration: Mode2*/ + reg_value |= (reg_value & (~0xE0)) | (((uint8_t)_op_mode) << 5); + + /* Measurement Mode Selection: Continuous Measurements (duty cycled) */ + reg_value |= (reg_value & (~0x08)) | ((uint8_t)_meas_mode << 3); + + return mode_select(reg_value); +} + +void AP_Baro_ICP201XX::wait_read() +{ + /* + * If FIR filter is enabled, it will cause a settling effect on the first 14 pressure values. + * Therefore the first 14 pressure output values are discarded. + **/ + uint8_t fifo_packets = 0; + uint8_t fifo_packets_to_skip = 14; + + do { + hal.scheduler->delay(10); + read_reg(REG_FIFO_FILL, &fifo_packets); + fifo_packets = (uint8_t)(fifo_packets & 0x1F); + } while (fifo_packets >= fifo_packets_to_skip); + + flush_fifo(); + fifo_packets = 0; + + do { + hal.scheduler->delay(10); + read_reg(REG_FIFO_FILL, &fifo_packets); + fifo_packets = (uint8_t)(fifo_packets & 0x1F); + } while (fifo_packets == 0); +} + +bool AP_Baro_ICP201XX::flush_fifo() +{ + uint8_t reg_value; + + if (!read_reg(REG_FIFO_FILL, ®_value)) { + return false; + } + + reg_value |= 0x80; + + if (!write_reg(REG_FIFO_FILL, reg_value)) { + return false; + } + + return true; +} + +void AP_Baro_ICP201XX::timer() +{ + float p = 0; + float t = 0; + + if (get_sensor_data(&p, &t)) { + WITH_SEMAPHORE(_sem); + + accum.psum += p; + accum.tsum += t; + accum.count++; + last_measure_us = AP_HAL::micros(); + } else { + if (AP_HAL::micros() - last_measure_us > CONVERSION_INTERVAL*3) { + flush_fifo(); + last_measure_us = AP_HAL::micros(); + } + } +} + +void AP_Baro_ICP201XX::update() +{ + 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; + } +} + +#endif // AP_BARO_ICP201XX_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_ICP201XX.h b/libraries/AP_Baro/AP_Baro_ICP201XX.h new file mode 100755 index 0000000000..5401816234 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_ICP201XX.h @@ -0,0 +1,85 @@ +#pragma once + +#include "AP_Baro_Backend.h" + +#ifndef AP_BARO_ICP201XX_ENABLED +#define AP_BARO_ICP201XX_ENABLED AP_BARO_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_BARO_ICP201XX_ENABLED + +#include +#include +#include + +class AP_Baro_ICP201XX : public AP_Baro_Backend +{ +public: + void update() override; + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); + +private: + AP_Baro_ICP201XX(AP_Baro &baro, AP_HAL::OwnPtr dev); + + bool init(); + void dummy_reg(); + bool read_reg(uint8_t reg, uint8_t *buf, uint8_t len); + bool read_reg(uint8_t reg, uint8_t *val); + bool write_reg(uint8_t reg, uint8_t val); + bool mode_select(uint8_t mode); + bool read_otp_data(uint8_t addr, uint8_t cmd, uint8_t *val); + bool get_sensor_data(float *pressure, float *temperature); + void soft_reset(); + bool boot_sequence(); + bool configure(); + void wait_read(); + bool flush_fifo(); + void timer(); + + uint8_t instance; + + AP_HAL::OwnPtr dev; + + // accumulation structure, protected by _sem + struct { + float tsum; + float psum; + uint32_t count; + } accum; + + // time last read command was sent + uint32_t last_measure_us; + + enum class OP_MODE : uint8_t { + OP_MODE0 = 0, /* Mode 0: Bw:6.25 Hz ODR: 25Hz */ + OP_MODE1, /* Mode 1: Bw:30 Hz ODR: 120Hz */ + OP_MODE2, /* Mode 2: Bw:10 Hz ODR: 40Hz */ + OP_MODE3, /* Mode 3: Bw:0.5 Hz ODR: 2Hz */ + OP_MODE4, /* Mode 4: User configurable Mode */ + } _op_mode{OP_MODE::OP_MODE2}; + + enum class FIFO_READOUT_MODE : uint8_t { + FIFO_READOUT_MODE_PRES_TEMP = 0, /* Pressure and temperature as pair and address wraps to the start address of the Pressure value ( pressure first ) */ + FIFO_READOUT_MODE_TEMP_ONLY = 1, /* Temperature only reporting */ + FIFO_READOUT_MODE_TEMP_PRES = 2, /* Pressure and temperature as pair and address wraps to the start address of the Temperature value ( Temperature first ) */ + FIFO_READOUT_MODE_PRES_ONLY = 3 /* Pressure only reporting */ + } _fifo_readout_mode{FIFO_READOUT_MODE::FIFO_READOUT_MODE_PRES_TEMP}; + + enum class POWER_MODE : uint8_t { + POWER_MODE_NORMAL = 0, /* Normal Mode: Device is in standby and goes to active mode during the execution of a measurement */ + POWER_MODE_ACTIVE = 1 /* Active Mode: Power on DVDD and enable the high frequency clock */ + } _power_mode{POWER_MODE::POWER_MODE_NORMAL}; + + enum MEAS_MODE : uint8_t { + MEAS_MODE_FORCED_TRIGGER = 0, /* Force trigger mode based on icp201xx_forced_meas_trigger_t **/ + MEAS_MODE_CONTINUOUS = 1 /* Continuous measurements based on selected mode ODR settings*/ + } _meas_mode{MEAS_MODE::MEAS_MODE_CONTINUOUS}; + + enum FORCED_MEAS_TRIGGER : uint8_t { + FORCE_MEAS_STANDBY = 0, /* Stay in Stand by */ + FORCE_MEAS_TRIGGER_FORCE_MEAS = 1 /* Trigger for forced measurements */ + } _forced_meas_trigger{FORCED_MEAS_TRIGGER::FORCE_MEAS_STANDBY}; +}; + +#endif // AP_BARO_ICP201XX_ENABLED \ No newline at end of file