Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: add support for ICP101XX

Browse Source
This commit is contained in:
李孟晓 2022-06-02 01:53:24 +00:00 committed by Andrew Tridgell
parent 336a6b0359
commit 522f1fa6de
4 changed files with 384 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Baro/AP_Baro.cpp
View File

@ -46,6 +46,7 @@
#include "AP_Baro_UAVCAN.h" #include "AP_Baro_UAVCAN.h"
#include "AP_Baro_MSP.h" #include "AP_Baro_MSP.h"
#include "AP_Baro_ExternalAHRS.h" #include "AP_Baro_ExternalAHRS.h"
#include "AP_Baro_ICP101XX.h"
#include <AP_Airspeed/AP_Airspeed.h> #include <AP_Airspeed/AP_Airspeed.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>

1
libraries/AP_Baro/AP_Baro_Backend.h
View File

@ -56,6 +56,7 @@ public:
DEVTYPE_BARO_SPL06 = 0x0C, DEVTYPE_BARO_SPL06 = 0x0C,
DEVTYPE_BARO_UAVCAN = 0x0D, DEVTYPE_BARO_UAVCAN = 0x0D,
DEVTYPE_BARO_MSP = 0x0E, DEVTYPE_BARO_MSP = 0x0E,
DEVTYPE_BARO_ICP101XX = 0x0F,
}; };
protected: protected:

315
libraries/AP_Baro/AP_Baro_ICP101XX.cpp Executable file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "AP_Baro_ICP101XX.h"
#if AP_BARO_ICP101XX_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 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<AP_HAL::I2CDevice> _dev)
: AP_Baro_Backend(baro)
, dev(std::move(_dev))
{
}
AP_Baro_Backend *AP_Baro_ICP101XX::probe(AP_Baro &baro,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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

67
libraries/AP_Baro/AP_Baro_ICP101XX.h Executable file
View File

@ -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 <AP_HAL/AP_HAL.h>
#include <AP_HAL/Semaphores.h>
#include <AP_HAL/Device.h>
class AP_Baro_ICP101XX : public AP_Baro_Backend
{
public:
void update() override;
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
private:
AP_Baro_ICP101XX(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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<AP_HAL::I2CDevice> 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