libraries/AP_Airspeed/AP_Airspeed.h

   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     AP_Int8  skip_cal;
  35     AP_Int8  tube_order;
  36 #endif
  37     AP_Int8  type;
  38     AP_Int8  bus;
  39 #if AP_AIRSPEED_AUTOCAL_ENABLE
  40     AP_Int8  autocal;
  41 #endif
  42
  43     static const struct AP_Param::GroupInfo var_info[];
  44 };
  45
  46
  47 class Airspeed_Calibration {
  48 public:
  49     friend class AP_Airspeed;
  50     // constructor
  51     Airspeed_Calibration();
  52
  53     // initialise the calibration
  54     void init(float initial_ratio);
  55
  56     // take current airspeed in m/s and ground speed vector and return
  57     // new scaling factor
  58     float update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal);
  59
  60 private:
  61     // state of kalman filter for airspeed ratio estimation
  62     Matrix3f P; // covariance matrix
  63     const float Q0; // process noise matrix top left and middle element
  64     const float Q1; // process noise matrix bottom right element
  65     Vector3f state; // state vector
  66     const float DT; // time delta
  67 };
  68
  69 class AP_Airspeed
  70 {
  71 public:
  72     friend class AP_Airspeed_Backend;
  73
  74     // constructor
  75     AP_Airspeed();
  76
  77     void set_fixedwing_parameters(const class AP_FixedWing *_fixed_wing_parameters);
  78
  79     void init(void);
  80     void allocate();
  81
  82
  83     // indicate which bit in LOG_BITMASK indicates we should log airspeed readings
  84     void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; }
  85
  86 #if AP_AIRSPEED_AUTOCAL_ENABLE
  87     // inflight ratio calibration
  88     void set_calibration_enabled(bool enable) {calibration_enabled = enable;}
  89 #endif //AP_AIRSPEED_AUTOCAL_ENABLE
  90
  91     // read the analog source and update airspeed
  92     void update(void);
  93
  94     // calibrate the airspeed. This must be called on startup if the
  95     // altitude/climb_rate/acceleration interfaces are ever used
  96     void calibrate(bool in_startup);
  97
  98     // return the current airspeed in m/s
  99     float get_airspeed(uint8_t i) const;
 100     float get_airspeed(void) const { return get_airspeed(primary); }
 101
 102     // return the unfiltered airspeed in m/s
 103     float get_raw_airspeed(uint8_t i) const;
 104     float get_raw_airspeed(void) const { return get_raw_airspeed(primary); }
 105
 106     // return the current airspeed ratio (dimensionless)
 107     float get_airspeed_ratio(uint8_t i) const {
 108 #ifndef HAL_BUILD_AP_PERIPH
 109         return param[i].ratio;
 110 #else
 111         return 0.0;
 112 #endif
 113     }
 114     float get_airspeed_ratio(void) const { return get_airspeed_ratio(primary); }
 115
 116     // get temperature if available
 117     bool get_temperature(uint8_t i, float &temperature);
 118     bool get_temperature(float &temperature) { return get_temperature(primary, temperature); }
 119
 120     // set the airspeed ratio (dimensionless)
 121 #ifndef HAL_BUILD_AP_PERIPH
 122     void set_airspeed_ratio(uint8_t i, float ratio) {
 123         param[i].ratio.set(ratio);
 124     }
 125     void set_airspeed_ratio(float ratio) { set_airspeed_ratio(primary, ratio); }
 126 #endif
 127
 128     // return true if airspeed is enabled, and airspeed use is set
 129     bool use(uint8_t i) const;
 130     bool use(void) const { return use(primary); }
 131
 132     // force disabling of all airspeed sensors
 133     void force_disable_use(bool value) {
 134         _force_disable_use = value;
 135     }
 136
 137     // return true if airspeed is enabled
 138     bool enabled(uint8_t i) const;
 139     bool enabled(void) const { return enabled(primary); }
 140
 141     // return the differential pressure in Pascal for the last airspeed reading
 142     float get_differential_pressure(uint8_t i) const;
 143     float get_differential_pressure(void) const { return get_differential_pressure(primary); }
 144
 145     // update airspeed ratio calibration
 146     void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
 147
 148     // return health status of sensor
 149     bool healthy(uint8_t i) const;
 150     bool healthy(void) const { return healthy(primary); }
 151
 152     // return true if all enabled sensors are healthy
 153     bool all_healthy(void) const;
 154
 155     // return time in ms of last update
 156     uint32_t last_update_ms(uint8_t i) const { return state[i].last_update_ms; }
 157     uint32_t last_update_ms(void) const { return last_update_ms(primary); }
 158
 159 #if AP_AIRSPEED_HYGROMETER_ENABLE
 160     bool get_hygrometer(uint8_t i, uint32_t &last_sample_ms, float &temperature, float &humidity) const;
 161 #endif
 162
 163     static const struct AP_Param::GroupInfo var_info[];
 164
 165     enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE = 0,
 166                             PITOT_TUBE_ORDER_NEGATIVE = 1,
 167                             PITOT_TUBE_ORDER_AUTO     = 2 };
 168
 169     enum OptionsMask {
 170         ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE                   = (1<<0),   // If set then use airspeed failure check
 171         ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE         = (1<<1),   // If set then automatically enable the airspeed sensor use when healthy again.
 172         DISABLE_VOLTAGE_CORRECTION                            = (1<<2),
 173         USE_EKF_CONSISTENCY                                   = (1<<3),
 174         REPORT_OFFSET                                         = (1<<4),   // report offset cal to GCS
 175     };
 176
 177     enum airspeed_type {
 178         TYPE_NONE=0,
 179         TYPE_I2C_MS4525=1,
 180         TYPE_ANALOG=2,
 181         TYPE_I2C_MS5525=3,
 182         TYPE_I2C_MS5525_ADDRESS_1=4,
 183         TYPE_I2C_MS5525_ADDRESS_2=5,
 184         TYPE_I2C_SDP3X=6,
 185         TYPE_I2C_DLVR_5IN=7,
 186         TYPE_UAVCAN=8,
 187         TYPE_I2C_DLVR_10IN=9,
 188         TYPE_I2C_DLVR_20IN=10,
 189         TYPE_I2C_DLVR_30IN=11,
 190         TYPE_I2C_DLVR_60IN=12,
 191         TYPE_NMEA_WATER=13,
 192         TYPE_MSP=14,
 193         TYPE_I2C_ASP5033=15,
 194         TYPE_EXTERNAL=16,
 195         TYPE_SITL=100,
 196     };
 197
 198     // get current primary sensor
 199     uint8_t get_primary(void) const { return primary; }
 200
 201     // get number of sensors
 202     uint8_t get_num_sensors(void) const { return num_sensors; }
 203
 204     static AP_Airspeed *get_singleton() { return _singleton; }
 205
 206     // return the current corrected pressure, public for AP_Periph
 207     float get_corrected_pressure(uint8_t i) const;
 208     float get_corrected_pressure(void) const {
 209         return get_corrected_pressure(primary);
 210     }
 211
 212 #if AP_AIRSPEED_MSP_ENABLED
 213     void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
 214 #endif
 215
 216 #if AP_AIRSPEED_EXTERNAL_ENABLED
 217     void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt);
 218 #endif
 219
 220     enum class CalibrationState {
 221         NOT_STARTED,
 222         IN_PROGRESS,
 223         SUCCESS,
 224         FAILED
 225     };
 226     // get aggregate calibration state for the Airspeed library:
 227     CalibrationState get_calibration_state() const;
 228
 229 private:
 230     static AP_Airspeed *_singleton;
 231
 232     AP_Int8 _enable;
 233     bool lib_enabled() const;
 234
 235     AP_Int8 primary_sensor;
 236     AP_Int8 max_speed_pcnt;
 237     AP_Int32 _options;    // bitmask options for airspeed
 238     AP_Float _wind_max;
 239     AP_Float _wind_warn;
 240     AP_Float _wind_gate;
 241
 242     AP_Airspeed_Params param[AIRSPEED_MAX_SENSORS];
 243
 244     CalibrationState calibration_state[AIRSPEED_MAX_SENSORS];
 245
 246     struct airspeed_state {
 247         float   raw_airspeed;
 248         float   airspeed;
 249         float   last_pressure;
 250         float   filtered_pressure;
 251         float   corrected_pressure;
 252         uint32_t last_update_ms;
 253         bool use_zero_offset;
 254         bool    healthy;
 255
 256         // state of runtime calibration
 257         struct {
 258             uint32_t start_ms;
 259             float    sum;
 260             uint16_t count;
 261             uint16_t read_count;
 262         } cal;
 263
 264 #if AP_AIRSPEED_AUTOCAL_ENABLE
 265         Airspeed_Calibration calibration;
 266         float last_saved_ratio;
 267         uint8_t counter;
 268 #endif // AP_AIRSPEED_AUTOCAL_ENABLE
 269
 270         struct {
 271             uint32_t last_check_ms;
 272             float health_probability;
 273             float test_ratio;
 274             int8_t param_use_backup;
 275             uint32_t last_warn_ms;
 276         } failures;
 277
 278 #if AP_AIRSPEED_HYGROMETER_ENABLE
 279         uint32_t last_hygrometer_log_ms;
 280 #endif
 281     } state[AIRSPEED_MAX_SENSORS];
 282
 283     bool calibration_enabled;
 284
 285     // can be set to true to disable the use of the airspeed sensor
 286     bool _force_disable_use;
 287
 288     // current primary sensor
 289     uint8_t primary;
 290     uint8_t num_sensors;
 291
 292     uint32_t _log_bit = -1;     // stores which bit in LOG_BITMASK is used to indicate we should log airspeed readings
 293
 294     void read(uint8_t i);
 295     // return the differential pressure in Pascal for the last airspeed reading for the requested instance
 296     // returns 0 if the sensor is not enabled
 297     float get_pressure(uint8_t i);
 298
 299     // get the health probability
 300     float get_health_probability(uint8_t i) const {
 301         return state[i].failures.health_probability;
 302     }
 303     float get_health_probability(void) const {
 304         return get_health_probability(primary);
 305     }
 306
 307     // get the consistency test ratio
 308     float get_test_ratio(uint8_t i) const {
 309         return state[i].failures.test_ratio;
 310     }
 311     float get_test_ratio(void) const {
 312         return get_test_ratio(primary);
 313     }
 314
 315     void update_calibration(uint8_t i, float raw_pressure);
 316     void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
 317     void send_airspeed_calibration(const Vector3f &vg);
 318     // return the current calibration offset
 319     float get_offset(uint8_t i) const {
 320 #ifndef HAL_BUILD_AP_PERIPH
 321         return param[i].offset;
 322 #else
 323         return 0.0;
 324 #endif
 325     }
 326     float get_offset(void) const { return get_offset(primary); }
 327
 328     void check_sensor_failures();
 329     void check_sensor_ahrs_wind_max_failures(uint8_t i);
 330
 331     AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
 332
 333     void Log_Airspeed();
 334
 335     bool add_backend(AP_Airspeed_Backend *backend);
 336
 337     const AP_FixedWing *fixed_wing_parameters;
 338
 339     void convert_per_instance();
 340
 341 };
 342
 343 namespace AP {
 344     AP_Airspeed *airspeed();
 345 };
 346
 347 #endif  // AP_AIRSPEED_ENABLED