2016-01-29 07:38:33 -04:00
|
|
|
#pragma once
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
2016-01-29 07:38:33 -04:00
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include "AP_Airspeed_Backend.h"
|
|
|
|
#include "AP_Airspeed_I2C.h"
|
2016-01-29 07:38:33 -04:00
|
|
|
#include "AP_Airspeed_PX4.h"
|
|
|
|
#include "AP_Airspeed_analog.h"
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2013-07-19 19:14:58 -03:00
|
|
|
class Airspeed_Calibration {
|
|
|
|
public:
|
2013-08-28 09:35:50 -03:00
|
|
|
friend class AP_Airspeed;
|
2013-07-19 19:14:58 -03:00
|
|
|
// constructor
|
2016-08-08 00:29:50 -03:00
|
|
|
Airspeed_Calibration();
|
2013-07-19 19:14:58 -03:00
|
|
|
|
|
|
|
// initialise the calibration
|
|
|
|
void init(float initial_ratio);
|
|
|
|
|
|
|
|
// take current airspeed in m/s and ground speed vector and return
|
|
|
|
// new scaling factor
|
2016-08-08 00:28:24 -03:00
|
|
|
float update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal);
|
2013-07-19 19:14:58 -03:00
|
|
|
|
|
|
|
private:
|
|
|
|
// state of kalman filter for airspeed ratio estimation
|
|
|
|
Matrix3f P; // covarience matrix
|
|
|
|
const float Q0; // process noise matrix top left and middle element
|
|
|
|
const float Q1; // process noise matrix bottom right element
|
|
|
|
Vector3f state; // state vector
|
|
|
|
const float DT; // time delta
|
|
|
|
};
|
|
|
|
|
2012-07-15 22:21:20 -03:00
|
|
|
class AP_Airspeed
|
|
|
|
{
|
|
|
|
public:
|
2012-08-17 03:08:43 -03:00
|
|
|
// constructor
|
2016-08-08 00:29:50 -03:00
|
|
|
AP_Airspeed();
|
2012-10-09 21:01:22 -03:00
|
|
|
|
2013-06-02 22:40:06 -03:00
|
|
|
void init(void);
|
|
|
|
|
2012-08-17 03:08:43 -03:00
|
|
|
// read the analog source and update _airspeed
|
|
|
|
void read(void);
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2012-08-17 03:08:43 -03:00
|
|
|
// calibrate the airspeed. This must be called on startup if the
|
|
|
|
// altitude/climb_rate/acceleration interfaces are ever used
|
2014-11-13 06:12:37 -04:00
|
|
|
void calibrate(bool in_startup);
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2012-08-17 03:08:43 -03:00
|
|
|
// return the current airspeed in m/s
|
2013-07-13 08:53:38 -03:00
|
|
|
float get_airspeed(void) const {
|
2012-08-17 03:08:43 -03:00
|
|
|
return _airspeed;
|
|
|
|
}
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2013-06-02 22:40:06 -03:00
|
|
|
// return the unfiltered airspeed in m/s
|
2013-07-13 08:53:38 -03:00
|
|
|
float get_raw_airspeed(void) const {
|
2013-06-02 22:40:06 -03:00
|
|
|
return _raw_airspeed;
|
|
|
|
}
|
|
|
|
|
2012-08-17 03:08:43 -03:00
|
|
|
// return the current airspeed in cm/s
|
2013-07-13 08:53:38 -03:00
|
|
|
float get_airspeed_cm(void) const {
|
2012-08-17 03:08:43 -03:00
|
|
|
return _airspeed*100;
|
|
|
|
}
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2013-07-13 08:53:38 -03:00
|
|
|
// return the current airspeed ratio (dimensionless)
|
|
|
|
float get_airspeed_ratio(void) const {
|
|
|
|
return _ratio;
|
|
|
|
}
|
|
|
|
|
2014-01-27 19:35:35 -04:00
|
|
|
// get temperature if available
|
|
|
|
bool get_temperature(float &temperature);
|
|
|
|
|
2013-07-13 08:53:38 -03:00
|
|
|
// set the airspeed ratio (dimensionless)
|
|
|
|
void set_airspeed_ratio(float ratio) {
|
|
|
|
_ratio.set(ratio);
|
|
|
|
}
|
|
|
|
|
2012-07-15 22:21:20 -03:00
|
|
|
// return true if airspeed is enabled, and airspeed use is set
|
2013-07-13 08:53:38 -03:00
|
|
|
bool use(void) const {
|
2015-01-19 20:26:20 -04:00
|
|
|
return _enable && _use;
|
2012-08-17 03:08:43 -03:00
|
|
|
}
|
2012-07-15 22:21:20 -03:00
|
|
|
|
|
|
|
// return true if airspeed is enabled
|
2013-07-13 08:53:38 -03:00
|
|
|
bool enabled(void) const {
|
2012-08-17 03:08:43 -03:00
|
|
|
return _enable;
|
|
|
|
}
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2013-06-02 22:40:06 -03:00
|
|
|
// force disable the airspeed sensor
|
|
|
|
void disable(void) {
|
|
|
|
_enable.set(0);
|
|
|
|
}
|
|
|
|
|
2012-07-15 22:21:20 -03:00
|
|
|
// used by HIL to set the airspeed
|
2012-08-17 03:08:43 -03:00
|
|
|
void set_HIL(float airspeed) {
|
|
|
|
_airspeed = airspeed;
|
|
|
|
}
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2013-07-19 19:14:58 -03:00
|
|
|
// return the differential pressure in Pascal for the last
|
|
|
|
// airspeed reading. Used by the calibration code
|
|
|
|
float get_differential_pressure(void) const {
|
2014-11-13 02:49:04 -04:00
|
|
|
return _last_pressure;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return the current offset
|
|
|
|
float get_offset(void) const {
|
|
|
|
return _offset;
|
|
|
|
}
|
|
|
|
|
2016-05-24 01:22:40 -03:00
|
|
|
// return the current corrected pressure
|
|
|
|
float get_corrected_pressure(void) const {
|
|
|
|
return _corrected_pressure;
|
2013-07-19 19:14:58 -03:00
|
|
|
}
|
|
|
|
|
2013-07-19 22:11:04 -03:00
|
|
|
// set the apparent to true airspeed ratio
|
|
|
|
void set_EAS2TAS(float v) {
|
|
|
|
_EAS2TAS = v;
|
|
|
|
}
|
|
|
|
|
|
|
|
// get the apparent to true airspeed ratio
|
|
|
|
float get_EAS2TAS(void) const {
|
|
|
|
return _EAS2TAS;
|
|
|
|
}
|
|
|
|
|
2013-07-19 19:14:58 -03:00
|
|
|
// update airspeed ratio calibration
|
2016-08-08 00:28:24 -03:00
|
|
|
void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
|
2013-08-28 09:35:50 -03:00
|
|
|
|
|
|
|
// log data to MAVLink
|
|
|
|
void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
|
2013-07-19 19:14:58 -03:00
|
|
|
|
2013-10-29 19:03:35 -03:00
|
|
|
// return health status of sensor
|
2016-05-04 19:38:24 -03:00
|
|
|
bool healthy(void) const { return _healthy && fabsf(_offset) > 0 && _enable; }
|
2013-10-29 19:03:35 -03:00
|
|
|
|
2016-01-29 07:38:33 -04:00
|
|
|
void setHIL(float pressure) { _healthy=_hil_set=true; _hil_pressure=pressure; }
|
2014-08-10 05:16:53 -03:00
|
|
|
|
2013-12-30 06:23:58 -04:00
|
|
|
// return time in ms of last update
|
|
|
|
uint32_t last_update_ms(void) const { return _last_update_ms; }
|
|
|
|
|
2014-02-17 18:24:14 -04:00
|
|
|
void setHIL(float airspeed, float diff_pressure, float temperature);
|
|
|
|
|
2012-10-09 21:01:22 -03:00
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2012-07-15 22:21:20 -03:00
|
|
|
|
2016-01-29 07:38:33 -04:00
|
|
|
enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE = 0,
|
|
|
|
PITOT_TUBE_ORDER_NEGATIVE = 1,
|
|
|
|
PITOT_TUBE_ORDER_AUTO = 2 };
|
2013-08-28 09:35:50 -03:00
|
|
|
|
2012-07-15 22:21:20 -03:00
|
|
|
private:
|
2012-08-17 03:08:43 -03:00
|
|
|
AP_Float _offset;
|
|
|
|
AP_Float _ratio;
|
2016-06-08 21:10:54 -03:00
|
|
|
AP_Float _psi_range;
|
2012-08-17 03:08:43 -03:00
|
|
|
AP_Int8 _use;
|
|
|
|
AP_Int8 _enable;
|
2013-06-02 22:40:06 -03:00
|
|
|
AP_Int8 _pin;
|
2013-07-19 19:14:58 -03:00
|
|
|
AP_Int8 _autocal;
|
2014-02-14 07:07:12 -04:00
|
|
|
AP_Int8 _tube_order;
|
2014-11-13 06:12:37 -04:00
|
|
|
AP_Int8 _skip_cal;
|
2013-06-02 22:40:06 -03:00
|
|
|
float _raw_airspeed;
|
2012-08-17 03:08:43 -03:00
|
|
|
float _airspeed;
|
2013-06-02 22:40:06 -03:00
|
|
|
float _last_pressure;
|
2016-05-24 01:22:40 -03:00
|
|
|
float _corrected_pressure;
|
2013-07-19 22:11:04 -03:00
|
|
|
float _EAS2TAS;
|
2014-08-10 05:16:53 -03:00
|
|
|
bool _healthy:1;
|
|
|
|
bool _hil_set:1;
|
|
|
|
float _hil_pressure;
|
2013-12-30 06:23:58 -04:00
|
|
|
uint32_t _last_update_ms;
|
2013-06-02 22:40:06 -03:00
|
|
|
|
2016-05-24 01:22:40 -03:00
|
|
|
// state of runtime calibration
|
|
|
|
struct {
|
|
|
|
uint32_t start_ms;
|
|
|
|
uint16_t count;
|
|
|
|
float sum;
|
2016-06-26 21:52:59 -03:00
|
|
|
uint16_t read_count;
|
2016-05-24 01:22:40 -03:00
|
|
|
} _cal;
|
|
|
|
|
2013-07-19 19:14:58 -03:00
|
|
|
Airspeed_Calibration _calibration;
|
|
|
|
float _last_saved_ratio;
|
2016-07-12 02:02:22 -03:00
|
|
|
uint8_t _counter;
|
2013-07-19 19:14:58 -03:00
|
|
|
|
2013-06-02 22:40:06 -03:00
|
|
|
float get_pressure(void);
|
2016-05-24 01:22:40 -03:00
|
|
|
void update_calibration(float raw_pressure);
|
2013-09-28 05:40:29 -03:00
|
|
|
|
2016-06-08 21:10:54 -03:00
|
|
|
AP_Airspeed_Analog analog{_pin, _psi_range};
|
2014-12-30 07:00:02 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
2016-06-08 21:10:54 -03:00
|
|
|
AP_Airspeed_PX4 digital{_psi_range};
|
2013-09-28 05:40:29 -03:00
|
|
|
#else
|
2016-06-08 21:10:54 -03:00
|
|
|
AP_Airspeed_I2C digital{_psi_range};
|
2013-09-28 05:40:29 -03:00
|
|
|
#endif
|
2012-07-15 22:21:20 -03:00
|
|
|
};
|