From f1ade970a39ccd4ee1ff725bda4c7ad0362848af Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 4 Jan 2016 11:59:19 -0200 Subject: [PATCH] AP_Baro: MS5611: Use AP_HAL::Device abstraction This allows to share almost all the I2C/SPI code and remove the AP_Serial abstraction since that is now handled by AP_HAL itself. --- libraries/AP_Baro/AP_Baro.cpp | 67 ++++---- libraries/AP_Baro/AP_Baro_MS5611.cpp | 236 ++++++++------------------- libraries/AP_Baro/AP_Baro_MS5611.h | 87 ++-------- libraries/AP_HAL/AP_HAL_Boards.h | 25 ++- 4 files changed, 129 insertions(+), 286 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index a61baf7007..20ea6c970e 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -20,6 +20,8 @@ */ #include "AP_Baro.h" +#include + #include #include #include @@ -67,7 +69,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = { // @Description: This selects which barometer will be the primary if multiple barometers are found // @Values: 0:FirstBaro,1:2ndBaro,2:3rdBaro AP_GROUPINFO("PRIMARY", 6, AP_Baro, _primary_baro, 0), - + AP_GROUPEND }; @@ -273,7 +275,7 @@ float AP_Baro::get_calibration_temperature(uint8_t instance) const if (ret > 25) { ret = 25; } - return ret; + return ret; } @@ -296,49 +298,36 @@ void AP_Baro::init(void) _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_BMP085 drivers[0] = new AP_Baro_BMP085(*this, - hal.i2c_mgr->get_device(HAL_BARO_BMP085_BUS, HAL_BARO_BMP085_I2C_ADDR)); + std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP085_BUS, HAL_BARO_BMP085_I2C_ADDR))); + _num_drivers = 1; +#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_I2C + drivers[0] = new AP_Baro_MS5611(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR)), + HAL_BARO_MS5611_USE_TIMER); _num_drivers = 1; -#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611 && HAL_BARO_MS5611_I2C_BUS == 0 - { - 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 && HAL_BARO_MS5611_I2C_BUS == 1 - { - drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(hal.i2c1, HAL_BARO_MS5611_I2C_ADDR), false); - _num_drivers = 1; - } #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI - { - drivers[0] = new AP_Baro_MS5611(*this, - new AP_SerialBus_SPI(AP_HAL::SPIDevice_MS5611, - AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH), - 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; - } + drivers[0] = new AP_Baro_MS5611(*this, + std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)), + true); + _num_drivers = 1; +#elif HAL_BARO_DEFAULT == HAL_BARO_MS5607_I2C + drivers[0] = new AP_Baro_MS5607(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)), + true); + _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_MS5637_I2C - { - AP_SerialBus *bus = new AP_SerialBus_I2C(HAL_BARO_MS5611_I2C_POINTER, - HAL_BARO_MS5611_I2C_ADDR); - drivers[0] = new AP_Baro_MS5637(*this, bus, true); - _num_drivers = 1; - } + drivers[0] = new AP_Baro_MS5637(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5637_I2C_BUS, HAL_BARO_MS5637_I2C_ADDR)), + true); + _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT - { - drivers[0] = new AP_Baro_QFLIGHT(*this); - _num_drivers = 1; - } + drivers[0] = new AP_Baro_QFLIGHT(*this); + _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_QURT - { - drivers[0] = new AP_Baro_QURT(*this); - _num_drivers = 1; - } + drivers[0] = new AP_Baro_QURT(*this); + _num_drivers = 1; #endif + if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) { AP_HAL::panic("Baro: unable to initialise driver"); } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 0dd1d8664f..f135c1537f 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -13,31 +13,31 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ - -/* - originally written by Jose Julio, Pat Hickey and Jordi Muñoz - - Heavily modified by Andrew Tridgell -*/ #include "AP_Baro_MS5611.h" -#include +#include -extern const AP_HAL::HAL& hal; +extern const AP_HAL::HAL &hal; -#define CMD_MS5611_RESET 0x1E -#define CMD_MS56XX_PROM 0xA0 +static const uint8_t CMD_MS56XX_RESET = 0x1E; +static const uint8_t CMD_MS56XX_READ_ADC = 0x00; -#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ -#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ +/* PROM start address */ +static const uint8_t CMD_MS56XX_PROM = 0xA0; + +/* write to one of these addresses to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR256 0x40 +#define ADDR_CMD_CONVERT_D1_OSR512 0x42 +#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 +#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 +#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 + +/* write to one of these addresses to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR256 0x50 +#define ADDR_CMD_CONVERT_D2_OSR512 0x52 +#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 +#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 +#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* use an OSR of 1024 to reduce the self-heating effect of the @@ -45,149 +45,36 @@ extern const AP_HAL::HAL& hal; are quite sensitive to this effect and that reducing the OSR can make a big difference */ -#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 -#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 - -// SPI Device ////////////////////////////////////////////////////////////////// - -AP_SerialBus_SPI::AP_SerialBus_SPI(enum AP_HAL::SPIDeviceType 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) { - AP_HAL::panic("did not get valid SPI device driver!"); - } - _spi_sem = _spi->get_semaphore(); - if (_spi_sem == NULL) { - AP_HAL::panic("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) { - AP_HAL::panic("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, ®) == 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(); -} +static const uint8_t ADDR_CMD_CONVERT_PRESSURE = ADDR_CMD_CONVERT_D1_OSR1024; +static const uint8_t ADDR_CMD_CONVERT_TEMPERATURE = ADDR_CMD_CONVERT_D2_OSR1024; /* constructor */ -AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) +AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer) : AP_Baro_Backend(baro) - , _serial(serial) + , _dev(std::move(dev)) , _use_timer(use_timer) { } void AP_Baro_MS56XX::_init() { + if (!_dev) { + AP_HAL::panic("AP_Baro_MS56XX: failed to use device"); + } + _instance = _frontend.register_sensor(); - _serial->init(); // we need to suspend timers to prevent other SPI drivers grabbing // the bus while we do the long initialisation hal.scheduler->suspend_timer_procs(); - if (!_serial->sem_take_blocking()){ + if (!_dev->get_semaphore()->take(10)) { AP_HAL::panic("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"); } - _serial->write(CMD_MS5611_RESET); + _dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0); hal.scheduler->delay(4); uint16_t prom[8]; @@ -203,8 +90,8 @@ void AP_Baro_MS56XX::_init() _c5 = prom[5]; _c6 = prom[6]; - // Send a command to read Temp first - _serial->write(ADDR_CMD_CONVERT_D2); + // Send a command to read temperature first + _dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0); _last_timer = AP_HAL::micros(); _state = 0; @@ -213,7 +100,7 @@ void AP_Baro_MS56XX::_init() _d1_count = 0; _d2_count = 0; - _serial->sem_give(); + _dev->get_semaphore()->give(); hal.scheduler->resume_timer_procs(); @@ -251,6 +138,27 @@ static uint16_t crc4(uint16_t *data) return (n_rem >> 12) & 0xF; } +uint16_t AP_Baro_MS56XX::_read_prom_word(uint8_t word) +{ + const uint8_t reg = CMD_MS56XX_PROM + (word << 1); + uint8_t val[2]; + + if (!_dev->transfer(®, 1, val, 2)) { + return 0; + } + return (val[0] << 8) | val[1]; +} + +uint32_t AP_Baro_MS56XX::_read_adc() +{ + uint8_t val[3]; + + if (!_dev->transfer(&CMD_MS56XX_READ_ADC, 1, val, 3)) { + return 0; + } + return (val[0] << 16) | (val[1] << 8) | val[2]; +} + bool AP_Baro_MS56XX::_read_prom(uint16_t prom[8]) { /* @@ -261,7 +169,7 @@ bool AP_Baro_MS56XX::_read_prom(uint16_t prom[8]) * CRC field must me removed for CRC-4 calculation. */ for (uint8_t i = 0; i < 8; i++) { - prom[i] = _serial->read_16bits(CMD_MS56XX_PROM + (i << 1)); + prom[i] = _read_prom_word(i); } /* save the read crc */ @@ -284,7 +192,7 @@ bool AP_Baro_MS5637::_read_prom(uint16_t prom[8]) * calculation. */ for (uint8_t i = 0; i < 7; i++) { - prom[i] = _serial->read_16bits(CMD_MS56XX_PROM + (i << 1)); + prom[i] = _read_prom_word(i); } prom[7] = 0; @@ -311,13 +219,13 @@ void AP_Baro_MS56XX::_timer(void) return; } - if (!_serial->sem_take_nonblocking()) { + if (!_dev->get_semaphore()->take_nonblocking()) { return; } if (_state == 0) { // On state 0 we read temp - uint32_t d2 = _serial->read_24bits(0); + uint32_t d2 = _read_adc(); if (d2 != 0) { _s_D2 += d2; _d2_count++; @@ -329,16 +237,16 @@ void AP_Baro_MS56XX::_timer(void) _d2_count = 16; } - if (_serial->write(ADDR_CMD_CONVERT_D1)) { // Command to read pressure + if (_dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0)) { _state++; } } else { /* if read fails, re-initiate a temperature read command or we are * stuck */ - _serial->write(ADDR_CMD_CONVERT_D2); + _dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0); } } else { - uint32_t d1 = _serial->read_24bits(0); + uint32_t d1 = _read_adc(); if (d1 != 0) { // occasional zero values have been seen on the PXF // board. These may be SPI errors, but safest to ignore @@ -355,23 +263,23 @@ void AP_Baro_MS56XX::_timer(void) _updated = true; if (_state == 4) { - if (_serial->write(ADDR_CMD_CONVERT_D2)) { // Command to read temperature + if (_dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0)) { _state = 0; } } else { - if (_serial->write(ADDR_CMD_CONVERT_D1)) { // Command to read pressure + if (_dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0)) { _state++; } } } else { /* if read fails, re-initiate a pressure read command or we are * stuck */ - _serial->write(ADDR_CMD_CONVERT_D1); + _dev->transfer(&ADDR_CMD_CONVERT_PRESSURE, 1, nullptr, 0); } } _last_timer = AP_HAL::micros(); - _serial->sem_give(); + _dev->get_semaphore()->give(); } void AP_Baro_MS56XX::update() @@ -408,8 +316,8 @@ void AP_Baro_MS56XX::update() } /* MS5611 class */ -AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) - : AP_Baro_MS56XX(baro, serial, use_timer) +AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer) + : AP_Baro_MS56XX(baro, std::move(dev), use_timer) { _init(); } @@ -450,8 +358,8 @@ void AP_Baro_MS5611::_calculate() } /* MS5607 Class */ -AP_Baro_MS5607::AP_Baro_MS5607(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) - : AP_Baro_MS56XX(baro, serial, use_timer) +AP_Baro_MS5607::AP_Baro_MS5607(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer) + : AP_Baro_MS56XX(baro, std::move(dev), use_timer) { _init(); } @@ -491,9 +399,9 @@ void AP_Baro_MS5607::_calculate() _copy_to_frontend(_instance, pressure, temperature); } -/* MS563 Class */ -AP_Baro_MS5637::AP_Baro_MS5637(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) - : AP_Baro_MS56XX(baro, serial, use_timer) +/* MS5637 Class */ +AP_Baro_MS5637::AP_Baro_MS5637(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer) + : AP_Baro_MS56XX(baro, std::move(dev), use_timer) { _init(); } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 036c331c82..a438d86aa3 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -4,72 +4,7 @@ #include "AP_Baro_Backend.h" #include - -/** Abstract serial bus device driver for I2C/SPI. */ -class AP_SerialBus -{ -public: - /** Initialize the driver. */ - virtual void init() = 0; - - /** Read a 16-bit value from register "reg". */ - virtual uint16_t read_16bits(uint8_t reg) = 0; - - /** Read a 24-bit value */ - virtual uint32_t read_24bits(uint8_t reg) = 0; - - /** Write to a register with no data. */ - virtual bool write(uint8_t reg) = 0; - - /** Acquire the internal semaphore for this device. - * take_nonblocking should be used from the timer process, - * take_blocking from synchronous code (i.e. init) */ - virtual bool sem_take_nonblocking() = 0; - virtual bool sem_take_blocking() = 0; - - /** Release the internal semaphore for this device. */ - virtual void sem_give() = 0; -}; - -/** SPI serial device. */ -class AP_SerialBus_SPI : public AP_SerialBus -{ -public: - AP_SerialBus_SPI(enum AP_HAL::SPIDeviceType device, enum AP_HAL::SPIDeviceDriver::bus_speed speed); - void init(); - uint16_t read_16bits(uint8_t reg); - uint32_t read_24bits(uint8_t reg); - uint32_t read_adc(uint8_t reg); - bool write(uint8_t reg); - bool sem_take_nonblocking(); - bool sem_take_blocking(); - void sem_give(); - -private: - enum AP_HAL::SPIDeviceType _device; - enum AP_HAL::SPIDeviceDriver::bus_speed _speed; - AP_HAL::SPIDeviceDriver *_spi; - AP_HAL::Semaphore *_spi_sem; -}; - -/** I2C serial device. */ -class AP_SerialBus_I2C : public AP_SerialBus -{ -public: - 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); - bool write(uint8_t reg); - bool sem_take_nonblocking(); - bool sem_take_blocking(); - void sem_give(); - -private: - AP_HAL::I2CDriver *_i2c; - uint8_t _addr; - AP_HAL::Semaphore *_i2c_sem; -}; +#include class AP_Baro_MS56XX : public AP_Baro_Backend { @@ -78,14 +13,18 @@ public: void accumulate(); protected: - AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_timer); + AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer); void _init(); virtual void _calculate() = 0; virtual bool _read_prom(uint16_t prom[8]); + + uint16_t _read_prom_word(uint8_t word); + uint32_t _read_adc(); + void _timer(); - AP_SerialBus *_serial; + AP_HAL::OwnPtr _dev; /* Asynchronous state: */ volatile bool _updated; @@ -107,24 +46,24 @@ protected: class AP_Baro_MS5611 : public AP_Baro_MS56XX { public: - AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer); -private: + AP_Baro_MS5611(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer); +protected: void _calculate(); }; class AP_Baro_MS5607 : public AP_Baro_MS56XX { public: - AP_Baro_MS5607(AP_Baro &baro, AP_SerialBus *serial, bool use_timer); -private: + AP_Baro_MS5607(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer); +protected: void _calculate(); }; class AP_Baro_MS5637 : public AP_Baro_MS56XX { public: - AP_Baro_MS5637(AP_Baro &baro, AP_SerialBus *serial, bool use_timer); -private: + AP_Baro_MS5637(AP_Baro &baro, AP_HAL::OwnPtr dev, bool use_timer); +protected: void _calculate(); bool _read_prom(uint16_t prom[8]) override; }; diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index c844dad924..43dfabddb6 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -72,9 +72,9 @@ // barometer driver types #define HAL_BARO_BMP085 1 -#define HAL_BARO_MS5611 2 +#define HAL_BARO_MS5611_I2C 2 #define HAL_BARO_MS5611_SPI 3 -#define HAL_BARO_MS5607 4 +#define HAL_BARO_MS5607_I2C 4 #define HAL_BARO_PX4 5 #define HAL_BARO_HIL 6 #define HAL_BARO_VRBRAIN 7 @@ -202,6 +202,7 @@ #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_INS_DEFAULT HAL_INS_MPU9250 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI +#define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 #define HAL_GPIO_A_LED_PIN 61 #define HAL_GPIO_B_LED_PIN 48 @@ -217,9 +218,9 @@ #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C #define HAL_INS_AK8963_I2C_BUS 1 #define HAL_COMPASS_AK8963_I2C_ADDR 0x0d +#define HAL_BARO_DEFAULT HAL_BARO_MS5607_I2C #define HAL_BARO_MS5607_I2C_BUS 1 #define HAL_BARO_MS5607_I2C_ADDR 0x77 -#define HAL_BARO_DEFAULT HAL_BARO_MS5607 #define HAL_UTILS_HEAT HAL_LINUX_HEAT_PWM #define HAL_LINUX_HEAT_PWM_NUM 6 #define HAL_LINUX_HEAT_KP 20000 @@ -248,9 +249,10 @@ #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_INS_DEFAULT HAL_INS_MPU60XX_SPI -#define HAL_BARO_DEFAULT HAL_BARO_MS5611 -#define HAL_BARO_MS5611_I2C_BUS 1 +#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C +#define HAL_BARO_MS5611_I2C_BUS 10 #define HAL_BARO_MS5611_I2C_ADDR 0x77 +#define HAL_BARO_MS5611_USE_TIMER true #define HAL_COMPASS_DEFAULT HAL_COMPASS_HMC5843_MPU6000 #define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0" #define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320 @@ -272,9 +274,10 @@ #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_INS_DEFAULT HAL_INS_MPU9250 -#define HAL_BARO_DEFAULT HAL_BARO_MS5611 -#define HAL_BARO_MS5611_I2C_BUS 0 +#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C +#define HAL_BARO_MS5611_I2C_BUS 1 #define HAL_BARO_MS5611_I2C_ADDR 0x77 +#define HAL_BARO_MS5611_USE_TIMER false #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 #define HAL_GPIO_A_LED_PIN 16 #define HAL_GPIO_B_LED_PIN 16 @@ -286,12 +289,14 @@ #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_INS_DEFAULT HAL_INS_RASPILOT #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI +#define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_RASPILOT #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_INS_DEFAULT HAL_INS_MPU9250 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI +#define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 #define HAL_GPIO_A_LED_PIN 24 #define HAL_GPIO_B_LED_PIN 25 @@ -310,6 +315,7 @@ #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" #define HAL_INS_DEFAULT HAL_INS_MPU9250 #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI +#define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 #define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0" #define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320 @@ -334,9 +340,10 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" -#define HAL_BARO_DEFAULT HAL_BARO_MS5611 -#define HAL_BARO_MS5611_I2C_BUS 0 +#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C +#define HAL_BARO_MS5611_I2C_BUS 1 #define HAL_BARO_MS5611_I2C_ADDR 0x77 +#define HAL_BARO_MS5611_USE_TIMER true #define HAL_INS_DEFAULT HAL_INS_BH #define HAL_INS_MPU60XX_I2C_ADDR 0x69 #define HAL_COMPASS_DEFAULT HAL_COMPASS_BH