Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: support for MSP barometer

Browse Source
This commit is contained in:
yaapu 2020-09-04 00:09:52 +02:00 committed by Andrew Tridgell
parent 94670879f8
commit ea53ce2a3f
5 changed files with 124 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_Baro/AP_Baro.cpp
View File

@ -46,6 +46,7 @@
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include "AP_Baro_UAVCAN.h"
#endif
#include "AP_Baro_MSP.h"
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_AHRS/AP_AHRS.h>
@ -162,12 +163,12 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
// @Increment: 1
AP_GROUPINFO("FLTR_RNG", 13, AP_Baro, _filter_range, HAL_BARO_FILTER_DEFAULT),
#ifdef HAL_PROBE_EXTERNAL_I2C_BAROS
#if defined(HAL_PROBE_EXTERNAL_I2C_BAROS) || defined(HAL_MSP_BARO_ENABLED)
// @Param: PROBE_EXT
// @DisplayName: External barometers to probe
// @Description: This sets which types of external i2c barometer to look for. It is a bitmask of barometer types. The I2C buses to probe is based on GND_EXT_BUS. If GND_EXT_BUS is -1 then it will probe all external buses, otherwise it will probe just the bus number given in GND_EXT_BUS.
// @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06
// @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388,2048:SPL06
// @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06,12:MSP
// @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388,2048:SPL06,4096:MSP
// @User: Advanced
AP_GROUPINFO("PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT),
#endif
@ -493,6 +494,8 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
*/
void AP_Baro::init(void)
{
init_done = true;
// ensure that there isn't a previous ground temperature saved
if (!is_zero(_user_ground_temperature)) {
_user_ground_temperature.set_and_save(0.0f);
@ -641,6 +644,18 @@ void AP_Baro::init(void)
_probe_i2c_barometers();
#endif
#if HAL_MSP_BARO_ENABLED
if ((_baro_probe_ext.get() & PROBE_MSP) && msp_instance_mask == 0) {
// allow for late addition of MSP sensor
msp_instance_mask |= 1;
}
for (uint8_t i=0; i<8; i++) {
if (msp_instance_mask & (1U<<i)) {
ADD_BACKEND(new AP_Baro_MSP(*this, i));
}
}
#endif
#if !defined(HAL_BARO_ALLOW_INIT_NO_BARO) // most boards requires external baro
if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
@ -901,6 +916,26 @@ void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction)
}
}
#if HAL_MSP_BARO_ENABLED
/*
handle MSP barometer data
*/
void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
{
if (pkt.instance > 7) {
return;
}
if (!init_done) {
msp_instance_mask |= 1U<<pkt.instance;
} else if (msp_instance_mask != 0) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->handle_msp(pkt);
}
}
}
#endif
namespace AP {
AP_Baro &baro()

16
libraries/AP_Baro/AP_Baro.h
View File

@ -4,6 +4,11 @@
#include <AP_Param/AP_Param.h>
#include <Filter/Filter.h>
#include <Filter/DerivativeFilter.h>
#include <AP_MSP/msp.h>
#ifndef HAL_MSP_BARO_ENABLED
#define HAL_MSP_BARO_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES
#endif
// maximum number of sensor instances
#define BARO_MAX_INSTANCES 3
@ -189,6 +194,10 @@ public:
return _rsem;
}
#if HAL_MSP_BARO_ENABLED
void handle_msp(const MSP::msp_baro_data_message_t &pkt);
#endif
private:
// singleton
static AP_Baro *_singleton;
@ -205,6 +214,12 @@ private:
uint32_t _log_baro_bit = -1;
bool init_done;
#if HAL_MSP_BARO_ENABLED
uint8_t msp_instance_mask;
#endif
// bitmask values for GND_PROBE_EXT
enum {
PROBE_BMP085=(1<<0),
@ -219,6 +234,7 @@ private:
PROBE_MS5837=(1<<9),
PROBE_BMP388=(1<<10),
PROBE_SPL06 =(1<<11),
PROBE_MSP =(1<<12),
};
struct sensor {

5
libraries/AP_Baro/AP_Baro_Backend.h
View File

@ -24,6 +24,10 @@ public:
bool pressure_ok(float press);
uint32_t get_error_count() const { return _error_count; }
#if HAL_MSP_BARO_ENABLED
virtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {}
#endif
/*
device driver IDs. These are used to fill in the devtype field
of the device ID, which shows up as GND_BARO_ID* parameters to
@ -43,6 +47,7 @@ public:
DEVTYPE_BARO_MS5611 = 0x0B,
DEVTYPE_BARO_SPL06 = 0x0C,
DEVTYPE_BARO_UAVCAN = 0x0D,
DEVTYPE_BARO_MSP = 0x0E,
};
protected:

36
libraries/AP_Baro/AP_Baro_MSP.cpp Normal file
View File

@ -0,0 +1,36 @@
#include "AP_Baro_MSP.h"
#if HAL_MSP_BARO_ENABLED
AP_Baro_MSP::AP_Baro_MSP(AP_Baro &baro, uint8_t _msp_instance) :
AP_Baro_Backend(baro)
{
msp_instance = _msp_instance;
instance = _frontend.register_sensor();
set_bus_id(instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_MSP,0,msp_instance,0));
}
// Read the sensor
void AP_Baro_MSP::update(void)
{
if (count) {
WITH_SEMAPHORE(_sem);
_copy_to_frontend(instance, sum_pressure/count, sum_temp/count);
sum_pressure = sum_temp = 0;
count = 0;
}
}
void AP_Baro_MSP::handle_msp(const MSP::msp_baro_data_message_t &pkt)
{
if (pkt.instance != msp_instance) {
// not for us
return;
}
WITH_SEMAPHORE(_sem);
sum_pressure += pkt.pressure_pa;
sum_temp += pkt.temp*0.01;
count++;
}
#endif // HAL_MSP_BARO_ENABLED

27
libraries/AP_Baro/AP_Baro_MSP.h Normal file
View File

@ -0,0 +1,27 @@
/*
MSP backend barometer
*/
#pragma once
#include "AP_Baro_Backend.h"
#define MOVING_AVERAGE_WEIGHT 0.20f // a 5 samples moving average
#if HAL_MSP_BARO_ENABLED
class AP_Baro_MSP : public AP_Baro_Backend
{
public:
AP_Baro_MSP(AP_Baro &baro, uint8_t msp_instance);
void update(void) override;
void handle_msp(const MSP::msp_baro_data_message_t &pkt) override;
private:
uint8_t instance;
uint8_t msp_instance;
float sum_pressure;
float sum_temp;
uint16_t count;
};
#endif // HAL_MSP_BARO_ENABLED