diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 967d87dc62..cbff73b282 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -36,6 +36,7 @@
#include "AP_Baro_MS5611.h"
#include "AP_Baro_ICM20789.h"
#include "AP_Baro_LPS2XH.h"
+#include "AP_Baro_FBM320.h"
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include "AP_Baro_qflight.h"
#endif
@@ -513,6 +514,9 @@ void AP_Baro::init(void)
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5637_I2C_BUS, HAL_BARO_MS5637_I2C_ADDR)),
AP_Baro_MS56XX::BARO_MS5637));
+#elif HAL_BARO_DEFAULT == HAL_BARO_FBM320_I2C
+ ADD_BACKEND(AP_Baro_FBM320::probe(*this,
+ std::move(hal.i2c_mgr->get_device(HAL_BARO_FBM320_I2C_BUS, HAL_BARO_FBM320_I2C_ADDR))));
#elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT
drivers[0] = new AP_Baro_QFLIGHT(*this);
_num_drivers = 1;
diff --git a/libraries/AP_Baro/AP_Baro_FBM320.cpp b/libraries/AP_Baro/AP_Baro_FBM320.cpp
new file mode 100644
index 0000000000..7f028a0a6a
--- /dev/null
+++ b/libraries/AP_Baro/AP_Baro_FBM320.cpp
@@ -0,0 +1,223 @@
+/*
+ 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 .
+ */
+/*
+ FCM320 barometer driver
+ */
+
+#include "AP_Baro_FBM320.h"
+
+#include
+#include
+
+extern const AP_HAL::HAL &hal;
+
+#define FBM320_REG_ID 0x6B
+#define FBM320_REG_DATA 0xF6
+#define FBM320_REG_CMD 0xF4
+
+#define FBM320_CMD_READ_T 0x2E
+#define FBM320_CMD_READ_P 0xF4
+
+#define FBM320_WHOAMI 0x42
+
+AP_Baro_FBM320::AP_Baro_FBM320(AP_Baro &baro, AP_HAL::OwnPtr _dev)
+ : AP_Baro_Backend(baro)
+ , dev(std::move(_dev))
+{
+}
+
+AP_Baro_Backend *AP_Baro_FBM320::probe(AP_Baro &baro,
+ AP_HAL::OwnPtr _dev)
+{
+ if (!_dev) {
+ return nullptr;
+ }
+
+ AP_Baro_FBM320 *sensor = new AP_Baro_FBM320(baro, std::move(_dev));
+ if (!sensor || !sensor->init()) {
+ delete sensor;
+ return nullptr;
+ }
+ return sensor;
+}
+
+/*
+ read calibration data
+ */
+bool AP_Baro_FBM320::read_calibration(void)
+{
+ uint8_t tmp[2];
+ uint16_t R[10];
+
+ for (uint8_t i=0; i<9; i++) {
+ if (!dev->read_registers(0xAA+(i*2),&tmp[0],1)) {
+ return false;
+ }
+ if (!dev->read_registers(0xAB+(i*2),&tmp[1],1)) {
+ return false;
+ }
+ R[i] = ((uint8_t)tmp[0] << 8 | tmp[1]);
+ }
+
+ if (!dev->read_registers(0xA4,&tmp[0],1)) {
+ return false;
+ }
+ if (!dev->read_registers(0xF1,&tmp[0],1)) {
+ return false;
+ }
+ R[9] = ((uint8_t)tmp[0] << 8) | tmp[1];
+
+
+ /* Use R0~R9 calculate C0~C12 of FBM320-02 */
+ calibration.C0 = R[0] >> 4;
+ calibration.C1 = ((R[1] & 0xFF00) >> 5) | (R[2] & 7);
+ calibration.C2 = ((R[1] & 0xFF) << 1) | (R[4] & 1);
+ calibration.C3 = R[2] >> 3;
+ calibration.C4 = ((uint32_t)R[3] << 2) | (R[0] & 3);
+ calibration.C5 = R[4] >> 1;
+ calibration.C6 = R[5] >> 3;
+ calibration.C7 = ((uint32_t)R[6] << 3) | (R[5] & 7);
+ calibration.C8 = R[7] >> 3;
+ calibration.C9 = R[8] >> 2;
+ calibration.C10 = ((R[9] & 0xFF00) >> 6) | (R[8] & 3);
+ calibration.C11 = R[9] & 0xFF;
+ calibration.C12 = ((R[0] & 0x0C) << 1) | (R[7] & 7);
+
+ return true;
+}
+
+bool AP_Baro_FBM320::init()
+{
+ if (!dev || !dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+ return false;
+ }
+
+ dev->set_speed(AP_HAL::Device::SPEED_HIGH);
+
+ uint8_t whoami;
+ if (!dev->read_registers(FBM320_REG_ID, &whoami, 1) ||
+ whoami != FBM320_WHOAMI) {
+ // not a FBM320
+ dev->get_semaphore()->give();
+ return false;
+ }
+ printf("FBM320 ID 0x%x\n", whoami);
+
+ if (!read_calibration()) {
+ dev->get_semaphore()->give();
+ return false;
+ }
+
+ dev->write_register(FBM320_REG_CMD, FBM320_CMD_READ_T);
+
+ instance = _frontend.register_sensor();
+
+ dev->get_semaphore()->give();
+
+ // request 50Hz update
+ dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_FBM320::timer, void));
+
+ return true;
+}
+
+/*
+ calculate corrected pressure and temperature
+ */
+void AP_Baro_FBM320::calculate_PT(int32_t UT, int32_t UP, int32_t &pressure, int32_t &temperature)
+{
+ const struct fbm320_calibration &cal = calibration;
+ int32_t DT, DT2, X01, X02, X03, X11, X12, X13, X21, X22, X23, X24, X25, X26, X31, X32, CF, PP1, PP2, PP3, PP4;
+
+ DT = ((UT - 8388608) >> 4) + (cal.C0 << 4);
+ X01 = (cal.C1 + 4459) * DT >> 1;
+ X02 = ((((cal.C2 - 256) * DT) >> 14) * DT) >> 4;
+ X03 = (((((cal.C3 * DT) >> 18) * DT) >> 18) * DT);
+
+ temperature = ((2500 << 15) - X01 - X02 - X03) >> 15;
+
+ DT2 = (X01 + X02 + X03) >> 12;
+ X11 = ((cal.C5 - 4443) * DT2);
+ X12 = (((cal.C6 * DT2) >> 16) * DT2) >> 2;
+ X13 = ((X11 + X12) >> 10) + ((cal.C4 + 120586) << 4);
+
+ X21 = ((cal.C8 + 7180) * DT2) >> 10;
+ X22 = (((cal.C9 * DT2) >> 17) * DT2) >> 12;
+ X23 = (X22 >= X21) ? (X22 - X21) : (X21 - X22);
+
+ X24 = (X23 >> 11) * (cal.C7 + 166426);
+ X25 = ((X23 & 0x7FF) * (cal.C7 + 166426)) >> 11;
+ X26 = (X21 >= X22) ? (((0 - X24 - X25) >> 11) + cal.C7 + 166426) : (((X24 + X25) >> 11) + cal.C7 + 166426);
+
+ PP1 = ((UP - 8388608) - X13) >> 3;
+ PP2 = (X26 >> 11) * PP1;
+ PP3 = ((X26 & 0x7FF) * PP1) >> 11;
+ PP4 = (PP2 + PP3) >> 10;
+
+ CF = (2097152 + cal.C12 * DT2) >> 3;
+ X31 = (((CF * cal.C10) >> 17) * PP4) >> 2;
+ X32 = (((((CF * cal.C11) >> 15) * PP4) >> 18) * PP4);
+
+ pressure = ((X31 + X32) >> 15) + PP4 + 99880;
+}
+
+// acumulate a new sensor reading
+void AP_Baro_FBM320::timer(void)
+{
+ uint8_t buf[3];
+
+ if (!dev->read_registers(0xF6, buf, sizeof(buf))) {
+ return;
+ }
+ int32_t value = ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | (uint32_t)buf[2];
+
+ if (step == 0) {
+ value_T = value;
+ } else {
+ int32_t pressure, temperature;
+ calculate_PT(value_T, value, pressure, temperature);
+ if (_sem->take_nonblocking()) {
+ pressure_sum += pressure;
+ // sum and convert to degrees
+ temperature_sum += temperature*0.01;
+ count++;
+ _sem->give();
+ }
+ }
+
+ if (step++ >= 5) {
+ dev->write_register(FBM320_REG_CMD, FBM320_CMD_READ_T);
+ step = 0;
+ } else {
+ dev->write_register(FBM320_REG_CMD, FBM320_CMD_READ_P);
+ }
+}
+
+// transfer data to the frontend
+void AP_Baro_FBM320::update(void)
+{
+ if (count != 0 && _sem->take_nonblocking()) {
+ if (count == 0) {
+ _sem->give();
+ return;
+ }
+
+ _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
+ pressure_sum = 0;
+ temperature_sum = 0;
+ count=0;
+
+ _sem->give();
+ }
+}
diff --git a/libraries/AP_Baro/AP_Baro_FBM320.h b/libraries/AP_Baro/AP_Baro_FBM320.h
new file mode 100644
index 0000000000..8104e4f0ec
--- /dev/null
+++ b/libraries/AP_Baro/AP_Baro_FBM320.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include
+#include
+#include
+
+#include "AP_Baro_Backend.h"
+
+class AP_Baro_FBM320 : public AP_Baro_Backend {
+public:
+ AP_Baro_FBM320(AP_Baro &baro, AP_HAL::OwnPtr dev);
+
+ /* AP_Baro public interface: */
+ void update();
+
+ static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev);
+
+private:
+ bool init(void);
+ bool read_calibration(void);
+ void timer(void);
+ void calculate_PT(int32_t UT, int32_t UP, int32_t &pressure, int32_t &temperature);
+
+ AP_HAL::OwnPtr dev;
+
+ uint8_t instance;
+
+ uint32_t count;
+ float pressure_sum;
+ float temperature_sum;
+ uint8_t step;
+
+ int32_t value_T;
+
+ // Internal calibration registers
+ struct fbm320_calibration {
+ uint16_t C0, C1, C2, C3, C6, C8, C9, C10, C11, C12;
+ uint32_t C4, C5, C7;
+ } calibration;
+};