diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index da0a83e46c..12b26421d6 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -137,6 +137,16 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available) return altitude; } #endif + + if (quadplane.in_vtol_land_descent() && + !(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)) { + // when doing a VTOL landing we can use the waypoint height as + // ground height. We can't do this if using the + // LAND_FW_APPROACH as that uses the wp height as the approach + // height + return height_above_target(); + } + return relative_altitude; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 203d4a03cb..9862c21491 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2521,7 +2521,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 @@ -2640,8 +2648,7 @@ int8_t QuadPlane::forward_throttle_pct(void) int8_t fwd_throttle_min = plane.have_reverse_thrust() ? 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 @@ -2933,3 +2940,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; +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c4e7c847e1..cd3ce32176 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -499,6 +499,16 @@ private: QAutoTune qautotune; #endif + /* + 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(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,