5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

Plane: use WP height for height above ground in vtol landing

this allows for landings above or below the takeoff height without
requiring the use of terrain data. It allows both the use of vfwd
motor for holding against wind, and the correct height for land final
descent rate
This commit is contained in:
Andrew Tridgell 2019-02-24 13:53:52 +11:00
parent 4b33865ee2
commit bc95c25cc9
3 changed files with 51 additions and 3 deletions

View File

@ -137,6 +137,13 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
return altitude;
}
#endif
if (quadplane.in_vtol_land_descent()) {
// when doing a VTOL landing we can use the waypoint height as
// ground height
return height_above_target();
}
return relative_altitude;
}

View File

@ -2289,7 +2289,15 @@ bool QuadPlane::verify_vtol_land(void)
plane.auto_state.wp_distance < 2) {
poscontrol.state = QPOS_LAND_DESCEND;
gcs().send_text(MAV_SEVERITY_INFO,"Land descend started");
plane.set_next_WP(plane.next_WP_loc);
if (plane.control_mode == AUTO) {
// set height to mission height, so we can use the mission
// WP height for triggering land final if no rangefinder
// available
plane.set_next_WP(plane.mission.get_current_nav_cmd().content.location);
} else {
plane.set_next_WP(plane.next_WP_loc);
plane.next_WP_loc.alt = ahrs.get_home().alt;
}
}
// at land_final_alt begin final landing
@ -2402,8 +2410,7 @@ int8_t QuadPlane::forward_throttle_pct(void)
int8_t fwd_throttle_min = (plane.aparm.throttle_min <= 0) ? 0 : plane.aparm.throttle_min;
vel_forward.integrator = constrain_float(vel_forward.integrator, fwd_throttle_min, plane.aparm.throttle_max);
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_POSITION1 || poscontrol.state == QPOS_POSITION2)) {
if (in_vtol_land_approach()) {
// when we are doing horizontal positioning in a VTOL land
// we always allow the fwd motor to run. Otherwise a bad
// lidar could cause the aircraft not to be able to
@ -2683,3 +2690,27 @@ void QuadPlane::update_throttle_thr_mix(void)
}
}
}
/*
see if we are in the approach phase of a VTOL landing
*/
bool QuadPlane::in_vtol_land_approach(void) const
{
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_POSITION1 || poscontrol.state == QPOS_POSITION2)) {
return true;
}
return false;
}
/*
see if we are in the descent phase of a VTOL landing
*/
bool QuadPlane::in_vtol_land_descent(void) const
{
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_LAND_DESCEND || poscontrol.state == QPOS_LAND_FINAL)) {
return true;
}
return false;
}

View File

@ -466,6 +466,16 @@ private:
return true if current mission item is a vtol landing
*/
bool is_vtol_land(uint16_t id) const;
/*
are we in the approach phase of a VTOL landing?
*/
bool in_vtol_land_approach(void) const;
/*
are we in the descent phase of a VTOL landing?
*/
bool in_vtol_land_descent(void) const;
public:
void motor_test_output();