Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Baro/AP_Baro_ICP201XX.cpp

498 lines
13 KiB
C++
Raw Blame History

/*
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_Baro_ICP201XX.h"
#if AP_BARO_ICP201XX_ENABLED
#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 <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;
#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<AP_HAL::I2CDevice> _dev)
: AP_Baro_Backend(baro)
, dev(std::move(_dev))
{
}
AP_Baro_Backend *AP_Baro_ICP201XX::probe(AP_Baro &baro,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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_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(&reg, 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(&reg, 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, &reg_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, &reg_value);
reg_value |= 1 << 7;
write_reg(REG_OTP_DEBUG2, reg_value);
hal.scheduler->delay_microseconds(10);
read_reg(REG_OTP_DEBUG2, &reg_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, &reg_value);
if (ret) {
reg_value = (reg_value & (~0x3F)) | (offset & 0x3F);
ret &= write_reg(REG_TRIM1_MSB, reg_value);
}
ret &= read_reg(REG_TRIM2_MSB, &reg_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, &reg_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, &reg_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, &reg_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, &reg_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
Reference in New Issue View Git Blame Copy Permalink