mirror of https://github.com/ArduPilot/ardupilot
224 lines
6.1 KiB
C++
224 lines
6.1 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/>.
|
|
*/
|
|
/*
|
|
FCM320 barometer driver
|
|
*/
|
|
|
|
#include "AP_Baro_FBM320.h"
|
|
|
|
#include <utility>
|
|
#include <stdio.h>
|
|
|
|
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<AP_HAL::Device> _dev)
|
|
: AP_Baro_Backend(baro)
|
|
, dev(std::move(_dev))
|
|
{
|
|
}
|
|
|
|
AP_Baro_Backend *AP_Baro_FBM320::probe(AP_Baro &baro,
|
|
AP_HAL::OwnPtr<AP_HAL::Device> _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 (pressure_ok(pressure) && _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();
|
|
}
|
|
}
|