dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

added option to fix airspeed scaling to 1

This commit is contained in:
Silvan Fuhrer 2019-07-02 09:53:18 +02:00 committed by Beat Küng
parent d38164fc8e
commit 09f29dbb07
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
airdata/WindEstimator.cpp
View File

@ -326,7 +326,13 @@ WindEstimator::run_sanity_checks()
} }
// constrain airspeed scale factor, negative values physically do not make sense // constrain airspeed scale factor, negative values physically do not make sense
_state(tas) = math::max(0.0f, _state(tas)); if (_scale_estimation_on) {
_state(tas) = math::max(0.0f, _state(tas));
} else {
_state(tas) = 1.0f;
}
// attain symmetry // attain symmetry
for (unsigned row = 0; row < 3; row++) { for (unsigned row = 0; row < 3; row++) {

2
airdata/WindEstimator.hpp
View File

@ -88,6 +88,7 @@ public:
void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; } void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; } void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; } void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
void set_scale_estimation_on(bool scale_estimation_on) {_scale_estimation_on = scale_estimation_on; }
private: private:
enum { enum {
@ -120,6 +121,7 @@ private:
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
uint64_t _time_rejected_tas = uint64_t _time_rejected_tas =
0; ///<timestamp of when true airspeed measurements have consistently started to be rejected 0; ///<timestamp of when true airspeed measurements have consistently started to be rejected
bool _scale_estimation_on = false; ///< online scale estimation (IAS-->CAS/EAS) is on
// initialise state and state covariance matrix // initialise state and state covariance matrix
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas); bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);