From 522f1fa6de09fe08326a93f1d0b040446feaee07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E5=AD=9F=E6=99=93?= Date: Thu, 2 Jun 2022 01:53:24 +0000 Subject: [PATCH] AP_Baro: add support for ICP101XX --- libraries/AP_Baro/AP_Baro.cpp | 1 + libraries/AP_Baro/AP_Baro_Backend.h | 1 + libraries/AP_Baro/AP_Baro_ICP101XX.cpp | 315 +++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_ICP101XX.h | 67 ++++++ 4 files changed, 384 insertions(+) create mode 100755 libraries/AP_Baro/AP_Baro_ICP101XX.cpp create mode 100755 libraries/AP_Baro/AP_Baro_ICP101XX.h diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index e22fd8c126..b87a5f85b4 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -46,6 +46,7 @@ #include "AP_Baro_UAVCAN.h" #include "AP_Baro_MSP.h" #include "AP_Baro_ExternalAHRS.h" +#include "AP_Baro_ICP101XX.h" #include #include diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 32fbc8fdf8..3fcba57299 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -56,6 +56,7 @@ public: DEVTYPE_BARO_SPL06 = 0x0C, DEVTYPE_BARO_UAVCAN = 0x0D, DEVTYPE_BARO_MSP = 0x0E, + DEVTYPE_BARO_ICP101XX = 0x0F, }; protected: diff --git a/libraries/AP_Baro/AP_Baro_ICP101XX.cpp b/libraries/AP_Baro/AP_Baro_ICP101XX.cpp new file mode 100755 index 0000000000..f06df354d0 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_ICP101XX.cpp @@ -0,0 +1,315 @@ +/* + 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_ICP101XX.h" + +#if AP_BARO_ICP101XX_ENABLED + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +extern const AP_HAL::HAL &hal; + +#define ICP101XX_ID 0x08 +#define CMD_READ_ID 0xefc8 +#define CMD_SET_ADDR 0xc595 +#define CMD_READ_OTP 0xc7f7 +#define CMD_MEAS_LP 0x609c +#define CMD_MEAS_N 0x6825 +#define CMD_MEAS_LN 0x70df +#define CMD_MEAS_ULN 0x7866 +#define CMD_SOFT_RESET 0x805d + +/* + constructor + */ +AP_Baro_ICP101XX::AP_Baro_ICP101XX(AP_Baro &baro, AP_HAL::OwnPtr _dev) + : AP_Baro_Backend(baro) + , dev(std::move(_dev)) +{ +} + +AP_Baro_Backend *AP_Baro_ICP101XX::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + AP_Baro_ICP101XX *sensor = new AP_Baro_ICP101XX(baro, std::move(dev)); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_ICP101XX::init() +{ + if (!dev) { + return false; + } + + dev->get_semaphore()->take_blocking(); + + uint16_t id = 0; + read_response(CMD_READ_ID, (uint8_t *)&id, 2); + uint8_t whoami = (id >> 8) & 0x3f; // Product ID Bits 5:0 + if (whoami != ICP101XX_ID) { + goto failed; + } + + if (!send_command(CMD_SOFT_RESET)) { + goto failed; + } + + // wait for sensor to settle + hal.scheduler->delay(10); + + if (!read_calibration_data()) { + goto failed; + } + + // start a reading + if (!start_measure(CMD_MEAS_ULN)) { + goto failed; + } + + dev->set_retries(0); + + instance = _frontend.register_sensor(); + + dev->set_device_type(DEVTYPE_BARO_ICP101XX); + set_bus_id(instance, dev->get_bus_id()); + + dev->get_semaphore()->give(); + + dev->register_periodic_callback(measure_interval/2, FUNCTOR_BIND_MEMBER(&AP_Baro_ICP101XX::timer, void)); + + return true; + + failed: + dev->get_semaphore()->give(); + return false; +} + +bool AP_Baro_ICP101XX::read_measure_results(uint8_t *buf, uint8_t len) +{ + return dev->transfer(nullptr, 0, buf, len); +} + +bool AP_Baro_ICP101XX::read_response(uint16_t cmd, uint8_t *buf, uint8_t len) +{ + uint8_t buff[2]; + buff[0] = (cmd >> 8) & 0xff; + buff[1] = cmd & 0xff; + return dev->transfer(&buff[0], 2, buf, len); +} + +bool AP_Baro_ICP101XX::send_command(uint16_t cmd) +{ + uint8_t buf[2]; + buf[0] = (cmd >> 8) & 0xff; + buf[1] = cmd & 0xff; + return dev->transfer(buf, sizeof(buf), nullptr, 0); +} + +bool AP_Baro_ICP101XX::send_command(uint16_t cmd, uint8_t *data, uint8_t len) +{ + uint8_t buf[5]; + buf[0] = (cmd >> 8) & 0xff; + buf[1] = cmd & 0xff; + memcpy(&buf[2], data, len); + return dev->transfer(&buf[0], len + 2, nullptr, 0); +} + +int8_t AP_Baro_ICP101XX::cal_crc(uint8_t seed, uint8_t data) +{ + int8_t poly = 0x31; + int8_t var2; + uint8_t i; + + for (i = 0; i < 8; i++) { + if ((seed & 0x80) ^ (data & 0x80)) { + var2 = 1; + + } else { + var2 = 0; + } + + seed = (seed & 0x7F) << 1; + data = (data & 0x7F) << 1; + seed = seed ^ (uint8_t)(poly * var2); + } + + return (int8_t)seed; +} + +bool AP_Baro_ICP101XX::read_calibration_data(void) +{ + // setup for OTP read + uint8_t cmd[3] = { 0x00, 0x66, 0x9c }; + if (!send_command(CMD_SET_ADDR, cmd, 3)) { + return false; + } + for (uint8_t i = 0; i < 4; i++) { + uint8_t d[3]; + uint8_t crc = 0xff; + read_response(CMD_READ_OTP, d, 3); + for (int j = 0; j < 2; j++) { + crc = (uint8_t)cal_crc(crc, d[j]); + } + if (crc != d[2]) { + return false; + } + sensor_constants[i] = (d[0] << 8) | d[1]; + } + return true; +} + +bool AP_Baro_ICP101XX::start_measure(uint16_t mode) +{ + /* + From ds-000186-icp-101xx-v1.0.pdf, page 6, table 1 + + Sensor Measurement Max Time + Mode Time (Forced) + Low Power (LP) 1.6 ms 1.8 ms + Normal (N) 5.6 ms 6.3 ms + Low Noise (LN) 20.8 ms 23.8 ms + Ultra Low Noise(ULN) 83.2 ms 94.5 ms + */ + + switch (mode) { + case CMD_MEAS_LP: + measure_interval = 2000; + break; + + case CMD_MEAS_LN: + measure_interval = 24000; + break; + + case CMD_MEAS_ULN: + measure_interval = 95000; + break; + + case CMD_MEAS_N: + default: + measure_interval = 7000; + break; + } + + if (!send_command(mode)) { + return false; + } + + return true; +} + +void AP_Baro_ICP101XX::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_ICP101XX::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); +} + +void AP_Baro_ICP101XX::convert_data(uint32_t Praw, uint32_t Traw) +{ + // temperature is easy + float T = -45 + (175.0f / (1U<<16)) * Traw; + + // pressure involves a few more calculations + float P = get_pressure(Praw, Traw); + + if (!pressure_ok(P)) { + return; + } + + WITH_SEMAPHORE(_sem); + + accum.psum += P; + accum.tsum += T; + accum.count++; +} + +void AP_Baro_ICP101XX::timer(void) +{ + uint8_t d[9] {}; + if (read_measure_results(d, 9)) { + // ignore CRC bytes for now + uint32_t Traw = (uint32_t(d[0]) << 8) | d[1]; + uint32_t Praw = (uint32_t(d[3]) << 16) | (uint32_t(d[4]) << 8) | d[6]; + + convert_data(Praw, Traw); + start_measure(CMD_MEAS_ULN); + last_measure_us = AP_HAL::micros(); + } else { + if (AP_HAL::micros() - last_measure_us > measure_interval*3) { + // lost a sample + start_measure(CMD_MEAS_ULN); + last_measure_us = AP_HAL::micros(); + } + } +} + +void AP_Baro_ICP101XX::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_ICP101XX_ENABLED \ No newline at end of file diff --git a/libraries/AP_Baro/AP_Baro_ICP101XX.h b/libraries/AP_Baro/AP_Baro_ICP101XX.h new file mode 100755 index 0000000000..ee94db8af7 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_ICP101XX.h @@ -0,0 +1,67 @@ +#pragma once + +#include "AP_Baro_Backend.h" + +#ifndef AP_BARO_ICP101XX_ENABLED +#define AP_BARO_ICP101XX_ENABLED AP_BARO_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_BARO_ICP101XX_ENABLED + +#include +#include +#include + +class AP_Baro_ICP101XX : public AP_Baro_Backend +{ +public: + void update() override; + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); + +private: + AP_Baro_ICP101XX(AP_Baro &baro, AP_HAL::OwnPtr dev); + + bool init(); + bool send_cmd16(uint16_t cmd); + bool read_measure_results(uint8_t *buf, uint8_t len); + bool read_response(uint16_t cmd, uint8_t *buf, uint8_t len); + bool send_command(uint16_t cmd); + bool send_command(uint16_t cmd, uint8_t *data, uint8_t len); + int8_t cal_crc(uint8_t seed, uint8_t data); + bool start_measure(uint16_t mode); + bool read_calibration_data(void); + void convert_data(uint32_t Praw, uint32_t Traw); + void calculate_conversion_constants(const float p_Pa[3], const float p_LUT[3], + float &A, float &B, float &C); + float get_pressure(uint32_t p_LSB, uint32_t T_LSB); + void timer(void); + + // calibration data + int16_t sensor_constants[4]; + + uint8_t instance; + + AP_HAL::OwnPtr dev; + + // time last read command was sent + uint32_t last_measure_us; + + // accumulation structure, protected by _sem + struct { + float tsum; + float psum; + uint32_t count; + } accum; + + // conversion constants. Thanks to invensense for including python + // sample code in the datasheet! + const float p_Pa_calib[3] = {45000.0, 80000.0, 105000.0}; + const float LUT_lower = 3.5 * (1U<<20); + const float LUT_upper = 11.5 * (1U<<20); + const float quadr_factor = 1 / 16777216.0; + const float offst_factor = 2048.0; + uint32_t measure_interval = 0; +}; + +#endif // AP_BARO_ICP101XX_ENABLED \ No newline at end of file