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

Fix spurious RTL triggers

Browse Source 
Two sources:
1. global to local conversion was sometimes giving issues, so do everything in global
2. on startup the RTL didn't check if the home position was valid before processing it
This commit is contained in:
Julian Kent
2021-01-18 16:26:53 +01:00
committed by Lorenz Meier
parent b5f0a7ea03
commit af8d178ae5
4 changed files with 62 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+27 -30
View File
@@ -156,7 +156,7 @@ void RTL::find_RTL_destination()
}
// compare to safe landing positions
mission_safe_point_s closest_safe_point {} ;
mission_safe_point_s closest_safe_point {};
mission_stats_entry_s stats;
int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
int num_safe_points = 0;
@@ -216,31 +216,22 @@ void RTL::find_RTL_destination()
}
// figure out how long the RTL will take
const vehicle_local_position_s &local_pos_s = *_navigator->get_local_position();
float rtl_xy_speed, rtl_z_speed;
get_rtl_xy_z_speed(rtl_xy_speed, rtl_z_speed);
if (local_pos_s.xy_valid && local_pos_s.z_valid) {
matrix::Vector3f local_pos(local_pos_s.x, local_pos_s.y, local_pos_s.z);
matrix::Vector3f to_destination_vec;
get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
&to_destination_vec(0), &to_destination_vec(1));
to_destination_vec(2) = _destination.alt - global_position.alt;
matrix::Vector3f local_destination(0, 0, global_position.alt); // TODO
float time_to_home_s = time_to_home(to_destination_vec, get_wind(), rtl_xy_speed, rtl_z_speed);
if (!map_projection_initialized(&_projection_reference)) {
map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
}
map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &local_destination(0),
&local_destination(1));
float xy_speed, z_speed;
get_rtl_xy_z_speed(xy_speed, z_speed);
float time_to_home_s = time_to_home(local_pos, local_destination, get_wind(), xy_speed, z_speed);
float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get());
rtl_flight_time_s rtl_flight_time;
rtl_flight_time.timestamp = hrt_absolute_time();
rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
rtl_flight_time.rtl_time_s = time_to_home_s;
_rtl_flight_time_pub.publish(rtl_flight_time);
}
float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get());
rtl_flight_time_s rtl_flight_time{};
rtl_flight_time.timestamp = hrt_absolute_time();
rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
rtl_flight_time.rtl_time_s = time_to_home_s;
_rtl_flight_time_pub.publish(rtl_flight_time);
}
void RTL::on_activation()
@@ -681,16 +672,21 @@ void RTL::get_rtl_xy_z_speed(float &xy, float &z)
matrix::Vector2f RTL::get_wind()
{
_wind_estimate_sub.update();
matrix::Vector2f wind(_wind_estimate_sub.get().windspeed_north, _wind_estimate_sub.get().windspeed_east);
matrix::Vector2f wind;
if (hrt_absolute_time() - _wind_estimate_sub.get().timestamp < 1_s) {
wind(0) = _wind_estimate_sub.get().windspeed_north;
wind(1) = _wind_estimate_sub.get().windspeed_east;
}
return wind;
}
float time_to_home(const matrix::Vector3f &vehicle_local_pos,
const matrix::Vector3f &rtl_point_local_pos,
float time_to_home(const matrix::Vector3f &to_home_vec,
const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s)
{
const matrix::Vector2f to_home = (rtl_point_local_pos - vehicle_local_pos).xy();
const float alt_change = rtl_point_local_pos(2) - vehicle_local_pos(2);
const matrix::Vector2f to_home = to_home_vec.xy();
const float alt_change = to_home_vec(2);
const matrix::Vector2f to_home_dir = to_home.unit_or_zero();
const float dist_to_home = to_home.norm();
@@ -706,6 +702,7 @@ float time_to_home(const matrix::Vector3f &vehicle_local_pos,
}
// assume horizontal and vertical motions happen serially, so their time adds
const float time_to_home = dist_to_home / cruise_speed + fabsf(alt_change) / vehicle_descent_speed_m_s;
return time_to_home;
float horiz = dist_to_home / cruise_speed;
float descent = fabsf(alt_change) / vehicle_descent_speed_m_s;
return horiz + descent;
}