From aa904d6bbc063b7b04298a718ed1860f880331c1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jun 2021 18:56:59 +1000 Subject: [PATCH] Plane: fixed guided mode handling with new approach code ensure we treat guided approach as not a vtol mode --- ArduPlane/quadplane.cpp | 75 ++++++++++++++++++++++++++++------------- ArduPlane/quadplane.h | 10 ++---- 2 files changed, 54 insertions(+), 31 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 37126b18d5..3a97aca87b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2052,6 +2052,8 @@ void QuadPlane::update(void) const uint32_t now = AP_HAL::millis(); if (!in_vtol_mode()) { + // we're in a fixed wing mode, cope with transitions and check + // for assistance needed update_transition(); } else { @@ -2455,7 +2457,7 @@ bool QuadPlane::in_vtol_auto(void) const case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TO_ALT: - return plane.auto_state.vtol_loiter; + return plane.auto_state.vtol_loiter && poscontrol.get_state() > QPOS_APPROACH; case MAV_CMD_NAV_TAKEOFF: return is_vtol_takeoff(id); case MAV_CMD_NAV_VTOL_LAND: @@ -2485,7 +2487,8 @@ bool QuadPlane::in_vtol_mode(void) const } return (plane.control_mode->is_vtol_mode() || (plane.control_mode->is_guided_mode() - && plane.auto_state.vtol_loiter) || + && plane.auto_state.vtol_loiter && + poscontrol.get_state() > QPOS_APPROACH) || in_vtol_auto()); } @@ -2502,7 +2505,8 @@ bool QuadPlane::in_vtol_posvel_mode(void) const plane.control_mode == &plane.mode_qrtl || plane.control_mode == &plane.mode_qautotune || (plane.control_mode->is_guided_mode() && - plane.auto_state.vtol_loiter) || + plane.auto_state.vtol_loiter && + poscontrol.get_state() > QPOS_APPROACH) || in_vtol_auto()); } @@ -2558,20 +2562,29 @@ void QuadPlane::run_xy_controller(void) */ void QuadPlane::poscontrol_init_approach(void) { - poscontrol.start_closing_vel = landing_closing_velocity().length(); - poscontrol.start_dist = plane.current_loc.get_distance(plane.next_WP_loc); if ((options & OPTION_DISABLE_APPROACH) != 0) { // go straight to QPOS_POSITION1 poscontrol.set_state(QPOS_POSITION1); } else if (poscontrol.get_state() != QPOS_APPROACH) { - gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach v=%.1f d=%.1f", - poscontrol.start_closing_vel, - poscontrol.start_dist); + const float dist = plane.current_loc.get_distance(plane.next_WP_loc); + gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist); poscontrol.set_state(QPOS_APPROACH); poscontrol.thrust_loss_start_ms = 0; } } +/* + change position control state + */ +void QuadPlane::PosControlState::set_state(enum position_control_state s) +{ + state = s; + last_state_change_ms = AP_HAL::millis(); + if (state == QPOS_POSITION1) { + speed_scale = 0; + } +} + /* main landing controller. Used for landing and RTL. */ @@ -2582,6 +2595,7 @@ void QuadPlane::vtol_position_controller(void) } const Location &loc = plane.next_WP_loc; + uint32_t now_ms = AP_HAL::millis(); check_attitude_relax(); @@ -2589,6 +2603,17 @@ void QuadPlane::vtol_position_controller(void) switch (poscontrol.get_state()) { case QPOS_APPROACH: + if (in_vtol_mode()) { + // this means we're not running update_transition() and + // thus not doing qassist checking, force POSITION1 mode + // now. We don't expect this to trigger, it is a failsafe + // for a logic error + gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 nvtol"); + poscontrol.set_state(QPOS_POSITION1); + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + FALLTHROUGH; + case QPOS_AIRBRAKE: { float aspeed; const Vector2f closing_vel = landing_closing_velocity(); @@ -2650,7 +2675,7 @@ void QuadPlane::vtol_position_controller(void) /* we must switch to POSITION1 if our airspeed drops below the - assist speed. We additionally switch tp POSITION1 if we are + assist speed. We additionally switch to POSITION1 if we are too far above our desired velocity profile, or our attitude has deviated too much */ @@ -2673,7 +2698,7 @@ void QuadPlane::vtol_position_controller(void) // switch to vfwd for throttle control vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - vel_forward.last_ms = AP_HAL::millis(); + vel_forward.last_ms = now_ms; } if (tilt.tilt_mask == 0 && !is_tailsitter()) { @@ -2686,11 +2711,10 @@ void QuadPlane::vtol_position_controller(void) if (throttle_saturated && motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED && plane.auto_state.sink_rate > 0.2 && aspeed < aspeed_threshold+4) { - const uint32_t now = AP_HAL::millis(); if (poscontrol.thrust_loss_start_ms == 0) { - poscontrol.thrust_loss_start_ms = now; + poscontrol.thrust_loss_start_ms = now_ms; } - if (now - poscontrol.thrust_loss_start_ms > 5000) { + if (now_ms - poscontrol.thrust_loss_start_ms > 5000) { gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 thrust loss as=%.1f at=%.1f", aspeed, aspeed_threshold); poscontrol.set_state(QPOS_POSITION1); @@ -2801,8 +2825,7 @@ void QuadPlane::vtol_position_controller(void) attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); - if (plane.auto_state.wp_proportion >= 1 || - plane.auto_state.wp_distance < 5) { + if (plane.auto_state.wp_distance < 5) { poscontrol.set_state(QPOS_POSITION2); poscontrol.pilot_correction_done = false; gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f", @@ -2897,7 +2920,7 @@ void QuadPlane::vtol_position_controller(void) break; } } - if (plane.control_mode == &plane.mode_qrtl || plane.control_mode == &plane.mode_guided || vtol_loiter_auto) { + if (plane.control_mode == &plane.mode_guided || vtol_loiter_auto) { plane.ahrs.get_position(plane.current_loc); int32_t target_altitude_cm; if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME,target_altitude_cm)) { @@ -2949,6 +2972,15 @@ void QuadPlane::vtol_position_controller(void) } run_z_controller(); + + if (now_ms - poscontrol.last_log_ms >= 40) { + // log poscontrol at 25Hz + poscontrol.last_log_ms = now_ms; + AP::logger().Write("QPOS", "TimeUS,State,Dist", "QBf", + AP_HAL::micros64(), + poscontrol.get_state(), + plane.auto_state.wp_distance); + } } @@ -3110,7 +3142,6 @@ void QuadPlane::init_qrtl(void) // use do_RTL() to setup next_WP_loc plane.do_RTL(plane.home.alt + qrtl_alt*100UL); plane.prev_WP_loc = plane.current_loc; - poscontrol.speed_scale = 0; pos_control->set_accel_desired_xy_cmss(Vector2f()); pos_control->init_xy_controller(); poscontrol_init_approach(); @@ -3206,7 +3237,6 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) // initially aim for current altitude plane.next_WP_loc.alt = plane.current_loc.alt; poscontrol.set_state(QPOS_POSITION1); - poscontrol.speed_scale = 0; // initialise the position controller pos_control->init_xy_controller(); @@ -3624,18 +3654,16 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) */ void QuadPlane::guided_start(void) { - poscontrol.set_state(QPOS_POSITION1); - poscontrol.speed_scale = 0; guided_takeoff = false; setup_target_position(); 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; + } else { + // default back to old method + poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); } - // default back to old method - poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); poscontrol_init_approach(); } @@ -3978,7 +4006,6 @@ Vector2f QuadPlane::landing_desired_closing_velocity() // base target speed based on sqrt of distance float target_speed = safe_sqrt(2*transition_decel*dist); - target_speed = MIN(target_speed, poscontrol.start_closing_vel); Vector2f target_speed_xy = diff_wp.normalized() * target_speed; return target_speed_xy; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 1f85326281..5707570de1 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -467,15 +467,12 @@ private: QPOS_LAND_FINAL, QPOS_LAND_COMPLETE }; - struct { + class PosControlState { public: enum position_control_state get_state() const { return state; } - void set_state(enum position_control_state s) { - state = s; - last_state_change_ms = AP_HAL::millis(); - } + void set_state(enum position_control_state s); uint32_t time_since_state_start_ms() const { return AP_HAL::millis() - last_state_change_ms; } @@ -487,9 +484,8 @@ private: bool slow_descent:1; bool pilot_correction_active; bool pilot_correction_done; - float start_closing_vel; - float start_dist; uint32_t thrust_loss_start_ms; + uint32_t last_log_ms; private: uint32_t last_state_change_ms; enum position_control_state state;