AP_Airspeed: Added MS5525 airspeed driver
also improved averaging in MS4525 driver
This commit is contained in:
Andrew Tridgell
Tom Pittenger
parent
04731dccd6
commit
43bac678cd
@ -25,6 +25,7 @@
|
||||
#include <utility>
|
||||
#include "AP_Airspeed.h"
|
||||
#include "AP_Airspeed_MS4525.h"
|
||||
#include "AP_Airspeed_MS5525.h"
|
||||
#include "AP_Airspeed_analog.h"
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
@ -51,7 +52,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
|
||||
// @Param: TYPE
|
||||
// @DisplayName: Airspeed type
|
||||
// @Description: Type of airspeed sensor
|
||||
// @Values: 0:None,1:I2C-MS4525D0,2:Analog
|
||||
// @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525
|
||||
// @User: Standard
|
||||
AP_GROUPINFO_FLAGS("TYPE", 0, AP_Airspeed, _type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
@ -106,6 +107,13 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
|
||||
// @Description: This parameter allows you to to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("PSI_RANGE", 8, AP_Airspeed, _psi_range, PSI_RANGE_DEFAULT),
|
||||
|
||||
// @Param: BUS
|
||||
// @DisplayName: Airspeed I2C bus
|
||||
// @Description: The bus number of the I2C bus to look for the sensor on
|
||||
// @Values: 0:Bus0,1:Bus1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("BUS", 9, AP_Airspeed, _bus, 1),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
@ -148,9 +156,14 @@ void AP_Airspeed::init()
|
||||
case TYPE_ANALOG:
|
||||
sensor = new AP_Airspeed_Analog(*this);
|
||||
break;
|
||||
case TYPE_I2C_MS5525:
|
||||
sensor = new AP_Airspeed_MS5525(*this);
|
||||
break;
|
||||
}
|
||||
if (sensor && !sensor->init()) {
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed init failed");
|
||||
delete sensor;
|
||||
sensor = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
@ -193,8 +206,6 @@ void AP_Airspeed::calibrate(bool in_startup)
|
||||
if (in_startup && _skip_cal) {
|
||||
return;
|
||||
}
|
||||
// discard first reading
|
||||
get_pressure();
|
||||
_cal.start_ms = AP_HAL::millis();
|
||||
_cal.count = 0;
|
||||
_cal.sum = 0;
|
||||
@ -209,7 +220,7 @@ void AP_Airspeed::update_calibration(float raw_pressure)
|
||||
// consider calibration complete when we have at least 10 samples
|
||||
// over at least 1 second
|
||||
if (AP_HAL::millis() - _cal.start_ms >= 1000 &&
|
||||
_cal.read_count > 10) {
|
||||
_cal.read_count > 15) {
|
||||
if (_cal.count == 0) {
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy");
|
||||
} else {
|
||||
@ -219,7 +230,8 @@ void AP_Airspeed::update_calibration(float raw_pressure)
|
||||
_cal.start_ms = 0;
|
||||
return;
|
||||
}
|
||||
if (_healthy) {
|
||||
// we discard the first 5 samples
|
||||
if (_healthy && _cal.read_count > 5) {
|
||||
_cal.sum += raw_pressure;
|
||||
_cal.count++;
|
||||
}
|
||||
|
||||
@ -146,6 +146,7 @@ public:
|
||||
TYPE_NONE=0,
|
||||
TYPE_I2C_MS4525=1,
|
||||
TYPE_ANALOG=2,
|
||||
TYPE_I2C_MS5525=3,
|
||||
};
|
||||
|
||||
private:
|
||||
@ -155,6 +156,7 @@ private:
|
||||
AP_Int8 _use;
|
||||
AP_Int8 _type;
|
||||
AP_Int8 _pin;
|
||||
AP_Int8 _bus;
|
||||
AP_Int8 _autocal;
|
||||
AP_Int8 _tube_order;
|
||||
AP_Int8 _skip_cal;
|
||||
|
||||
@ -21,9 +21,18 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_Airspeed.h"
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend) :
|
||||
frontend(_frontend)
|
||||
{}
|
||||
{
|
||||
sem = hal.util->new_semaphore();
|
||||
}
|
||||
|
||||
AP_Airspeed_Backend::~AP_Airspeed_Backend(void)
|
||||
{
|
||||
delete sem;
|
||||
}
|
||||
|
||||
|
||||
int8_t AP_Airspeed_Backend::get_pin(void) const
|
||||
@ -36,3 +45,7 @@ float AP_Airspeed_Backend::get_psi_range(void) const
|
||||
return frontend._psi_range;
|
||||
}
|
||||
|
||||
uint8_t AP_Airspeed_Backend::get_bus(void) const
|
||||
{
|
||||
return frontend._bus;
|
||||
}
|
||||
|
||||
@ -26,6 +26,7 @@ class AP_Airspeed;
|
||||
class AP_Airspeed_Backend {
|
||||
public:
|
||||
AP_Airspeed_Backend(AP_Airspeed &frontend);
|
||||
virtual ~AP_Airspeed_Backend();
|
||||
|
||||
// probe and initialise the sensor
|
||||
virtual bool init(void) = 0;
|
||||
@ -39,6 +40,10 @@ public:
|
||||
protected:
|
||||
int8_t get_pin(void) const;
|
||||
float get_psi_range(void) const;
|
||||
uint8_t get_bus(void) const;
|
||||
|
||||
// semaphore for access to shared frontend data
|
||||
AP_HAL::Semaphore *sem;
|
||||
|
||||
private:
|
||||
AP_Airspeed &frontend;
|
||||
|
||||
@ -43,30 +43,49 @@ AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend) :
|
||||
// probe and initialise the sensor
|
||||
bool AP_Airspeed_MS4525::init()
|
||||
{
|
||||
_dev = hal.i2c_mgr->get_device(MS4525D0_I2C_BUS, MS4525D0_I2C_ADDR);
|
||||
const struct {
|
||||
uint8_t bus;
|
||||
uint8_t addr;
|
||||
} addresses[] = {
|
||||
{ 1, MS4525D0_I2C_ADDR },
|
||||
{ 0, MS4525D0_I2C_ADDR },
|
||||
};
|
||||
bool found = false;
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(addresses); i++) {
|
||||
_dev = hal.i2c_mgr->get_device(addresses[i].bus, addresses[i].addr);
|
||||
if (!_dev) {
|
||||
continue;
|
||||
}
|
||||
if (!_dev->get_semaphore()->take(0)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// take i2c bus sempahore
|
||||
if (!_dev || !_dev->get_semaphore()->take(200)) {
|
||||
// lots of retries during probe
|
||||
_dev->set_retries(10);
|
||||
|
||||
_measure();
|
||||
hal.scheduler->delay(10);
|
||||
_collect();
|
||||
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
found = (_last_sample_time_ms != 0);
|
||||
if (found) {
|
||||
printf("MS4525: Found sensor on bus %u address 0x%02x\n", addresses[i].bus, addresses[i].addr);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
printf("MS4525: no sensor found\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
// lots of retries during probe
|
||||
_dev->set_retries(5);
|
||||
|
||||
_measure();
|
||||
hal.scheduler->delay(10);
|
||||
_collect();
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
// drop to 2 retries for runtime
|
||||
_dev->set_retries(2);
|
||||
|
||||
if (_last_sample_time_ms != 0) {
|
||||
_dev->register_periodic_callback(20000,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, bool));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
_dev->register_periodic_callback(20000,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, bool));
|
||||
return true;
|
||||
}
|
||||
|
||||
// start a measurement
|
||||
@ -114,10 +133,18 @@ void AP_Airspeed_MS4525::_collect()
|
||||
*/
|
||||
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
|
||||
|
||||
_pressure = diff_press_PSI * PSI_to_Pa;
|
||||
_temperature = ((200.0f * dT_raw) / 2047) - 50;
|
||||
float press = diff_press_PSI * PSI_to_Pa;
|
||||
float temp = ((200.0f * dT_raw) / 2047) - 50;
|
||||
|
||||
_voltage_correction(press, temp);
|
||||
|
||||
_voltage_correction(_pressure, _temperature);
|
||||
if (sem->take(0)) {
|
||||
_press_sum += press;
|
||||
_temp_sum += temp;
|
||||
_press_count++;
|
||||
_temp_count++;
|
||||
sem->give();
|
||||
}
|
||||
|
||||
_last_sample_time_ms = AP_HAL::millis();
|
||||
}
|
||||
@ -166,6 +193,14 @@ bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
|
||||
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
|
||||
return false;
|
||||
}
|
||||
if (sem->take(0)) {
|
||||
if (_press_count > 0) {
|
||||
_pressure = _press_sum / _press_count;
|
||||
_press_count = 0;
|
||||
_press_sum = 0;
|
||||
}
|
||||
sem->give();
|
||||
}
|
||||
pressure = _pressure;
|
||||
return true;
|
||||
}
|
||||
@ -176,6 +211,14 @@ bool AP_Airspeed_MS4525::get_temperature(float &temperature)
|
||||
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
|
||||
return false;
|
||||
}
|
||||
if (sem->take(0)) {
|
||||
if (_temp_count > 0) {
|
||||
_temperature = _temp_sum / _temp_count;
|
||||
_temp_count = 0;
|
||||
_temp_sum = 0;
|
||||
}
|
||||
sem->give();
|
||||
}
|
||||
temperature = _temperature;
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -47,7 +47,11 @@ private:
|
||||
void _collect();
|
||||
bool _timer();
|
||||
void _voltage_correction(float &diff_press_pa, float &temperature);
|
||||
|
||||
|
||||
float _temp_sum;
|
||||
float _press_sum;
|
||||
uint32_t _temp_count;
|
||||
uint32_t _press_count;
|
||||
float _temperature;
|
||||
float _pressure;
|
||||
uint32_t _last_sample_time_ms;
|
||||
|
||||
286
libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
Normal file
286
libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
Normal file
@ -0,0 +1,286 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
backend driver for airspeed from a I2C MS5525D0 sensor
|
||||
*/
|
||||
#include "AP_Airspeed_MS5525.h"
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <stdio.h>
|
||||
#include <utility>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
#define MS5525D0_I2C_ADDR_1 0x76
|
||||
#define MS5525D0_I2C_ADDR_2 0x77
|
||||
|
||||
#define REG_RESET 0x1E
|
||||
#define REG_CONVERT_D1_OSR_256 0x40
|
||||
#define REG_CONVERT_D1_OSR_512 0x42
|
||||
#define REG_CONVERT_D1_OSR_1024 0x44
|
||||
#define REG_CONVERT_D1_OSR_2048 0x46
|
||||
#define REG_CONVERT_D1_OSR_4096 0x48
|
||||
#define REG_CONVERT_D2_OSR_256 0x50
|
||||
#define REG_CONVERT_D2_OSR_512 0x52
|
||||
#define REG_CONVERT_D2_OSR_1024 0x54
|
||||
#define REG_CONVERT_D2_OSR_2048 0x56
|
||||
#define REG_CONVERT_D2_OSR_4096 0x58
|
||||
#define REG_ADC_READ 0x00
|
||||
#define REG_PROM_BASE 0xA0
|
||||
|
||||
// go for 1024 oversampling. This should be fast enough to reduce
|
||||
// noise but low enough to keep self-heating small
|
||||
#define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024
|
||||
#define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_OSR_1024
|
||||
|
||||
AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend) :
|
||||
AP_Airspeed_Backend(_frontend)
|
||||
{
|
||||
}
|
||||
|
||||
// probe and initialise the sensor
|
||||
bool AP_Airspeed_MS5525::init()
|
||||
{
|
||||
const uint8_t addresses[] = { MS5525D0_I2C_ADDR_1, MS5525D0_I2C_ADDR_2 };
|
||||
bool found = false;
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(addresses); i++) {
|
||||
dev = hal.i2c_mgr->get_device(get_bus(), addresses[i]);
|
||||
if (!dev) {
|
||||
continue;
|
||||
}
|
||||
if (!dev->get_semaphore()->take(0)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// lots of retries during probe
|
||||
dev->set_retries(5);
|
||||
|
||||
found = read_prom();
|
||||
|
||||
if (found) {
|
||||
printf("MS5525: Found sensor on bus %u address 0x%02x\n", get_bus(), addresses[i]);
|
||||
break;
|
||||
}
|
||||
dev->get_semaphore()->give();
|
||||
}
|
||||
if (!found) {
|
||||
printf("MS5525: no sensor found\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Send a command to read temperature first
|
||||
uint8_t reg = REG_CONVERT_TEMPERATURE;
|
||||
dev->transfer(®, 1, nullptr, 0);
|
||||
state = 0;
|
||||
|
||||
// drop to 2 retries for runtime
|
||||
dev->set_retries(2);
|
||||
|
||||
dev->get_semaphore()->give();
|
||||
|
||||
// read at 80Hz
|
||||
dev->register_periodic_callback(1000000UL/80U,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS5525::timer, bool));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* CRC used by MS pressure devices
|
||||
*/
|
||||
uint16_t AP_Airspeed_MS5525::crc4_prom(void)
|
||||
{
|
||||
uint16_t n_rem = 0;
|
||||
uint8_t n_bit;
|
||||
|
||||
for (uint8_t cnt = 0; cnt < sizeof(prom); cnt++) {
|
||||
/* uneven bytes */
|
||||
if (cnt & 1) {
|
||||
n_rem ^= (uint8_t)((prom[cnt >> 1]) & 0x00FF);
|
||||
} else {
|
||||
n_rem ^= (uint8_t)(prom[cnt >> 1] >> 8);
|
||||
}
|
||||
|
||||
for (n_bit = 8; n_bit > 0; n_bit--) {
|
||||
if (n_rem & 0x8000) {
|
||||
n_rem = (n_rem << 1) ^ 0x3000;
|
||||
} else {
|
||||
n_rem = (n_rem << 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (n_rem >> 12) & 0xF;
|
||||
}
|
||||
|
||||
bool AP_Airspeed_MS5525::read_prom(void)
|
||||
{
|
||||
// reset the chip to ensure it has correct prom values loaded
|
||||
uint8_t reg = REG_RESET;
|
||||
if (!dev->transfer(®, 1, nullptr, 0)) {
|
||||
return false;
|
||||
}
|
||||
hal.scheduler->delay(5);
|
||||
|
||||
bool all_zero = true;
|
||||
for (uint8_t i = 0; i < 8; i++) {
|
||||
if (!dev->read_uint16_be(REG_PROM_BASE+i*2, prom[i])) {
|
||||
return false;
|
||||
}
|
||||
if (prom[i] != 0) {
|
||||
all_zero = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (all_zero) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* save the read crc */
|
||||
const uint16_t crc_read = prom[7] & 0xf;
|
||||
|
||||
/* remove CRC byte */
|
||||
prom[7] &= 0xff00;
|
||||
|
||||
uint16_t crc_calc = crc4_prom();
|
||||
if (crc_read != crc_calc) {
|
||||
printf("MS5525: CRC mismatch 0x%04x 0x%04x\n", crc_read, crc_calc);
|
||||
}
|
||||
return crc_read == crc_calc;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
read from the ADC
|
||||
*/
|
||||
int32_t AP_Airspeed_MS5525::read_adc()
|
||||
{
|
||||
uint8_t val[3];
|
||||
if (!dev->read_registers(REG_ADC_READ, val, 3)) {
|
||||
return 0;
|
||||
}
|
||||
return (val[0] << 16) | (val[1] << 8) | val[2];
|
||||
}
|
||||
|
||||
/*
|
||||
calculate pressure and temperature
|
||||
*/
|
||||
void AP_Airspeed_MS5525::calculate(void)
|
||||
{
|
||||
// table for the 001DS part, 1PSI range
|
||||
const uint8_t Q1 = 15;
|
||||
const uint8_t Q2 = 17;
|
||||
const uint8_t Q3 = 7;
|
||||
const uint8_t Q4 = 5;
|
||||
const uint8_t Q5 = 7;
|
||||
const uint8_t Q6 = 21;
|
||||
|
||||
int64_t dT = D2 - int64_t(prom[5]) * (1UL<<Q5);
|
||||
int64_t TEMP = 2000 + (dT*int64_t(prom[6]))/(1UL<<Q6);
|
||||
int64_t OFF = int64_t(prom[2])*(1UL<<Q2) + (int64_t(prom[4])*dT)/(1UL<<Q4);
|
||||
int64_t SENS = int64_t(prom[1])*(1UL<<Q1) + (int64_t(prom[3])*dT)/(1UL<<Q3);
|
||||
int64_t P = (D1*SENS/(1UL<<21)-OFF)/(1UL<<15);
|
||||
const float PSI_to_Pa = 6894.757f;
|
||||
float P_Pa = PSI_to_Pa * 1.0e-4 * P;
|
||||
float Temp_C = TEMP * 0.01;
|
||||
|
||||
#if 0
|
||||
static uint16_t counter;
|
||||
if (counter++ == 100) {
|
||||
printf("P=%.6f T=%.2f D1=%d D2=%d\n", P_Pa, Temp_C, D1, D2);
|
||||
counter=0;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (sem->take(0)) {
|
||||
pressure_sum += P_Pa;
|
||||
temperature_sum += Temp_C;
|
||||
press_count++;
|
||||
temp_count++;
|
||||
last_sample_time_ms = AP_HAL::millis();
|
||||
sem->give();
|
||||
}
|
||||
}
|
||||
|
||||
// 50Hz timer
|
||||
bool AP_Airspeed_MS5525::timer()
|
||||
{
|
||||
uint32_t adc_val = read_adc();
|
||||
|
||||
/*
|
||||
* If read fails, re-initiate a read command for current state or we are
|
||||
* stuck
|
||||
*/
|
||||
uint8_t next_state = state;
|
||||
if (adc_val != 0) {
|
||||
next_state = (state + 1) % 5;
|
||||
|
||||
if (state == 0) {
|
||||
D2 = adc_val;
|
||||
} else {
|
||||
D1 = adc_val;
|
||||
calculate();
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t next_cmd = next_state == 0 ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
|
||||
if (!dev->transfer(&next_cmd, 1, nullptr, 0)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
state = next_state;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// return the current differential_pressure in Pascal
|
||||
bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
|
||||
{
|
||||
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
|
||||
return false;
|
||||
}
|
||||
if (sem->take(0)) {
|
||||
if (press_count > 0) {
|
||||
pressure = pressure_sum / press_count;
|
||||
press_count = 0;
|
||||
pressure_sum = 0;
|
||||
}
|
||||
sem->give();
|
||||
}
|
||||
_pressure = pressure;
|
||||
return true;
|
||||
}
|
||||
|
||||
// return the current temperature in degrees C, if available
|
||||
bool AP_Airspeed_MS5525::get_temperature(float &_temperature)
|
||||
{
|
||||
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
|
||||
return false;
|
||||
}
|
||||
if (sem->take(0)) {
|
||||
if (temp_count > 0) {
|
||||
temperature = temperature_sum / temp_count;
|
||||
temp_count = 0;
|
||||
temperature_sum = 0;
|
||||
}
|
||||
sem->give();
|
||||
}
|
||||
_temperature = temperature;
|
||||
return true;
|
||||
}
|
||||
69
libraries/AP_Airspeed/AP_Airspeed_MS5525.h
Normal file
69
libraries/AP_Airspeed/AP_Airspeed_MS5525.h
Normal file
@ -0,0 +1,69 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
/*
|
||||
backend driver for airspeed from I2C
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_HAL/utility/OwnPtr.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <utility>
|
||||
|
||||
#include "AP_Airspeed_Backend.h"
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
|
||||
class AP_Airspeed_MS5525 : public AP_Airspeed_Backend
|
||||
{
|
||||
public:
|
||||
AP_Airspeed_MS5525(AP_Airspeed &frontend);
|
||||
~AP_Airspeed_MS5525(void) {}
|
||||
|
||||
// probe and initialise the sensor
|
||||
bool init() override;
|
||||
|
||||
// return the current differential_pressure in Pascal
|
||||
bool get_differential_pressure(float &pressure) override;
|
||||
|
||||
// return the current temperature in degrees C, if available
|
||||
bool get_temperature(float &temperature) override;
|
||||
|
||||
private:
|
||||
void measure();
|
||||
void collect();
|
||||
bool timer();
|
||||
bool read_prom(void);
|
||||
uint16_t crc4_prom(void);
|
||||
int32_t read_adc();
|
||||
void calculate();
|
||||
|
||||
float pressure;
|
||||
float temperature;
|
||||
float temperature_sum;
|
||||
float pressure_sum;
|
||||
uint32_t temp_count;
|
||||
uint32_t press_count;
|
||||
|
||||
uint32_t last_sample_time_ms;
|
||||
|
||||
uint16_t prom[8];
|
||||
uint8_t state;
|
||||
int32_t D1;
|
||||
int32_t D2;
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
|
||||
};
|
||||
Loading…
Reference in New Issue
Block a user