fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

addressed review comments

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2022-08-02 10:11:48 +03:00 committed by Silvan Fuhrer
parent a425bc4c92
commit f11f2e9797
4 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -42,25 +42,20 @@ bool
WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
const float tas_meas, const float tas_variance)
{
// do no initialise if ground velocity is low
// this should prevent the filter from initialising on the ground
if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) {
return false;
}
if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) {
// initialise wind states assuming zero side slip and horizontal flight
_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_rad);
_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_rad);
_state(INDEX_TAS_SCALE) = _scale_init;
constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty));
constexpr float heading_variance = sq(math::radians(10.0f));
const float cos_heading = cosf(heading_rad);
const float sin_heading = sinf(heading_rad);
// initialise wind states assuming zero side slip and horizontal flight
_state(INDEX_W_N) = velI(0) - tas_meas * cos_heading;
_state(INDEX_W_E) = velI(1) - tas_meas * sin_heading;
_state(INDEX_TAS_SCALE) = _scale_init;
constexpr float initial_sideslip_uncertainty = math::radians(INITIAL_BETA_ERROR_DEG);
const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty));
constexpr float heading_variance = sq(math::radians(INITIAL_HEADING_ERROR_DEG));
// rotate wind velocity into earth frame aligned with vehicle yaw
const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading;
const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading;
@ -83,7 +78,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari
_state.setZero();
_state(INDEX_TAS_SCALE) = 1.0f;
_P.setZero();
_P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR;
_P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = sq(INITIAL_WIND_ERROR);
}
// reset the timestamp for measurement rejection

7
src/lib/wind_estimator/WindEstimator.hpp
View File

@ -98,7 +98,12 @@ private:
INDEX_TAS_SCALE
}; ///< enum which can be used to access state.
static constexpr float INITIAL_WIND_VAR = 5.f; // initial variance for each wind state
static constexpr float INITIAL_WIND_ERROR =
2.5f; // initial uncertainty of each wind state when filter is initialised without airspeed [m/s]
static constexpr float INITIAL_BETA_ERROR_DEG =
15.0f; // sidelip uncertainty used to initialise the filter with a true airspeed measurement [deg]
static constexpr float INITIAL_HEADING_ERROR_DEG =
10.0f; // heading uncertainty used to initialise the filter with a true airspeed measurement [deg]
matrix::Vector3f _state{0.f, 0.f, 1.f};
matrix::Matrix3f _P; ///< state covariance matrix

10
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -185,7 +185,7 @@ private:
(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**< delay to declare airspeed valid again */
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::ASPD_WVAR_THR>) _param_wind_var_threshold
(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
)
void init(); /**< initialization of the airspeed validator instances */
@ -517,7 +517,8 @@ void AirspeedModule::update_wind_estimator_sideslip()
_wind_estimator_sideslip.update(_time_now_usec);
if (_vehicle_local_position_valid
&& _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
&& _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
Quatf q(_vehicle_attitude.q);
@ -543,10 +544,9 @@ void AirspeedModule::update_wind_estimator_sideslip()
void AirspeedModule::update_ground_minus_wind_airspeed()
{
const float wind_variance = sqrtf(_wind_estimate_sideslip.variance_north * _wind_estimate_sideslip.variance_north +
_wind_estimate_sideslip.variance_east * _wind_estimate_sideslip.variance_east);
const float wind_uncertainty = sqrtf(_wind_estimate_sideslip.variance_north + _wind_estimate_sideslip.variance_east);
if (wind_variance < _param_wind_var_threshold.get()) {
if (wind_uncertainty < _param_wind_sigma_max_synth_tas.get()) {
// calculate airspeed estimate based on groundspeed-windspeed
const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;

8
src/modules/airspeed_selector/airspeed_selector_params.c
View File

@ -229,15 +229,15 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);
/**
* Horizontal wind variance threshold for synthetic airspeed.
* Horizontal wind uncertainty threshold for synthetic airspeed.
*
* The synthetic airspeed estimate (from groundspeed and heading) will be declared valid
* as soon as the wind variance drops below this value.
* as soon and as long the horizontal wind uncertainty drops below this value.
*
* @unit (m/s)^2
* @unit m/s
* @min 0.001
* @max 5
* @decimal 3
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_WVAR_THR, 0.3f);
PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f);