yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Minor tweak in wind handling

Browse Source
This commit is contained in:
Julian Kent
2020-07-28 14:51:39 +02:00
committed by Lorenz Meier
parent e847ef1a4d
commit a736ba2435
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/navigator/rtl.cpp
View File

@@ -37,6 +37,7 @@
* *
* @author Julian Oes <julian@oes.ch> * @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Kent <julian@auterion.com>
*/ */
#include "rtl.h" #include "rtl.h"
@@ -685,8 +686,8 @@ float time_to_home(const matrix::Vector3f &vehicle_local_pos,
const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * wind_towards_home).norm(); const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * wind_towards_home).norm();
// Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient
const float cruise_speed = sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + fminf(0, const float cruise_speed = fminf(vehicle_speed_m_s,
wind_towards_home); sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + wind_towards_home);
if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) { if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) {
return INFINITY; // we never reach home if the wind is stronger than vehicle speed return INFINITY; // we never reach home if the wind is stronger than vehicle speed