AP_Baro: Fix GCS DPS310 HWID issue

DPS280/DPS310 use the same driver(AP_Baro_DPS280.cpp/h), but it is necessary to distinguish between the two.
This commit is contained in:
daniel.li 2023-09-24 16:27:51 +08:00 committed by Peter Barker
parent 93da99bea2
commit ec601684e6
2 changed files with 8 additions and 5 deletions

View File

@ -60,7 +60,7 @@ AP_Baro_Backend *AP_Baro_DPS280::probe(AP_Baro &baro,
if (sensor) {
sensor->is_dps310 = _is_dps310;
}
if (!sensor || !sensor->init()) {
if (!sensor || !sensor->init(_is_dps310)) {
delete sensor;
return nullptr;
}
@ -153,7 +153,7 @@ void AP_Baro_DPS280::set_config_registers(void)
}
}
bool AP_Baro_DPS280::init()
bool AP_Baro_DPS280::init(bool _is_dps310)
{
if (!dev) {
return false;
@ -190,8 +190,11 @@ bool AP_Baro_DPS280::init()
set_config_registers();
instance = _frontend.register_sensor();
dev->set_device_type(DEVTYPE_BARO_DPS280);
if(_is_dps310) {
dev->set_device_type(DEVTYPE_BARO_DPS310);
} else {
dev->set_device_type(DEVTYPE_BARO_DPS280);
}
set_bus_id(instance, dev->get_bus_id());
dev->get_semaphore()->give();

View File

@ -29,7 +29,7 @@ public:
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, bool _is_dps310=false);
protected:
bool init(void);
bool init(bool _is_dps310);
bool read_calibration(void);
void timer(void);
void calculate_PT(int32_t UT, int32_t UP, float &pressure, float &temperature);