mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 05:43:34 +08:00
Rename equivalent airspeed (EAS) to calibrated airspeed (CAS)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
@@ -1,10 +1,10 @@
|
|||||||
uint64 timestamp # time since system start (microseconds)
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid
|
float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid
|
||||||
float32 equivalent_airspeed_m_s # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid
|
float32 calibrated_airspeed_m_s # calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid
|
||||||
float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid
|
float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid
|
||||||
|
|
||||||
float32 equivalent_ground_minus_wind_m_s # EAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
|
float32 calibrated_ground_minus_wind_m_s # CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
|
||||||
float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
|
float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
|
||||||
|
|
||||||
bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid.
|
bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid.
|
||||||
|
|||||||
@@ -186,17 +186,17 @@ float calc_IAS(float differential_pressure)
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
float calc_TAS_from_EAS(float speed_equivalent, float pressure_ambient, float temperature_celsius)
|
float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float temperature_celsius)
|
||||||
{
|
{
|
||||||
if (!PX4_ISFINITE(temperature_celsius)) {
|
if (!PX4_ISFINITE(temperature_celsius)) {
|
||||||
temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees celcius
|
temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees celcius
|
||||||
}
|
}
|
||||||
|
|
||||||
return speed_equivalent * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient,
|
return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient,
|
||||||
temperature_celsius));
|
temperature_celsius));
|
||||||
}
|
}
|
||||||
|
|
||||||
float calc_EAS_from_IAS(float speed_indicated, float scale)
|
float calc_CAS_from_IAS(float speed_indicated, float scale)
|
||||||
{
|
{
|
||||||
return speed_indicated * scale;
|
return speed_indicated * scale;
|
||||||
}
|
}
|
||||||
@@ -228,7 +228,7 @@ float get_air_density(float static_pressure, float temperature_celsius)
|
|||||||
return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
|
return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
|
||||||
}
|
}
|
||||||
|
|
||||||
float calc_EAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius)
|
float calc_CAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius)
|
||||||
{
|
{
|
||||||
return speed_true * sqrtf(get_air_density(pressure_ambient, temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
return speed_true * sqrtf(get_air_density(pressure_ambient, temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||||
}
|
}
|
||||||
|
|||||||
+11
-13
@@ -86,34 +86,32 @@ __EXPORT float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel,
|
|||||||
__EXPORT float calc_IAS(float differential_pressure);
|
__EXPORT float calc_IAS(float differential_pressure);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate true airspeed (TAS) from equivalent airspeed (EAS).
|
* Calculate true airspeed (TAS) from calibrated airspeed (CAS).
|
||||||
*
|
*
|
||||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||||
*
|
*
|
||||||
* @param speed_equivalent current equivalent airspeed
|
* @param speed_equivalent current calibrated airspeed
|
||||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||||
* @param temperature_celsius air temperature in degrees celcius
|
* @param temperature_celsius air temperature in degrees celcius
|
||||||
* @return TAS in m/s
|
* @return TAS in m/s
|
||||||
*/
|
*/
|
||||||
__EXPORT float calc_TAS_from_EAS(float speed_indicated, float pressure_ambient,
|
__EXPORT float calc_TAS_from_CAS(float speed_indicated, float pressure_ambient,
|
||||||
float temperature_celsius);
|
float temperature_celsius);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate equivalent airspeed (EAS) from indicated airspeed (IAS).
|
* Calculate calibrated airspeed (CAS) from indicated airspeed (IAS).
|
||||||
* Note that we neglect the conversion from CAS (calibrated airspeed) to EAS.
|
|
||||||
*
|
*
|
||||||
* @param speed_indicated current indicated airspeed
|
* @param speed_indicated current indicated airspeed
|
||||||
* @param scale scale from IAS to CAS (accounting for instrument and pitot position erros)
|
* @param scale scale from IAS to CAS (accounting for instrument and pitot position erros)
|
||||||
* @return EAS in m/s
|
* @return CAS in m/s
|
||||||
*/
|
*/
|
||||||
__EXPORT float calc_EAS_from_IAS(float speed_indicated, float scale);
|
__EXPORT float calc_CAS_from_IAS(float speed_indicated, float scale);
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Directly calculate true airspeed (TAS)
|
* Directly calculate true airspeed (TAS)
|
||||||
*
|
*
|
||||||
* Here we assume to have no instrument or pitot position error (IAS = CAS),
|
* Here we assume to have no instrument or pitot position error (IAS = CAS).
|
||||||
* and neglect the CAS to EAS conversion (CAS = EAS).
|
|
||||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind.
|
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind.
|
||||||
*
|
*
|
||||||
* @param total_pressure pressure inside the pitot/prandtl tube
|
* @param total_pressure pressure inside the pitot/prandtl tube
|
||||||
@@ -132,16 +130,16 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe
|
|||||||
__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
|
__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate equivalent airspeed (EAS) from true airspeed (TAS).
|
* Calculate calibrated airspeed (CAS) from true airspeed (TAS).
|
||||||
* It is the inverse function to calc_TAS_from_EAS()
|
* It is the inverse function to calc_TAS_from_CAS()
|
||||||
*
|
*
|
||||||
*
|
*
|
||||||
* @param speed_true current true airspeed
|
* @param speed_true current true airspeed
|
||||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||||
* @param temperature_celsius air temperature in degrees celcius
|
* @param temperature_celsius air temperature in degrees celcius
|
||||||
* @return EAS in m/s
|
* @return CAS in m/s
|
||||||
*/
|
*/
|
||||||
__EXPORT float calc_EAS_from_TAS(float speed_true, float pressure_ambient,
|
__EXPORT float calc_CAS_from_TAS(float speed_true, float pressure_ambient,
|
||||||
float temperature_celsius);
|
float temperature_celsius);
|
||||||
|
|
||||||
__END_DECLS
|
__END_DECLS
|
||||||
|
|||||||
@@ -33,7 +33,7 @@
|
|||||||
|
|
||||||
/**
|
/**
|
||||||
* @file AirspeedValidator.cpp
|
* @file AirspeedValidator.cpp
|
||||||
* Estimates airspeed scale error (from indicated to equivalent airspeed), performes
|
* Estimates airspeed scale error (from indicated to calibrated airspeed), performes
|
||||||
* checks on airspeed measurement input and reports airspeed valid or invalid.
|
* checks on airspeed measurement input and reports airspeed valid or invalid.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
@@ -52,8 +52,8 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
|
|||||||
_previous_airspeed_timestamp = input_data.airspeed_timestamp;
|
_previous_airspeed_timestamp = input_data.airspeed_timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
update_EAS_scale();
|
update_CAS_scale();
|
||||||
update_EAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
|
update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
|
||||||
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
|
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
|
||||||
input_data.lpos_vy,
|
input_data.lpos_vy,
|
||||||
input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
|
input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
|
||||||
@@ -117,22 +117,22 @@ AirspeedValidator::set_airspeed_scale_manual(float airspeed_scale_manual)
|
|||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
AirspeedValidator::update_EAS_scale()
|
AirspeedValidator::update_CAS_scale()
|
||||||
{
|
{
|
||||||
if (_wind_estimator.is_estimate_valid()) {
|
if (_wind_estimator.is_estimate_valid()) {
|
||||||
_EAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);
|
_CAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.5f, 2.0f);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
_EAS_scale = _airspeed_scale_manual;
|
_CAS_scale = _airspeed_scale_manual;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
AirspeedValidator::update_EAS_TAS(float air_pressure_pa, float air_temperature_celsius)
|
AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius)
|
||||||
{
|
{
|
||||||
_EAS = calc_EAS_from_IAS(_IAS, _EAS_scale);
|
_CAS = calc_CAS_from_IAS(_IAS, _CAS_scale);
|
||||||
_TAS = calc_TAS_from_EAS(_EAS, air_pressure_pa, air_temperature_celsius);
|
_TAS = calc_TAS_from_CAS(_CAS, air_pressure_pa, air_temperature_celsius);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
@@ -204,7 +204,7 @@ AirspeedValidator::check_load_factor(float accel_z)
|
|||||||
if (_in_fixed_wing_flight) {
|
if (_in_fixed_wing_flight) {
|
||||||
|
|
||||||
if (!bad_number_fail) {
|
if (!bad_number_fail) {
|
||||||
float max_lift_ratio = fmaxf(_EAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f);
|
float max_lift_ratio = fmaxf(_CAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f);
|
||||||
max_lift_ratio *= max_lift_ratio;
|
max_lift_ratio *= max_lift_ratio;
|
||||||
|
|
||||||
_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel_z) / 9.81f) / max_lift_ratio;
|
_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel_z) / 9.81f) / max_lift_ratio;
|
||||||
|
|||||||
@@ -79,10 +79,10 @@ public:
|
|||||||
void update_airspeed_validator(const airspeed_validator_update_data &input_data);
|
void update_airspeed_validator(const airspeed_validator_update_data &input_data);
|
||||||
|
|
||||||
float get_IAS() { return _IAS; }
|
float get_IAS() { return _IAS; }
|
||||||
float get_EAS() { return _EAS; }
|
float get_CAS() { return _CAS; }
|
||||||
float get_TAS() { return _TAS; }
|
float get_TAS() { return _TAS; }
|
||||||
bool get_airspeed_valid() { return _airspeed_valid; }
|
bool get_airspeed_valid() { return _airspeed_valid; }
|
||||||
float get_EAS_scale() {return _EAS_scale;}
|
float get_CAS_scale() {return _CAS_scale;}
|
||||||
|
|
||||||
wind_estimate_s get_wind_estimator_states(uint64_t timestamp);
|
wind_estimate_s get_wind_estimator_states(uint64_t timestamp);
|
||||||
|
|
||||||
@@ -115,15 +115,15 @@ private:
|
|||||||
WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator
|
WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator
|
||||||
|
|
||||||
// wind estimator parameter
|
// wind estimator parameter
|
||||||
bool _wind_estimator_scale_estimation_on{false}; ///< online scale estimation (IAS-->CAS/EAS) is on
|
bool _wind_estimator_scale_estimation_on{false}; ///< online scale estimation (IAS-->CAS) is on
|
||||||
float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale
|
float _airspeed_scale_manual{1.0f}; ///< manually entered airspeed scale
|
||||||
|
|
||||||
// general states
|
// general states
|
||||||
bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks
|
bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks
|
||||||
float _IAS{0.0f}; ///< indicated airsped in m/s
|
float _IAS{0.0f}; ///< indicated airsped in m/s
|
||||||
float _EAS{0.0f}; ///< equivalent airspeed in m/s
|
float _CAS{0.0f}; ///< calibrated airspeed in m/s
|
||||||
float _TAS{0.0f}; ///< true airspeed in m/s
|
float _TAS{0.0f}; ///< true airspeed in m/s
|
||||||
float _EAS_scale{1.0f}; ///< scale factor from IAS to EAS
|
float _CAS_scale{1.0f}; ///< scale factor from IAS to CAS
|
||||||
|
|
||||||
uint64_t _time_last_airspeed{0}; ///< time last airspeed measurement was received (uSec)
|
uint64_t _time_last_airspeed{0}; ///< time last airspeed measurement was received (uSec)
|
||||||
|
|
||||||
@@ -162,8 +162,8 @@ private:
|
|||||||
float lpos_vy,
|
float lpos_vy,
|
||||||
float lpos_vz,
|
float lpos_vz,
|
||||||
float lpos_evh, float lpos_evv, const float att_q[4]);
|
float lpos_evh, float lpos_evv, const float att_q[4]);
|
||||||
void update_EAS_scale();
|
void update_CAS_scale();
|
||||||
void update_EAS_TAS(float air_pressure_pa, float air_temperature_celsius);
|
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
|
||||||
void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
|
void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
|
||||||
float estimator_status_mag_test_ratio);
|
float estimator_status_mag_test_ratio);
|
||||||
void check_load_factor(float accel_z);
|
void check_load_factor(float accel_z);
|
||||||
|
|||||||
@@ -139,7 +139,7 @@ private:
|
|||||||
bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
|
bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
|
||||||
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
|
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
|
||||||
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
|
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
|
||||||
float _ground_minus_wind_EAS{0.0f}; /**< equivalent airspeed from groundspeed minus windspeed */
|
float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
|
||||||
|
|
||||||
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
|
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
|
||||||
|
|
||||||
@@ -335,8 +335,8 @@ AirspeedModule::Run()
|
|||||||
input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
|
input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
|
||||||
|
|
||||||
/* update in_fixed_wing_flight for the current airspeed sensor validator */
|
/* update in_fixed_wing_flight for the current airspeed sensor validator */
|
||||||
/* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */
|
/* takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed */
|
||||||
if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) {
|
if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_CAS > _airspeed_stall.get()) {
|
||||||
_in_takeoff_situation = false;
|
_in_takeoff_situation = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -412,12 +412,12 @@ void AirspeedModule::update_params()
|
|||||||
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
|
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
|
||||||
if (_valid_airspeed_index > 0) {
|
if (_valid_airspeed_index > 0) {
|
||||||
|
|
||||||
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
|
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||||
_param_west_airspeed_scale.commit_no_notification();
|
_param_west_airspeed_scale.commit_no_notification();
|
||||||
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
||||||
|
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
|
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
|
||||||
(double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
|
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
|
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
|
||||||
@@ -482,7 +482,7 @@ void AirspeedModule::update_ground_minus_wind_airspeed()
|
|||||||
float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
|
float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
|
||||||
float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
|
float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
|
||||||
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
|
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
|
||||||
_ground_minus_wind_EAS = calc_EAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
|
_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
|
||||||
_vehicle_air_data.baro_temp_celcius);
|
_vehicle_air_data.baro_temp_celcius);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -540,9 +540,9 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||||||
airspeed_validated_s airspeed_validated = {};
|
airspeed_validated_s airspeed_validated = {};
|
||||||
airspeed_validated.timestamp = _time_now_usec;
|
airspeed_validated.timestamp = _time_now_usec;
|
||||||
airspeed_validated.true_ground_minus_wind_m_s = NAN;
|
airspeed_validated.true_ground_minus_wind_m_s = NAN;
|
||||||
airspeed_validated.equivalent_ground_minus_wind_m_s = NAN;
|
airspeed_validated.calibrated_ground_minus_wind_m_s = NAN;
|
||||||
airspeed_validated.indicated_airspeed_m_s = NAN;
|
airspeed_validated.indicated_airspeed_m_s = NAN;
|
||||||
airspeed_validated.equivalent_airspeed_m_s = NAN;
|
airspeed_validated.calibrated_airspeed_m_s = NAN;
|
||||||
airspeed_validated.true_airspeed_m_s = NAN;
|
airspeed_validated.true_airspeed_m_s = NAN;
|
||||||
airspeed_validated.airspeed_sensor_measurement_valid = false;
|
airspeed_validated.airspeed_sensor_measurement_valid = false;
|
||||||
airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
|
airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
|
||||||
@@ -552,20 +552,20 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case airspeed_index::GROUND_MINUS_WIND_INDEX:
|
case airspeed_index::GROUND_MINUS_WIND_INDEX:
|
||||||
/* Take IAS, EAS, TAS from groundspeed-windspeed */
|
/* Take IAS, CAS, TAS from groundspeed-windspeed */
|
||||||
airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_EAS;
|
airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_CAS;
|
||||||
airspeed_validated.equivalent_airspeed_m_s = _ground_minus_wind_EAS;
|
airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS;
|
||||||
airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
|
airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
|
||||||
airspeed_validated.equivalent_ground_minus_wind_m_s = _ground_minus_wind_EAS;
|
airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
|
||||||
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
|
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS();
|
airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS();
|
||||||
airspeed_validated.equivalent_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_EAS();
|
airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_CAS();
|
||||||
airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS();
|
airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS();
|
||||||
airspeed_validated.equivalent_ground_minus_wind_m_s = _ground_minus_wind_EAS;
|
airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
|
||||||
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
|
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
|
||||||
airspeed_validated.airspeed_sensor_measurement_valid = true;
|
airspeed_validated.airspeed_sensor_measurement_valid = true;
|
||||||
break;
|
break;
|
||||||
@@ -607,12 +607,12 @@ int AirspeedModule::print_usage(const char *reason)
|
|||||||
PRINT_MODULE_DESCRIPTION(
|
PRINT_MODULE_DESCRIPTION(
|
||||||
R"DESCR_STR(
|
R"DESCR_STR(
|
||||||
### Description
|
### Description
|
||||||
This module provides a single airspeed_validated topic, containing an indicated (IAS),
|
This module provides a single airspeed_validated topic, containing indicated (IAS),
|
||||||
equivalent (EAS), true airspeed (TAS) and the information if the estimation currently
|
calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
|
||||||
is invalid and if based sensor readings or on groundspeed minus windspeed.
|
is invalid and if based sensor readings or on groundspeed minus windspeed.
|
||||||
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
|
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
|
||||||
to a valid sensor in case of failure detection. For failure detection as well as for
|
to a valid sensor in case of failure detection. For failure detection as well as for
|
||||||
the estimation of a scale factor from IAS to EAS, it runs several wind estimators
|
the estimation of a scale factor from IAS to CAS, it runs several wind estimators
|
||||||
and also publishes those.
|
and also publishes those.
|
||||||
|
|
||||||
)DESCR_STR");
|
)DESCR_STR");
|
||||||
|
|||||||
@@ -240,7 +240,7 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
|
|||||||
float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
|
float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
|
||||||
{
|
{
|
||||||
_airspeed_validated_sub.update();
|
_airspeed_validated_sub.update();
|
||||||
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().equivalent_airspeed_m_s)
|
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
|
||||||
&& (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s);
|
&& (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s);
|
||||||
|
|
||||||
// if no airspeed measurement is available out best guess is to use the trim airspeed
|
// if no airspeed measurement is available out best guess is to use the trim airspeed
|
||||||
@@ -248,7 +248,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
|
|||||||
|
|
||||||
if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
|
if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
|
||||||
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
|
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
|
||||||
airspeed = math::max(0.5f, _airspeed_validated_sub.get().equivalent_airspeed_m_s);
|
airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
|
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
|
||||||
|
|||||||
@@ -176,16 +176,16 @@ FixedwingPositionControl::airspeed_poll()
|
|||||||
const airspeed_validated_s &airspeed_validated = _airspeed_validated_sub.get();
|
const airspeed_validated_s &airspeed_validated = _airspeed_validated_sub.get();
|
||||||
_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed
|
_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed
|
||||||
|
|
||||||
if (PX4_ISFINITE(airspeed_validated.equivalent_airspeed_m_s)
|
if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
|
||||||
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
|
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
|
||||||
&& (airspeed_validated.equivalent_airspeed_m_s > 0.0f)) {
|
&& (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {
|
||||||
|
|
||||||
airspeed_valid = true;
|
airspeed_valid = true;
|
||||||
|
|
||||||
_airspeed_last_valid = airspeed_validated.timestamp;
|
_airspeed_last_valid = airspeed_validated.timestamp;
|
||||||
_airspeed = airspeed_validated.equivalent_airspeed_m_s;
|
_airspeed = airspeed_validated.calibrated_airspeed_m_s;
|
||||||
|
|
||||||
_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.equivalent_airspeed_m_s, 0.9f, 2.0f);
|
_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -1707,7 +1707,7 @@ protected:
|
|||||||
_airspeed_validated_sub.copy(&airspeed_validated);
|
_airspeed_validated_sub.copy(&airspeed_validated);
|
||||||
|
|
||||||
mavlink_vfr_hud_t msg{};
|
mavlink_vfr_hud_t msg{};
|
||||||
msg.airspeed = airspeed_validated.equivalent_airspeed_m_s;
|
msg.airspeed = airspeed_validated.calibrated_airspeed_m_s;
|
||||||
msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy);
|
msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy);
|
||||||
msg.heading = math::degrees(wrap_2pi(lpos.heading));
|
msg.heading = math::degrees(wrap_2pi(lpos.heading));
|
||||||
|
|
||||||
|
|||||||
@@ -388,8 +388,8 @@ void Sensors::diff_pres_poll()
|
|||||||
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
|
diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
|
||||||
air_temperature_celsius);
|
air_temperature_celsius);
|
||||||
|
|
||||||
airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
|
airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
|
||||||
air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here
|
air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
|
||||||
|
|
||||||
airspeed.air_temperature_celsius = air_temperature_celsius;
|
airspeed.air_temperature_celsius = air_temperature_celsius;
|
||||||
|
|
||||||
|
|||||||
@@ -174,7 +174,7 @@ void Standard::update_vtol_state()
|
|||||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
|
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
|
||||||
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
|
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
|
||||||
|
|
||||||
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
|
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
|
||||||
&& !_params->airspeed_disabled;
|
&& !_params->airspeed_disabled;
|
||||||
const bool minimum_trans_time_elapsed = time_since_trans_start > _params->front_trans_time_min;
|
const bool minimum_trans_time_elapsed = time_since_trans_start > _params->front_trans_time_min;
|
||||||
|
|
||||||
@@ -182,7 +182,7 @@ void Standard::update_vtol_state()
|
|||||||
|
|
||||||
if (minimum_trans_time_elapsed) {
|
if (minimum_trans_time_elapsed) {
|
||||||
if (airspeed_triggers_transition) {
|
if (airspeed_triggers_transition) {
|
||||||
transition_to_fw = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
|
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
transition_to_fw = true;
|
transition_to_fw = true;
|
||||||
@@ -247,16 +247,16 @@ void Standard::update_transition_state()
|
|||||||
|
|
||||||
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
|
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
|
||||||
if (_airspeed_trans_blend_margin > 0.0f &&
|
if (_airspeed_trans_blend_margin > 0.0f &&
|
||||||
PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
|
PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
|
||||||
_airspeed_validated->equivalent_airspeed_m_s > 0.0f &&
|
_airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
|
||||||
_airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend &&
|
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend &&
|
||||||
time_since_trans_start > _params->front_trans_time_min) {
|
time_since_trans_start > _params->front_trans_time_min) {
|
||||||
|
|
||||||
mc_weight = 1.0f - fabsf(_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
|
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
|
||||||
_airspeed_trans_blend_margin;
|
_airspeed_trans_blend_margin;
|
||||||
// time based blending when no airspeed sensor is set
|
// time based blending when no airspeed sensor is set
|
||||||
|
|
||||||
} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) {
|
} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
|
||||||
mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
|
mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
|
||||||
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
|
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
|
||||||
|
|
||||||
@@ -416,4 +416,4 @@ Standard::waiting_on_tecs()
|
|||||||
{
|
{
|
||||||
// keep thrust from transition
|
// keep thrust from transition
|
||||||
_v_att_sp->thrust_body[0] = _pusher_throttle;
|
_v_att_sp->thrust_body[0] = _pusher_throttle;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -122,14 +122,14 @@ void Tailsitter::update_vtol_state()
|
|||||||
case vtol_mode::TRANSITION_FRONT_P1: {
|
case vtol_mode::TRANSITION_FRONT_P1: {
|
||||||
|
|
||||||
|
|
||||||
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
|
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
|
||||||
&& !_params->airspeed_disabled;
|
&& !_params->airspeed_disabled;
|
||||||
|
|
||||||
bool transition_to_fw = false;
|
bool transition_to_fw = false;
|
||||||
|
|
||||||
if (pitch <= PITCH_TRANSITION_FRONT_P1) {
|
if (pitch <= PITCH_TRANSITION_FRONT_P1) {
|
||||||
if (airspeed_triggers_transition) {
|
if (airspeed_triggers_transition) {
|
||||||
transition_to_fw = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
|
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
transition_to_fw = true;
|
transition_to_fw = true;
|
||||||
|
|||||||
@@ -148,14 +148,14 @@ void Tiltrotor::update_vtol_state()
|
|||||||
|
|
||||||
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
|
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
|
||||||
|
|
||||||
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
|
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
|
||||||
&& !_params->airspeed_disabled;
|
&& !_params->airspeed_disabled;
|
||||||
|
|
||||||
bool transition_to_p2 = false;
|
bool transition_to_p2 = false;
|
||||||
|
|
||||||
if (time_since_trans_start > _params->front_trans_time_min) {
|
if (time_since_trans_start > _params->front_trans_time_min) {
|
||||||
if (airspeed_triggers_transition) {
|
if (airspeed_triggers_transition) {
|
||||||
transition_to_p2 = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;
|
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
|
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
|
||||||
@@ -300,19 +300,19 @@ void Tiltrotor::update_transition_state()
|
|||||||
_mc_yaw_weight = 1.0f;
|
_mc_yaw_weight = 1.0f;
|
||||||
|
|
||||||
// reduce MC controls once the plane has picked up speed
|
// reduce MC controls once the plane has picked up speed
|
||||||
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
|
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
|
||||||
_airspeed_validated->equivalent_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
|
_airspeed_validated->calibrated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
|
||||||
_mc_yaw_weight = 0.0f;
|
_mc_yaw_weight = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s) &&
|
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
|
||||||
_airspeed_validated->equivalent_airspeed_m_s >= _params->airspeed_blend) {
|
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend) {
|
||||||
_mc_roll_weight = 1.0f - (_airspeed_validated->equivalent_airspeed_m_s - _params->airspeed_blend) /
|
_mc_roll_weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
|
||||||
(_params->transition_airspeed - _params->airspeed_blend);
|
(_params->transition_airspeed - _params->airspeed_blend);
|
||||||
}
|
}
|
||||||
|
|
||||||
// without airspeed do timed weight changes
|
// without airspeed do timed weight changes
|
||||||
if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)) &&
|
if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
|
||||||
time_since_trans_start > _params->front_trans_time_min) {
|
time_since_trans_start > _params->front_trans_time_min) {
|
||||||
_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
|
_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
|
||||||
(_params->front_trans_time_openloop - _params->front_trans_time_min);
|
(_params->front_trans_time_openloop - _params->front_trans_time_min);
|
||||||
|
|||||||
Reference in New Issue
Block a user