fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 01:13:33 +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 15:29:25 +02:00
committed by Silvan Fuhrer
parent a425bc4c92
commit f11f2e9797
4 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/airspeed_selector_main.cpp
+5 -5
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;
src/modules/airspeed_selector/airspeed_selector_params.c
+4 -4
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);