aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: added MS5607 support

Browse Source
This commit is contained in:
Julien BERAUD 2015-07-10 13:56:06 +10:00 committed by Andrew Tridgell
parent 3cf952d1f8
commit d407737434
3 changed files with 103 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Baro/AP_Baro.cpp
View File

@ -282,9 +282,9 @@ void AP_Baro::init(void)
drivers[0] = new AP_Baro_BMP085(*this);
_num_drivers = 1;
}
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611 && HAL_BARO_MS5611_I2C_BUS == 0
{
drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(MS5611_I2C_ADDR), false);
drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(hal.i2c, HAL_BARO_MS5611_I2C_ADDR), false);
_num_drivers = 1;
}
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI
@ -295,6 +295,11 @@ void AP_Baro::init(void)
true);
_num_drivers = 1;
}
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5607 && HAL_BARO_MS5607_I2C_BUS == 1
{
drivers[0] = new AP_Baro_MS5607(*this, new AP_SerialBus_I2C(hal.i2c1, HAL_BARO_MS5607_I2C_ADDR), true);
_num_drivers = 1;
}
#endif
if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) {
hal.scheduler->panic(PSTR("Baro: unable to initialise driver"));

98
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -99,7 +99,8 @@ void AP_SerialBus_SPI::sem_give()
/// I2C SerialBus
AP_SerialBus_I2C::AP_SerialBus_I2C(uint8_t addr) :
AP_SerialBus_I2C::AP_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_i2c(i2c),
_addr(addr),
_i2c_sem(NULL)
{
@ -107,7 +108,7 @@ AP_SerialBus_I2C::AP_SerialBus_I2C(uint8_t addr) :
void AP_SerialBus_I2C::init()
{
_i2c_sem = hal.i2c->get_semaphore();
_i2c_sem = _i2c->get_semaphore();
if (_i2c_sem == NULL) {
hal.scheduler->panic(PSTR("AP_SerialBus_I2C did not get valid I2C semaphore!"));
}
@ -116,7 +117,7 @@ void AP_SerialBus_I2C::init()
uint16_t AP_SerialBus_I2C::read_16bits(uint8_t reg)
{
uint8_t buf[2];
if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
if (_i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
return (((uint16_t)(buf[0]) << 8) | buf[1]);
}
return 0;
@ -125,7 +126,7 @@ uint16_t AP_SerialBus_I2C::read_16bits(uint8_t reg)
uint32_t AP_SerialBus_I2C::read_24bits(uint8_t reg)
{
uint8_t buf[3];
if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
if (_i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
return (((uint32_t)buf[0]) << 16) | (((uint32_t)buf[1]) << 8) | buf[2];
}
return 0;
@ -133,7 +134,7 @@ uint32_t AP_SerialBus_I2C::read_24bits(uint8_t reg)
void AP_SerialBus_I2C::write(uint8_t reg)
{
hal.i2c->write(_addr, 1, &reg);
_i2c->write(_addr, 1, &reg);
}
bool AP_SerialBus_I2C::sem_take_blocking()
@ -154,7 +155,7 @@ void AP_SerialBus_I2C::sem_give()
/*
constructor
*/
AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) :
AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) :
AP_Baro_Backend(baro),
_serial(serial),
_updated(false),
@ -165,7 +166,7 @@ AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_tim
_instance = _frontend.register_sensor();
_serial->init();
if (!_serial->sem_take_blocking()){
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: failed to take serial semaphore for init"));
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"));
}
_serial->write(CMD_MS5611_RESET);
@ -173,12 +174,12 @@ AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_tim
// 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);
_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"));
@ -197,21 +198,21 @@ AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_tim
_serial->sem_give();
if (_use_timer) {
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Baro_MS5611::_timer, void));
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, void));
}
}
/**
* MS5611 crc4 method based on PX4Firmware code
*/
bool AP_Baro_MS5611::_check_crc(void)
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,
_C1, _C2, _C3, _C4, _C5, _C6,
_serial->read_16bits(CMD_MS5611_PROM_CRC) };
n_rem = 0x00;
@ -254,7 +255,7 @@ bool AP_Baro_MS5611::_check_crc(void)
We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
temperature does not change so quickly...
*/
void AP_Baro_MS5611::_timer(void)
void AP_Baro_MS56XX::_timer(void)
{
// Throttle read rate to 100hz maximum.
if (hal.scheduler->micros() - _last_timer < 10000) {
@ -311,7 +312,7 @@ void AP_Baro_MS5611::_timer(void)
_serial->sem_give();
}
void AP_Baro_MS5611::update()
void AP_Baro_MS56XX::update()
{
if (!_use_timer) {
// if we're not using the timer then accumulate one more time
@ -336,14 +337,19 @@ void AP_Baro_MS5611::update()
hal.scheduler->resume_timer_procs();
if (d1count != 0) {
D1 = ((float)sD1) / d1count;
_D1 = ((float)sD1) / d1count;
}
if (d2count != 0) {
D2 = ((float)sD2) / d2count;
_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()
{
@ -359,10 +365,10 @@ void AP_Baro_MS5611::_calculate()
// 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;
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
@ -375,7 +381,47 @@ void AP_Baro_MS5611::_calculate()
SENS = SENS - SENS2;
}
float pressure = (D1*SENS/2097152 - OFF)/32768;
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);
}
@ -385,7 +431,7 @@ void AP_Baro_MS5611::_calculate()
avoid conflicts on the semaphore from calling it in a timer, which
conflicts with the compass driver use of I2C
*/
void AP_Baro_MS5611::accumulate(void)
void AP_Baro_MS56XX::accumulate(void)
{
if (!_use_timer) {
// the timer isn't being called as a timer, so we need to call

33
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -6,8 +6,6 @@
#include <AP_HAL.h>
#include "AP_Baro.h"
#define MS5611_I2C_ADDR 0x77
/** Abstract serial bus device driver for I2C/SPI. */
class AP_SerialBus
{
@ -59,7 +57,7 @@ private:
class AP_SerialBus_I2C : public AP_SerialBus
{
public:
AP_SerialBus_I2C(uint8_t addr);
AP_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
void init();
uint16_t read_16bits(uint8_t reg);
uint32_t read_24bits(uint8_t reg);
@ -69,22 +67,22 @@ public:
void sem_give();
private:
AP_HAL::I2CDriver *_i2c;
uint8_t _addr;
AP_HAL::Semaphore *_i2c_sem;
};
class AP_Baro_MS5611 : public AP_Baro_Backend
class AP_Baro_MS56XX : public AP_Baro_Backend
{
public:
AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer);
AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_timer);
void update();
void accumulate();
private:
virtual void _calculate() = 0;
AP_SerialBus *_serial;
uint8_t _instance;
void _calculate();
bool _check_crc();
void _timer();
@ -99,9 +97,26 @@ private:
bool _use_timer;
protected:
// Internal calibration registers
uint16_t C1,C2,C3,C4,C5,C6;
float D1,D2;
uint16_t _C1,_C2,_C3,_C4,_C5,_C6;
float _D1,_D2;
uint8_t _instance;
};
class AP_Baro_MS5611 : public AP_Baro_MS56XX
{
public:
AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer);
private:
void _calculate();
};
class AP_Baro_MS5607 : public AP_Baro_MS56XX
{
public:
AP_Baro_MS5607(AP_Baro &baro, AP_SerialBus *serial, bool use_timer);
private:
void _calculate();
};
#endif // __AP_BARO_MS5611_H__