ardupilot/libraries/AP_Airspeed/AP_Airspeed.h

190 lines
5.2 KiB
C++

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Airspeed_Backend.h"
#include "AP_Airspeed_I2C.h"
#include "AP_Airspeed_PX4.h"
#include "AP_Airspeed_analog.h"
class Airspeed_Calibration {
public:
friend class AP_Airspeed;
// constructor
Airspeed_Calibration();
// initialise the calibration
void init(float initial_ratio);
// take current airspeed in m/s and ground speed vector and return
// new scaling factor
float update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal);
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
};
class AP_Airspeed
{
public:
// constructor
AP_Airspeed();
void init(void);
// read the analog source and update _airspeed
void read(void);
// calibrate the airspeed. This must be called on startup if the
// altitude/climb_rate/acceleration interfaces are ever used
void calibrate(bool in_startup);
// return the current airspeed in m/s
float get_airspeed(void) const {
return _airspeed;
}
// return the unfiltered airspeed in m/s
float get_raw_airspeed(void) const {
return _raw_airspeed;
}
// return the current airspeed in cm/s
float get_airspeed_cm(void) const {
return _airspeed*100;
}
// return the current airspeed ratio (dimensionless)
float get_airspeed_ratio(void) const {
return _ratio;
}
// get temperature if available
bool get_temperature(float &temperature);
// set the airspeed ratio (dimensionless)
void set_airspeed_ratio(float ratio) {
_ratio.set(ratio);
}
// return true if airspeed is enabled, and airspeed use is set
bool use(void) const {
return _enable && _use;
}
// return true if airspeed is enabled
bool enabled(void) const {
return _enable;
}
// force disable the airspeed sensor
void disable(void) {
_enable.set(0);
}
// used by HIL to set the airspeed
void set_HIL(float airspeed) {
_airspeed = airspeed;
}
// return the differential pressure in Pascal for the last
// airspeed reading. Used by the calibration code
float get_differential_pressure(void) const {
return _last_pressure;
}
// return the current offset
float get_offset(void) const {
return _offset;
}
// return the current corrected pressure
float get_corrected_pressure(void) const {
return _corrected_pressure;
}
// 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;
}
// update airspeed ratio calibration
void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
// log data to MAVLink
void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
// return health status of sensor
bool healthy(void) const { return _healthy && fabsf(_offset) > 0 && _enable; }
void setHIL(float pressure) { _healthy=_hil_set=true; _hil_pressure=pressure; }
// return time in ms of last update
uint32_t last_update_ms(void) const { return _last_update_ms; }
void setHIL(float airspeed, float diff_pressure, float temperature);
static const struct AP_Param::GroupInfo var_info[];
enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE = 0,
PITOT_TUBE_ORDER_NEGATIVE = 1,
PITOT_TUBE_ORDER_AUTO = 2 };
private:
AP_Float _offset;
AP_Float _ratio;
AP_Float _psi_range;
AP_Int8 _use;
AP_Int8 _enable;
AP_Int8 _pin;
AP_Int8 _autocal;
AP_Int8 _tube_order;
AP_Int8 _skip_cal;
float _raw_airspeed;
float _airspeed;
float _last_pressure;
float _corrected_pressure;
float _EAS2TAS;
bool _healthy:1;
bool _hil_set:1;
float _hil_pressure;
uint32_t _last_update_ms;
// state of runtime calibration
struct {
uint32_t start_ms;
uint16_t count;
float sum;
uint16_t read_count;
} _cal;
Airspeed_Calibration _calibration;
float _last_saved_ratio;
uint8_t _counter;
float get_pressure(void);
void update_calibration(float raw_pressure);
AP_Airspeed_Analog analog{_pin, _psi_range};
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
AP_Airspeed_PX4 digital{_psi_range};
#else
AP_Airspeed_I2C digital{_psi_range};
#endif
};