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_ICP101XX.cpp

315 lines
7.7 KiB
C++
Executable File
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_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
Reference in New Issue View Git Blame Copy Permalink