From d251c51ca135ffef597cb1dfd800fcb875fba6bf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 31 May 2022 02:38:55 +0100 Subject: [PATCH] Plane: Quadplane: QRTL climb to QRTL alt first if in Q mode --- ArduPlane/mode.h | 7 +++ ArduPlane/mode_qrtl.cpp | 123 +++++++++++++++++++++++++++++++--------- ArduPlane/quadplane.cpp | 3 +- 3 files changed, 104 insertions(+), 29 deletions(-) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 4a95f6d8d6..37e68ad94f 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -599,6 +599,13 @@ public: protected: bool _enter() override; + +private: + + enum class SubMode { + climb, + RTL, + } submode; }; class ModeQAcro : public Mode diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 1c59174461..138cf62406 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -11,28 +11,52 @@ bool ModeQRTL::_enter() plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL); return true; } + submode = SubMode::RTL; + plane.prev_WP_loc = plane.current_loc; + + // clear QPOS state + quadplane.poscontrol.set_state(QuadPlane::QPOS_NONE); + + const int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; + if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { + // VTOL motors are active, either in VTOL flight or assisted flight + Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + + const float dist = plane.current_loc.get_distance(destination); + const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)); + + const float dist_to_climb = quadplane.qrtl_alt - plane.relative_ground_altitude(plane.g.rangefinder_landing); + if (dist < 1.5*radius) { + // we're close to destination and already running VTOL motors, don't transition and don't climb + gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius); + poscontrol.set_state(QuadPlane::QPOS_POSITION1); + + } else if (is_positive(dist_to_climb)) { + // climb before returning, only next waypoint altitude is used + submode = SubMode::climb; + plane.next_WP_loc = plane.current_loc; +#if AP_TERRAIN_AVAILABLE + if (plane.terrain_enabled_in_mode(mode_number())) { + plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt * 100UL, Location::AltFrame::ABOVE_TERRAIN); + return true; + } +#endif + plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame()); + return true; + } + } // use do_RTL() to setup next_WP_loc - plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL); - plane.prev_WP_loc = plane.current_loc; - pos_control->set_accel_desired_xy_cmss(Vector2f()); - pos_control->init_xy_controller(); + plane.do_RTL(RTL_alt_abs_cm); quadplane.poscontrol_init_approach(); - float dist = plane.next_WP_loc.get_distance(plane.current_loc); - const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)); - if (dist < 1.5*radius && - quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { - // we're close to destination and already running VTOL motors, don't transition - gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius); - poscontrol.set_state(QuadPlane::QPOS_POSITION1); - } + int32_t from_alt; int32_t to_alt; if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) { poscontrol.slow_descent = from_alt > to_alt; return true; } - // defualt back to old method + // default back to old method poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); return true; } @@ -47,20 +71,63 @@ void ModeQRTL::update() */ void ModeQRTL::run() { - quadplane.vtol_position_controller(); - if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) { - // change target altitude to home alt - plane.next_WP_loc.alt = plane.home.alt; - } - if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) { - // start landing logic - quadplane.verify_vtol_land(); - } + switch (submode) { + case SubMode::climb: { + // request zero velocity + Vector2f vel, accel; + pos_control->input_vel_accel_xy(vel, accel); + quadplane.run_xy_controller(); - // when in approach allow stick mixing - if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE || - quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) { - plane.stabilize_stick_mixing_fbw(); + // nav roll and pitch are controller by position controller + plane.nav_roll_cd = pos_control->get_roll_cd(); + plane.nav_pitch_cd = pos_control->get_pitch_cd(); + if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { + pos_control->set_externally_limited_xy(); + } + // weathervane with no pilot input + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, + plane.nav_pitch_cd, + quadplane.get_weathervane_yaw_rate_cds()); + + // climb at full WP nav speed + quadplane.set_climb_rate_cms(quadplane.wp_nav->get_default_speed_up(), false); + quadplane.run_z_controller(); + + ftype alt_diff; + if (!plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) { + // climb finshed or cant get alt diff, head home + submode = SubMode::RTL; + plane.prev_WP_loc = plane.current_loc; + plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL); + quadplane.poscontrol_init_approach(); + if (plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff)) { + poscontrol.slow_descent = is_positive(alt_diff); + } else { + // default back to old method + poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); + } + } + break; + } + + case SubMode::RTL: { + quadplane.vtol_position_controller(); + if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) { + // change target altitude to home alt + plane.next_WP_loc.alt = plane.home.alt; + } + if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) { + // start landing logic + quadplane.verify_vtol_land(); + } + + // when in approach allow stick mixing + if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE || + quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) { + plane.stabilize_stick_mixing_fbw(); + } + break; + } } } @@ -72,7 +139,7 @@ bool ModeQRTL::update_target_altitude() /* update height target in approach */ - if (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH) { + if ((submode != SubMode::RTL) || (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH)) { return false; } @@ -99,7 +166,7 @@ bool ModeQRTL::update_target_altitude() // only nudge during approach bool ModeQRTL::allows_throttle_nudging() const { - return plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH; + return (submode == SubMode::RTL) && (plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH); } #endif diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b8b60cdf1a..5626ebdfba 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2187,6 +2187,7 @@ void QuadPlane::poscontrol_init_approach(void) } poscontrol.pilot_correction_done = false; poscontrol.xy_correction.zero(); + poscontrol.slow_descent = false; } /* @@ -3473,13 +3474,13 @@ void QuadPlane::guided_start(void) setup_target_position(); int32_t from_alt; int32_t to_alt; + poscontrol_init_approach(); if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) { poscontrol.slow_descent = from_alt > to_alt; } else { // default back to old method poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); } - poscontrol_init_approach(); } /*