Line data Source code
1 : #pragma once
2 :
3 : #include "AP_Airspeed_config.h"
4 :
5 : #if AP_AIRSPEED_ENABLED
6 :
7 : #include <AP_Param/AP_Param.h>
8 : #include <AP_Math/AP_Math.h>
9 :
10 : #if AP_AIRSPEED_MSP_ENABLED
11 : #include <AP_MSP/msp.h>
12 : #endif
13 : #if AP_AIRSPEED_EXTERNAL_ENABLED
14 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
15 : #endif
16 :
17 : class AP_Airspeed_Backend;
18 :
19 : class AP_Airspeed_Params {
20 : public:
21 : // Constructor
22 : AP_Airspeed_Params(void);
23 :
24 : // parameters for each instance
25 : AP_Int32 bus_id;
26 : #ifndef HAL_BUILD_AP_PERIPH
27 : AP_Float offset;
28 : AP_Float ratio;
29 : #endif
30 : AP_Float psi_range;
31 : #ifndef HAL_BUILD_AP_PERIPH
32 : AP_Int8 use;
33 : AP_Int8 pin;
34 :
35 : enum class SkipCalType : int8_t {
36 : // Do not skip boot calibration, this is the default
37 : None = 0,
38 :
39 : // Skip boot calibration, use saved offset, no calibration is required (but can be performed manually)
40 : NoCalRequired = 1,
41 :
42 : // Skip boot calibration, require manual calibration once per boot
43 : SkipBootCal = 2,
44 : };
45 : AP_Enum<SkipCalType> skip_cal;
46 :
47 : AP_Int8 tube_order;
48 : #endif
49 : AP_Int8 type;
50 : AP_Int8 bus;
51 : #if AP_AIRSPEED_AUTOCAL_ENABLE
52 : AP_Int8 autocal;
53 : #endif
54 :
55 : static const struct AP_Param::GroupInfo var_info[];
56 : };
57 :
58 :
59 : class Airspeed_Calibration {
60 : public:
61 : friend class AP_Airspeed;
62 : // constructor
63 : Airspeed_Calibration();
64 :
65 : // initialise the calibration
66 : void init(float initial_ratio);
67 :
68 : // take current airspeed in m/s and ground speed vector and return
69 : // new scaling factor
70 : float update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal);
71 :
72 : private:
73 : // state of kalman filter for airspeed ratio estimation
74 : Matrix3f P; // covariance matrix
75 : const float Q0; // process noise matrix top left and middle element
76 : const float Q1; // process noise matrix bottom right element
77 : Vector3f state; // state vector
78 : };
79 :
80 : class AP_Airspeed
81 : {
82 : public:
83 : friend class AP_Airspeed_Backend;
84 :
85 : // constructor
86 : AP_Airspeed();
87 :
88 : void set_fixedwing_parameters(const class AP_FixedWing *_fixed_wing_parameters);
89 :
90 : void init(void);
91 : void allocate();
92 :
93 :
94 : // indicate which bit in LOG_BITMASK indicates we should log airspeed readings
95 2061 : void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; }
96 :
97 : #if AP_AIRSPEED_AUTOCAL_ENABLE
98 : // inflight ratio calibration
99 0 : void set_calibration_enabled(bool enable) {calibration_enabled = enable;}
100 : #endif //AP_AIRSPEED_AUTOCAL_ENABLE
101 :
102 : // read the analog source and update airspeed
103 : void update(void);
104 :
105 : // calibrate the airspeed. This must be called on startup if the
106 : // altitude/climb_rate/acceleration interfaces are ever used
107 : void calibrate(bool in_startup);
108 :
109 : // return the current airspeed in m/s
110 : float get_airspeed(uint8_t i) const;
111 740414 : float get_airspeed(void) const { return get_airspeed(primary); }
112 :
113 : // return the unfiltered airspeed in m/s
114 : float get_raw_airspeed(uint8_t i) const;
115 0 : float get_raw_airspeed(void) const { return get_raw_airspeed(primary); }
116 :
117 : // return the current airspeed ratio (dimensionless)
118 0 : float get_airspeed_ratio(uint8_t i) const {
119 : #ifndef HAL_BUILD_AP_PERIPH
120 0 : return param[i].ratio;
121 : #else
122 : return 0.0;
123 : #endif
124 : }
125 : float get_airspeed_ratio(void) const { return get_airspeed_ratio(primary); }
126 :
127 : // get temperature if available
128 : bool get_temperature(uint8_t i, float &temperature) const;
129 11651 : bool get_temperature(float &temperature) const { return get_temperature(primary, temperature); }
130 :
131 : // set the airspeed ratio (dimensionless)
132 : #ifndef HAL_BUILD_AP_PERIPH
133 : void set_airspeed_ratio(uint8_t i, float ratio) {
134 : param[i].ratio.set(ratio);
135 : }
136 : void set_airspeed_ratio(float ratio) { set_airspeed_ratio(primary, ratio); }
137 : #endif
138 :
139 : // return true if airspeed is enabled, and airspeed use is set
140 : bool use(uint8_t i) const;
141 1332936 : bool use(void) const { return use(primary); }
142 :
143 : // force disabling of all airspeed sensors
144 13 : void force_disable_use(bool value) {
145 13 : _force_disable_use = value;
146 13 : }
147 :
148 : // return true if airspeed is enabled
149 : bool enabled(uint8_t i) const;
150 2092492 : bool enabled(void) const { return enabled(primary); }
151 :
152 : // return the differential pressure in Pascal for the last airspeed reading
153 : float get_differential_pressure(uint8_t i) const;
154 0 : float get_differential_pressure(void) const { return get_differential_pressure(primary); }
155 :
156 : // update airspeed ratio calibration
157 : void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
158 :
159 : // return health status of sensor
160 : bool healthy(uint8_t i) const;
161 1451406 : bool healthy(void) const { return healthy(primary); }
162 :
163 : // return true if all enabled sensors are healthy
164 : bool all_healthy(void) const;
165 :
166 : // return time in ms of last update
167 17219106 : uint32_t last_update_ms(uint8_t i) const { return state[i].last_update_ms; }
168 : uint32_t last_update_ms(void) const { return last_update_ms(primary); }
169 :
170 : #if AP_AIRSPEED_HYGROMETER_ENABLE
171 : bool get_hygrometer(uint8_t i, uint32_t &last_sample_ms, float &temperature, float &humidity) const;
172 : #endif
173 :
174 : static const struct AP_Param::GroupInfo var_info[];
175 :
176 : enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE = 0,
177 : PITOT_TUBE_ORDER_NEGATIVE = 1,
178 : PITOT_TUBE_ORDER_AUTO = 2 };
179 :
180 : enum OptionsMask {
181 : ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE = (1<<0), // If set then use airspeed failure check
182 : ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE = (1<<1), // If set then automatically enable the airspeed sensor use when healthy again.
183 : DISABLE_VOLTAGE_CORRECTION = (1<<2),
184 : USE_EKF_CONSISTENCY = (1<<3),
185 : REPORT_OFFSET = (1<<4), // report offset cal to GCS
186 : };
187 :
188 : enum airspeed_type {
189 : TYPE_NONE=0,
190 : #if AP_AIRSPEED_MS4525_ENABLED
191 : TYPE_I2C_MS4525=1,
192 : #endif // AP_AIRSPEED_MS4525_ENABLED
193 : #if AP_AIRSPEED_ANALOG_ENABLED
194 : TYPE_ANALOG=2,
195 : #endif // AP_AIRSPEED_ANALOG_ENABLED
196 : #if AP_AIRSPEED_MS5525_ENABLED
197 : TYPE_I2C_MS5525=3,
198 : TYPE_I2C_MS5525_ADDRESS_1=4,
199 : TYPE_I2C_MS5525_ADDRESS_2=5,
200 : #endif // AP_AIRSPEED_MS5525_ENABLED
201 : #if AP_AIRSPEED_SDP3X_ENABLED
202 : TYPE_I2C_SDP3X=6,
203 : #endif // AP_AIRSPEED_SDP3X_ENABLED
204 : #if AP_AIRSPEED_DLVR_ENABLED
205 : TYPE_I2C_DLVR_5IN=7,
206 : #endif // AP_AIRSPEED_DLVR_ENABLED
207 : #if AP_AIRSPEED_DRONECAN_ENABLED
208 : TYPE_UAVCAN=8,
209 : #endif // AP_AIRSPEED_DRONECAN_ENABLED
210 : #if AP_AIRSPEED_DLVR_ENABLED
211 : TYPE_I2C_DLVR_10IN=9,
212 : TYPE_I2C_DLVR_20IN=10,
213 : TYPE_I2C_DLVR_30IN=11,
214 : TYPE_I2C_DLVR_60IN=12,
215 : #endif // AP_AIRSPEED_DLVR_ENABLED
216 : #if AP_AIRSPEED_NMEA_ENABLED
217 : TYPE_NMEA_WATER=13,
218 : #endif // AP_AIRSPEED_NMEA_ENABLED
219 : #if AP_AIRSPEED_MSP_ENABLED
220 : TYPE_MSP=14,
221 : #endif // AP_AIRSPEED_MSP_ENABLED
222 : #if AP_AIRSPEED_ASP5033_ENABLED
223 : TYPE_I2C_ASP5033=15,
224 : #endif // AP_AIRSPEED_ASP5033_ENABLED
225 : #if AP_AIRSPEED_EXTERNAL_ENABLED
226 : TYPE_EXTERNAL=16,
227 : #endif // AP_AIRSPEED_EXTERNAL_ENABLED
228 : #if AP_AIRSPEED_AUAV_ENABLED
229 : TYPE_AUAV_10IN=17,
230 : TYPE_AUAV_5IN=18,
231 : TYPE_AUAV_30IN=19,
232 : #endif // AP_AIRSPEED_AUAV_ENABLED
233 : #if AP_AIRSPEED_SCRIPTING_ENABLED
234 : TYPE_SCRIPTING=20,
235 : #endif // AP_AIRSPEED_SCRIPTING_ENABLED
236 : #if AP_AIRSPEED_SITL_ENABLED
237 : TYPE_SITL=100,
238 : #endif // AP_AIRSPEED_SITL_ENABLED
239 : };
240 :
241 : // get current primary sensor
242 122463799 : uint8_t get_primary(void) const { return primary; }
243 :
244 : // get number of sensors
245 8611527 : uint8_t get_num_sensors(void) const { return num_sensors; }
246 :
247 : #if AP_AIRSPEED_SCRIPTING_ENABLED
248 : // get backend for a given instance, used by scripting
249 : AP_Airspeed_Backend *get_backend(uint8_t id) const;
250 : #endif // AP_AIRSPEED_SCRIPTING_ENABLED
251 :
252 287142353 : static AP_Airspeed *get_singleton() { return _singleton; }
253 :
254 : // return the current corrected pressure, public for AP_Periph
255 : float get_corrected_pressure(uint8_t i) const;
256 : float get_corrected_pressure(void) const {
257 : return get_corrected_pressure(primary);
258 : }
259 :
260 : #if AP_AIRSPEED_MSP_ENABLED
261 : void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
262 : #endif
263 :
264 : #if AP_AIRSPEED_EXTERNAL_ENABLED
265 : void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt);
266 : #endif
267 :
268 : enum class CalibrationState {
269 : NOT_STARTED,
270 : NOT_REQUIRED_ZERO_OFFSET,
271 : IN_PROGRESS,
272 : SUCCESS,
273 : FAILED
274 : };
275 :
276 : // get aggregate calibration state for the Airspeed library:
277 : CalibrationState get_calibration_state() const;
278 :
279 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
280 : bool arming_checks(size_t buflen, char *buffer) const;
281 :
282 : private:
283 : static AP_Airspeed *_singleton;
284 :
285 : AP_Int8 _enable;
286 : bool lib_enabled() const;
287 :
288 : AP_Int8 primary_sensor;
289 : AP_Int8 max_speed_pcnt;
290 : AP_Int32 _options; // bitmask options for airspeed
291 : AP_Float _wind_max;
292 : AP_Float _wind_warn;
293 : AP_Float _wind_gate;
294 :
295 : AP_Airspeed_Params param[AIRSPEED_MAX_SENSORS];
296 :
297 : struct airspeed_state {
298 : float raw_airspeed;
299 : float airspeed;
300 : float last_pressure;
301 : float filtered_pressure;
302 : float corrected_pressure;
303 : uint32_t last_update_ms;
304 : bool healthy;
305 :
306 : // Pre-flight offset calibration
307 : struct {
308 : uint32_t start_ms;
309 : float sum;
310 : uint16_t count;
311 : uint16_t read_count;
312 : CalibrationState state;
313 : } cal;
314 :
315 : #if AP_AIRSPEED_AUTOCAL_ENABLE
316 : // In flight ratio calibration
317 : Airspeed_Calibration calibration;
318 : float last_saved_ratio;
319 : uint8_t counter;
320 : #endif // AP_AIRSPEED_AUTOCAL_ENABLE
321 :
322 : struct {
323 : uint32_t last_check_ms;
324 : float health_probability;
325 : float test_ratio;
326 : int8_t param_use_backup;
327 : uint32_t last_warn_ms;
328 : } failures;
329 :
330 : #if AP_AIRSPEED_HYGROMETER_ENABLE
331 : uint32_t last_hygrometer_log_ms;
332 : #endif
333 : } state[AIRSPEED_MAX_SENSORS];
334 :
335 : bool calibration_enabled;
336 :
337 : // can be set to true to disable the use of the airspeed sensor
338 : bool _force_disable_use;
339 :
340 : // current primary sensor
341 : uint8_t primary;
342 : uint8_t num_sensors;
343 :
344 : // Track primary parameter, this allows changes to be honored in flight
345 : uint8_t last_user_primary;
346 :
347 : uint32_t _log_bit = -1; // stores which bit in LOG_BITMASK is used to indicate we should log airspeed readings
348 :
349 : void read(uint8_t i);
350 :
351 : // get the health probability
352 555991 : float get_health_probability(uint8_t i) const {
353 555991 : return state[i].failures.health_probability;
354 : }
355 : float get_health_probability(void) const {
356 : return get_health_probability(primary);
357 : }
358 :
359 : // get the consistency test ratio
360 555991 : float get_test_ratio(uint8_t i) const {
361 555991 : return state[i].failures.test_ratio;
362 : }
363 : float get_test_ratio(void) const {
364 : return get_test_ratio(primary);
365 : }
366 :
367 : void update_calibration(uint8_t i, float raw_pressure);
368 : void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
369 : void send_airspeed_calibration(const Vector3f &vg);
370 : // return the current calibration offset
371 : float get_offset(uint8_t i) const;
372 : float get_offset(void) const { return get_offset(primary); }
373 :
374 : void check_sensor_failures();
375 : void check_sensor_ahrs_wind_max_failures(uint8_t i);
376 :
377 : AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
378 :
379 : void Log_Airspeed();
380 :
381 : bool add_backend(AP_Airspeed_Backend *backend);
382 :
383 : const AP_FixedWing *fixed_wing_parameters;
384 :
385 : void convert_per_instance();
386 :
387 : // Select primary sensor based on user parameters and health
388 : uint8_t select_primary();
389 :
390 : };
391 :
392 : namespace AP {
393 : AP_Airspeed *airspeed();
394 : };
395 :
396 : #endif // AP_AIRSPEED_ENABLED
|
