Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 08:58:29 -04:00
Code Issues Packages Projects Releases Wiki Activity
7a3b59652f
ardupilot/libraries/AP_Baro/AP_Baro_MS5611.cpp
Julien BERAUD 3b5d73b1fe AP_Baro_MS5611: Fix state machine in case of error 
If there is a read error, reading from the adc will return 0 but moreover,
we need to re-initiate a read or else we are stuck forever.

From MS5611-01BA03 datasheet, p. 10, CONVERSION SEQUENCE:
"After the conversion, using ADC read command the result is clocked out with the MSB first.
If the conversion is not executed before the ADC read command, or the ADC read command is
repeated, it will give 0 as the output result."
2015-09-23 09:19:22 +10:00

458 lines
12 KiB
C++
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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/>.
*/
/*
originally written by Jose Julio, Pat Hickey and Jordi Muñoz
Heavily modified by Andrew Tridgell
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_Baro.h"
extern const AP_HAL::HAL& hal;
#define CMD_MS5611_RESET 0x1E
#define CMD_MS5611_PROM_Setup 0xA0
#define CMD_MS5611_PROM_C1 0xA2
#define CMD_MS5611_PROM_C2 0xA4
#define CMD_MS5611_PROM_C3 0xA6
#define CMD_MS5611_PROM_C4 0xA8
#define CMD_MS5611_PROM_C5 0xAA
#define CMD_MS5611_PROM_C6 0xAC
#define CMD_MS5611_PROM_CRC 0xAE
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling)
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling)
// SPI Device //////////////////////////////////////////////////////////////////
AP_SerialBus_SPI::AP_SerialBus_SPI(enum AP_HAL::SPIDevice device, enum AP_HAL::SPIDeviceDriver::bus_speed speed) :
_device(device),
_speed(speed),
_spi(NULL),
_spi_sem(NULL)
{
}
void AP_SerialBus_SPI::init()
{
_spi = hal.spi->device(_device);
if (_spi == NULL) {
hal.scheduler->panic(PSTR("did not get valid SPI device driver!"));
}
_spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) {
hal.scheduler->panic(PSTR("AP_SerialBus_SPI did not get valid SPI semaphroe!"));
}
_spi->set_bus_speed(_speed);
}
uint16_t AP_SerialBus_SPI::read_16bits(uint8_t reg)
{
uint8_t tx[3] = { reg, 0, 0 };
uint8_t rx[3];
_spi->transaction(tx, rx, 3);
return ((uint16_t) rx[1] << 8 ) | ( rx[2] );
}
uint32_t AP_SerialBus_SPI::read_24bits(uint8_t reg)
{
uint8_t tx[4] = { reg, 0, 0, 0 };
uint8_t rx[4];
_spi->transaction(tx, rx, 4);
return (((uint32_t)rx[1])<<16) | (((uint32_t)rx[2])<<8) | ((uint32_t)rx[3]);
}
bool AP_SerialBus_SPI::write(uint8_t reg)
{
uint8_t tx[1] = { reg };
_spi->transaction(tx, NULL, 1);
return true;
}
bool AP_SerialBus_SPI::sem_take_blocking()
{
return _spi_sem->take(10);
}
bool AP_SerialBus_SPI::sem_take_nonblocking()
{
return _spi_sem->take_nonblocking();
}
void AP_SerialBus_SPI::sem_give()
{
_spi_sem->give();
}
/// I2C SerialBus
AP_SerialBus_I2C::AP_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_i2c(i2c),
_addr(addr),
_i2c_sem(NULL)
{
}
void AP_SerialBus_I2C::init()
{
_i2c_sem = _i2c->get_semaphore();
if (_i2c_sem == NULL) {
hal.scheduler->panic(PSTR("AP_SerialBus_I2C did not get valid I2C semaphore!"));
}
}
uint16_t AP_SerialBus_I2C::read_16bits(uint8_t reg)
{
uint8_t buf[2];
if (_i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
return (((uint16_t)(buf[0]) << 8) | buf[1]);
}
return 0;
}
uint32_t AP_SerialBus_I2C::read_24bits(uint8_t reg)
{
uint8_t buf[3];
if (_i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
return (((uint32_t)buf[0]) << 16) | (((uint32_t)buf[1]) << 8) | buf[2];
}
return 0;
}
bool AP_SerialBus_I2C::write(uint8_t reg)
{
return _i2c->write(_addr, 1, &reg) == 0;
}
bool AP_SerialBus_I2C::sem_take_blocking()
{
return _i2c_sem->take(10);
}
bool AP_SerialBus_I2C::sem_take_nonblocking()
{
return _i2c_sem->take_nonblocking();
}
void AP_SerialBus_I2C::sem_give()
{
_i2c_sem->give();
}
/*
constructor
*/
AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) :
AP_Baro_Backend(baro),
_serial(serial),
_updated(false),
_state(0),
_last_timer(0),
_use_timer(use_timer),
_D1(0.0f),
_D2(0.0f)
{
_instance = _frontend.register_sensor();
_serial->init();
if (!_serial->sem_take_blocking()){
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"));
}
_serial->write(CMD_MS5611_RESET);
hal.scheduler->delay(4);
// We read the factory calibration
// The on-chip CRC is not used
_C1 = _serial->read_16bits(CMD_MS5611_PROM_C1);
_C2 = _serial->read_16bits(CMD_MS5611_PROM_C2);
_C3 = _serial->read_16bits(CMD_MS5611_PROM_C3);
_C4 = _serial->read_16bits(CMD_MS5611_PROM_C4);
_C5 = _serial->read_16bits(CMD_MS5611_PROM_C5);
_C6 = _serial->read_16bits(CMD_MS5611_PROM_C6);
if (!_check_crc()) {
hal.scheduler->panic(PSTR("Bad CRC on MS5611"));
}
// Send a command to read Temp first
_serial->write(CMD_CONVERT_D2_OSR4096);
_last_timer = hal.scheduler->micros();
_state = 0;
_s_D1 = 0;
_s_D2 = 0;
_d1_count = 0;
_d2_count = 0;
_serial->sem_give();
if (_use_timer) {
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, void));
}
}
/**
* MS5611 crc4 method based on PX4Firmware code
*/
bool AP_Baro_MS56XX::_check_crc(void)
{
int16_t cnt;
uint16_t n_rem;
uint16_t crc_read;
uint8_t n_bit;
uint16_t n_prom[8] = { _serial->read_16bits(CMD_MS5611_PROM_Setup),
_C1, _C2, _C3, _C4, _C5, _C6,
_serial->read_16bits(CMD_MS5611_PROM_CRC) };
n_rem = 0x00;
/* save the read crc */
crc_read = n_prom[7];
/* remove CRC byte */
n_prom[7] = (0xFF00 & (n_prom[7]));
for (cnt = 0; cnt < 16; cnt++) {
/* uneven bytes */
if (cnt & 1) {
n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
} else {
n_rem ^= (uint8_t)(n_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);
}
}
}
/* final 4 bit remainder is CRC value */
n_rem = (0x000F & (n_rem >> 12));
n_prom[7] = crc_read;
/* return true if CRCs match */
return (0x000F & crc_read) == (n_rem ^ 0x00);
}
/*
Read the sensor. This is a state machine
We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
temperature does not change so quickly...
*/
void AP_Baro_MS56XX::_timer(void)
{
// Throttle read rate to 100hz maximum.
if (hal.scheduler->micros() - _last_timer < 10000) {
return;
}
if (!_serial->sem_take_nonblocking()) {
return;
}
if (_state == 0) {
// On state 0 we read temp
uint32_t d2 = _serial->read_24bits(0);
if (d2 != 0) {
_s_D2 += d2;
_d2_count++;
if (_d2_count == 32) {
// we have summed 32 values. This only happens
// when we stop reading the barometer for a long time
// (more than 1.2 seconds)
_s_D2 >>= 1;
_d2_count = 16;
}
if (_serial->write(CMD_CONVERT_D1_OSR4096)) { // Command to read pressure
_state++;
}
} else {
/* if read fails, re-initiate a temperature read command or we are
* stuck */
_serial->write(CMD_CONVERT_D2_OSR4096);
}
} else {
uint32_t d1 = _serial->read_24bits(0);;
if (d1 != 0) {
// occasional zero values have been seen on the PXF
// board. These may be SPI errors, but safest to ignore
_s_D1 += d1;
_d1_count++;
if (_d1_count == 128) {
// we have summed 128 values. This only happens
// when we stop reading the barometer for a long time
// (more than 1.2 seconds)
_s_D1 >>= 1;
_d1_count = 64;
}
// Now a new reading exists
_updated = true;
if (_state == 4) {
if (_serial->write(CMD_CONVERT_D2_OSR4096)) { // Command to read temperature
_state = 0;
}
} else {
if (_serial->write(CMD_CONVERT_D1_OSR4096)) { // Command to read pressure
_state++;
}
}
} else {
/* if read fails, re-initiate a pressure read command or we are
* stuck */
_serial->write(CMD_CONVERT_D1_OSR4096);
}
}
_last_timer = hal.scheduler->micros();
_serial->sem_give();
}
void AP_Baro_MS56XX::update()
{
if (!_use_timer) {
// if we're not using the timer then accumulate one more time
// to cope with the calibration loop and minimise lag
accumulate();
}
if (!_updated) {
return;
}
uint32_t sD1, sD2;
uint8_t d1count, d2count;
// Suspend timer procs because these variables are written to
// in "_update".
hal.scheduler->suspend_timer_procs();
sD1 = _s_D1; _s_D1 = 0;
sD2 = _s_D2; _s_D2 = 0;
d1count = _d1_count; _d1_count = 0;
d2count = _d2_count; _d2_count = 0;
_updated = false;
hal.scheduler->resume_timer_procs();
if (d1count != 0) {
_D1 = ((float)sD1) / d1count;
}
if (d2count != 0) {
_D2 = ((float)sD2) / d2count;
}
_calculate();
}
/* MS5611 class */
AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer)
:AP_Baro_MS56XX(baro, serial, use_timer)
{}
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5611::_calculate()
{
float dT;
float TEMP;
float OFF;
float SENS;
// Formulas from manufacturer datasheet
// sub -20c temperature compensation is not included
// we do the calculations using floating point
// as this is much faster on an AVR2560, and also allows
// us to take advantage of the averaging of D1 and D1 over
// multiple samples, giving us more precision
dT = _D2-(((uint32_t)_C5)<<8);
TEMP = (dT * _C6)/8388608;
OFF = _C2 * 65536.0f + (_C4 * dT) / 128;
SENS = _C1 * 32768.0f + (_C3 * dT) / 256;
if (TEMP < 0) {
// second order temperature compensation when under 20 degrees C
float T2 = (dT*dT) / 0x80000000;
float Aux = TEMP*TEMP;
float OFF2 = 2.5f*Aux;
float SENS2 = 1.25f*Aux;
TEMP = TEMP - T2;
OFF = OFF - OFF2;
SENS = SENS - SENS2;
}
float pressure = (_D1*SENS/2097152 - OFF)/32768;
float temperature = (TEMP + 2000) * 0.01f;
_copy_to_frontend(_instance, pressure, temperature);
}
/* MS5607 Class */
AP_Baro_MS5607::AP_Baro_MS5607(AP_Baro &baro, AP_SerialBus *serial, bool use_timer)
:AP_Baro_MS56XX(baro, serial, use_timer)
{}
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5607::_calculate()
{
float dT;
float TEMP;
float OFF;
float SENS;
// Formulas from manufacturer datasheet
// sub -20c temperature compensation is not included
// we do the calculations using floating point
// as this is much faster on an AVR2560, and also allows
// us to take advantage of the averaging of D1 and D1 over
// multiple samples, giving us more precision
dT = _D2-(((uint32_t)_C5)<<8);
TEMP = (dT * _C6)/8388608;
OFF = _C2 * 131072.0f + (_C4 * dT) / 64;
SENS = _C1 * 65536.0f + (_C3 * dT) / 128;
if (TEMP < 0) {
// second order temperature compensation when under 20 degrees C
float T2 = (dT*dT) / 0x80000000;
float Aux = TEMP*TEMP;
float OFF2 = 61.0f*Aux/16.0f;
float SENS2 = 2.0f*Aux;
TEMP = TEMP - T2;
OFF = OFF - OFF2;
SENS = SENS - SENS2;
}
float pressure = (_D1*SENS/2097152 - OFF)/32768;
float temperature = (TEMP + 2000) * 0.01f;
_copy_to_frontend(_instance, pressure, temperature);
}
/*
Read the sensor from main code. This is only used for I2C MS5611 to
avoid conflicts on the semaphore from calling it in a timer, which
conflicts with the compass driver use of I2C
*/
void AP_Baro_MS56XX::accumulate(void)
{
if (!_use_timer) {
// the timer isn't being called as a timer, so we need to call
// it in accumulate()
_timer();
}
}
Reference in New Issue View Git Blame Copy Permalink