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

FW landing check if prev valid before using (#9284)

Browse Source
This commit is contained in:
Daniel Agar
2018-04-21 12:03:46 -04:00
committed by GitHub
parent 4be4b9810e
commit 0e823c82b6
1 changed files with 16 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+16 -17
View File
@@ -1314,8 +1314,13 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
_fw_pos_ctrl_status.abort_landing = false;
}
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1));
const float bearing_airplane_currwp = get_bearing_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1));
float bearing_lastwp_currwp = bearing_airplane_currwp;
if (pos_sp_prev.valid) {
bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
}
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
@@ -1417,16 +1422,6 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
}
}
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = 0.0f;
if (pos_sp_prev.valid) {
L_altitude_rel = pos_sp_prev.alt - terrain_alt;
}
float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
bearing_lastwp_currwp, bearing_airplane_currwp);
/* Check if we should start flaring with a vertical and a
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
@@ -1510,11 +1505,15 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel{0.0f};
float altitude_desired = terrain_alt;
const float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
bearing_lastwp_currwp, bearing_airplane_currwp);
if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
altitude_desired = terrain_alt + landing_slope_alt_rel_desired;
if (!_land_onslope) {
mavlink_log_info(&_mavlink_log_pub, "Landing, on slope");
@@ -1524,16 +1523,16 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
} else {
/* continue horizontally */
if (pos_sp_prev.valid) {
altitude_desired_rel = L_altitude_rel;
altitude_desired = pos_sp_prev.alt;
} else {
altitude_desired_rel = _global_pos.alt - terrain_alt;
altitude_desired = _global_pos.alt;
}
}
const float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
tecs_update_pitch_throttle(altitude_desired,
calculate_target_airspeed(airspeed_approach),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),