CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
Views: 1798
1
/*
2
This program is free software: you can redistribute it and/or modify
3
it under the terms of the GNU General Public License as published by
4
the Free Software Foundation, either version 3 of the License, or
5
(at your option) any later version.
6
7
This program is distributed in the hope that it will be useful,
8
but WITHOUT ANY WARRANTY; without even the implied warranty of
9
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10
GNU General Public License for more details.
11
12
You should have received a copy of the GNU General Public License
13
along with this program. If not, see <http://www.gnu.org/licenses/>.
14
*/
15
/*
16
driver for DLVR differential airspeed sensor
17
https://www.allsensors.com/products/DLVR-L01D
18
*/
19
20
#include "AP_Airspeed_DLVR.h"
21
22
#if AP_AIRSPEED_DLVR_ENABLED
23
24
#include <AP_Math/AP_Math.h>
25
26
extern const AP_HAL::HAL &hal;
27
28
#define DLVR_I2C_ADDR 0x28
29
30
#ifdef DLVR_DEBUGGING
31
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
32
#else
33
# define Debug(fmt, args ...)
34
#endif
35
36
37
AP_Airspeed_DLVR::AP_Airspeed_DLVR(AP_Airspeed &_frontend, uint8_t _instance, const float _range_inH2O) :
38
AP_Airspeed_Backend(_frontend, _instance),
39
range_inH2O(_range_inH2O)
40
{}
41
42
/*
43
probe for a sensor on a given i2c address
44
*/
45
AP_Airspeed_Backend *AP_Airspeed_DLVR::probe(AP_Airspeed &_frontend,
46
uint8_t _instance,
47
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev,
48
const float _range_inH2O)
49
{
50
if (!_dev) {
51
return nullptr;
52
}
53
AP_Airspeed_DLVR *sensor = NEW_NOTHROW AP_Airspeed_DLVR(_frontend, _instance, _range_inH2O);
54
if (!sensor) {
55
return nullptr;
56
}
57
sensor->dev = std::move(_dev);
58
sensor->setup();
59
return sensor;
60
}
61
62
// initialise the sensor
63
void AP_Airspeed_DLVR::setup()
64
{
65
WITH_SEMAPHORE(dev->get_semaphore());
66
dev->set_speed(AP_HAL::Device::SPEED_LOW);
67
dev->set_retries(2);
68
dev->set_device_type(uint8_t(DevType::DLVR));
69
set_bus_id(dev->get_bus_id());
70
71
dev->register_periodic_callback(1000000UL/50U,
72
FUNCTOR_BIND_MEMBER(&AP_Airspeed_DLVR::timer, void));
73
}
74
75
// probe and initialise the sensor
76
bool AP_Airspeed_DLVR::init()
77
{
78
dev = hal.i2c_mgr->get_device(get_bus(), DLVR_I2C_ADDR);
79
if (!dev) {
80
return false;
81
}
82
setup();
83
return true;
84
}
85
86
#define STATUS_SHIFT 30
87
#define TEMPERATURE_SHIFT 5
88
#define TEMPERATURE_MASK ((1 << 11) - 1)
89
#define PRESSURE_SHIFT 16
90
#define PRESSURE_MASK ((1 << 14) - 1)
91
92
#define DLVR_OFFSET 8192.0f
93
#define DLVR_SCALE 16384.0f
94
95
// 50Hz timer
96
void AP_Airspeed_DLVR::timer()
97
{
98
uint8_t raw_bytes[4];
99
if (!dev->read((uint8_t *)&raw_bytes, sizeof(raw_bytes))) {
100
return;
101
}
102
103
uint32_t data = (raw_bytes[0] << 24) |
104
(raw_bytes[1] << 16) |
105
(raw_bytes[2] << 8) |
106
raw_bytes[3];
107
108
if ((data >> STATUS_SHIFT)) {
109
// anything other then 00 in the status bits is an error
110
Debug("DLVR: Bad status read %u", (unsigned int)(data >> STATUS_SHIFT));
111
return;
112
}
113
114
uint32_t pres_raw = (data >> PRESSURE_SHIFT) & PRESSURE_MASK;
115
uint32_t temp_raw = (data >> TEMPERATURE_SHIFT) & TEMPERATURE_MASK;
116
117
float press_h2o = 1.25f * 2.0f * range_inH2O * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);
118
119
if ((press_h2o > range_inH2O) || (press_h2o < -range_inH2O)) {
120
Debug("DLVR: Out of range pressure %f", press_h2o);
121
return;
122
}
123
124
float temp = temp_raw * (200.0f / 2047.0f) - 50.0f;
125
126
WITH_SEMAPHORE(sem);
127
128
const uint32_t now = AP_HAL::millis();
129
130
#pragma GCC diagnostic push
131
#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as we are only worried about the case where we had never read
132
if ((temperature != 0) && (fabsf(temperature - temp) > 10) && ((now - last_sample_time_ms) < 2000)) {
133
Debug("DLVR: Temperature swing %f", temp);
134
return;
135
}
136
#pragma GCC diagnostic pop
137
138
pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;
139
temperature_sum += temp;
140
press_count++;
141
temp_count++;
142
last_sample_time_ms = now;
143
}
144
145
// return the current differential_pressure in Pascal
146
bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)
147
{
148
WITH_SEMAPHORE(sem);
149
150
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
151
return false;
152
}
153
154
if (press_count > 0) {
155
pressure = pressure_sum / press_count;
156
press_count = 0;
157
pressure_sum = 0;
158
}
159
160
_pressure = pressure;
161
return true;
162
}
163
164
// return the current temperature in degrees C, if available
165
bool AP_Airspeed_DLVR::get_temperature(float &_temperature)
166
{
167
WITH_SEMAPHORE(sem);
168
169
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
170
return false;
171
}
172
173
if (temp_count > 0) {
174
temperature = temperature_sum / temp_count;
175
temp_count = 0;
176
temperature_sum = 0;
177
}
178
179
_temperature = temperature;
180
return true;
181
}
182
183
#endif
184
185