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

FixedwingPositionControl: Explicitly set wind to zero when it is not valid.

Browse Source
This commit is contained in:
Konrad
2023-02-09 17:51:55 +01:00
committed by Silvan Fuhrer
parent de4b139540
commit 92277ebb96
1 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+7 -2
View File
@@ -303,6 +303,11 @@ FixedwingPositionControl::wind_poll()
// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
}
if (!_wind_valid) {
_wind_vel(0) = 0.f;
_wind_vel(1) = 0.f;
}
}
void
@@ -1095,7 +1100,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
0.0f;
navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed,
_wind_vel, curvature);
_wind_vel, curvature);
} else {
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
@@ -1242,7 +1247,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
ground_speed,
_wind_vel);
_wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;