mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
125 lines
3.3 KiB
C++
125 lines
3.3 KiB
C++
/*
|
|
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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#include "AP_Baro_AUAV.h"
|
|
|
|
#if AP_BARO_AUAV_ENABLED
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
AP_Baro_AUAV::AP_Baro_AUAV(AP_Baro &baro, AP_HAL::Device *_dev)
|
|
: AP_Baro_Backend(baro)
|
|
, dev(_dev)
|
|
{
|
|
}
|
|
|
|
AP_Baro_Backend *AP_Baro_AUAV::probe(AP_Baro &baro, AP_HAL::Device &_dev)
|
|
{
|
|
AP_Baro_AUAV *sensor = NEW_NOTHROW AP_Baro_AUAV(baro, &_dev);
|
|
if (!sensor || !sensor->init()) {
|
|
delete sensor;
|
|
return nullptr;
|
|
}
|
|
return sensor;
|
|
}
|
|
|
|
bool AP_Baro_AUAV::init()
|
|
{
|
|
if (!dev) {
|
|
return false;
|
|
}
|
|
|
|
{
|
|
// Take semaphore for i2c functions
|
|
WITH_SEMAPHORE(dev->get_semaphore());
|
|
dev->set_retries(10);
|
|
|
|
// Request a measurement
|
|
if (!sensor.measure()) {
|
|
return false;
|
|
}
|
|
hal.scheduler->delay(40);
|
|
|
|
// Test read and discard result as the compensation coefficients are not configured
|
|
float PComp, temperature;
|
|
if (sensor.collect(PComp, temperature) != AUAV_Pressure_sensor::Status::Normal) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Register sensor and set dev-id
|
|
instance = _frontend.register_sensor();
|
|
dev->set_device_type(DEVTYPE_BARO_AUAV);
|
|
set_bus_id(instance, dev->get_bus_id());
|
|
|
|
dev->register_periodic_callback(40000,
|
|
FUNCTOR_BIND_MEMBER(&AP_Baro_AUAV::timer, void));
|
|
|
|
return true;
|
|
}
|
|
|
|
// accumulate a new sensor reading
|
|
void AP_Baro_AUAV::timer(void)
|
|
{
|
|
if (sensor.stage != AUAV_Pressure_sensor::CoefficientStage::Done) {
|
|
sensor.read_coefficients();
|
|
return;
|
|
}
|
|
|
|
if (measurement_requested) {
|
|
// Read in result of last measurement
|
|
float Pcomp, temp_C;
|
|
switch (sensor.collect(Pcomp, temp_C)) {
|
|
case AUAV_Pressure_sensor::Status::Normal: {
|
|
// Convert to correct units
|
|
const float pressure_mbar = 250 + (1.25 * ((Pcomp-1677722)/16777216.0) * 1000.0);
|
|
{
|
|
WITH_SEMAPHORE(_sem);
|
|
pressure_sum += pressure_mbar * 100;
|
|
temperature_sum += temp_C;
|
|
count++;
|
|
}
|
|
break;
|
|
}
|
|
case AUAV_Pressure_sensor::Status::Busy:
|
|
// Don't request a new measurement
|
|
return;
|
|
|
|
case AUAV_Pressure_sensor::Status::Fault:
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Request a new measurement
|
|
measurement_requested = sensor.measure();
|
|
}
|
|
|
|
// transfer data to the frontend
|
|
void AP_Baro_AUAV::update(void)
|
|
{
|
|
if (count == 0) {
|
|
return;
|
|
}
|
|
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
_copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
|
|
|
|
pressure_sum = 0;
|
|
temperature_sum = 0;
|
|
count = 0;
|
|
}
|
|
|
|
#endif // AP_BARO_AUAV_ENABLED
|