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

fw_pos_control_l1 skip wind > max airspeed logic if airspeed invalid

Browse Source 
- cleanup airspeed validity check logic
This commit is contained in:
Daniel Agar
2019-01-12 14:19:53 +01:00
committed by Lorenz Meier
parent 2e3fa30c83
commit 45d7165eb3
2 changed files with 30 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+29 -28
View File
@@ -374,35 +374,36 @@ FixedwingPositionControl::manual_control_setpoint_poll()
void
FixedwingPositionControl::airspeed_poll()
{
bool airspeed_valid = _airspeed_valid;
if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
const airspeed_s &airspeed = _sub_airspeed.get();
const airspeed_s &as = _sub_airspeed.get();
_airspeed_valid = PX4_ISFINITE(airspeed.indicated_airspeed_m_s)
&& PX4_ISFINITE(airspeed.true_airspeed_m_s)
&& (airspeed.indicated_airspeed_m_s > 0.0f);
if (PX4_ISFINITE(as.indicated_airspeed_m_s)
&& PX4_ISFINITE(as.true_airspeed_m_s)
&& (as.indicated_airspeed_m_s > 0.0f)) {
_airspeed_last_received = hrt_absolute_time();
_airspeed = airspeed.indicated_airspeed_m_s;
airspeed_valid = true;
if (_airspeed_valid
&& airspeed.true_airspeed_m_s > airspeed.indicated_airspeed_m_s) {
_airspeed_last_valid = as.timestamp;
_airspeed = as.indicated_airspeed_m_s;
_eas2tas = max(airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, 1.0f);
} else {
_eas2tas = 1.0f;
_eas2tas = constrain(as.true_airspeed_m_s / as.indicated_airspeed_m_s, 0.9f, 2.0f);
}
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) {
_airspeed_valid = false;
// no airspeed updates for one second
if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
airspeed_valid = false;
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
// update TECS if validity changed
if (airspeed_valid != _airspeed_valid) {
_tecs.enable_airspeed(airspeed_valid);
_airspeed_valid = airspeed_valid;
}
}
void
@@ -820,20 +821,20 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
// l1 navigation logic breaks down when wind speed exceeds max airspeed
// compute 2D groundspeed from airspeed-heading projection
Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
Vector2f nav_speed_2d{0.0f, 0.0f};
Vector2f nav_speed_2d{ground_speed};
// angle between air_speed_2d and ground_speed
float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
if (_airspeed_valid) {
// l1 navigation logic breaks down when wind speed exceeds max airspeed
// compute 2D groundspeed from airspeed-heading projection
const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
nav_speed_2d = air_speed_2d;
// angle between air_speed_2d and ground_speed
const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
} else {
nav_speed_2d = ground_speed;
// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
nav_speed_2d = air_speed_2d;
}
}
/* no throttle limit as default */
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+1 -1
View File
@@ -233,7 +233,7 @@ private:
/* throttle and airspeed states */
bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists
hrt_abstime _airspeed_last_received{0}; ///< last time airspeed was received. Used to detect timeouts.
hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts.
float _airspeed{0.0f};
float _eas2tas{1.0f};