From 281377065cf39af91a2f552e9e3d7884137c204c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Mar 2022 13:57:00 +1100 Subject: [PATCH] Plane: setup target accel in POSITION1 state --- ArduPlane/quadplane.cpp | 48 +++++++++++++++++++++++++---------------- ArduPlane/quadplane.h | 7 ++++-- 2 files changed, 35 insertions(+), 20 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 45d0114c4b..3fd7cf4095 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2097,17 +2097,12 @@ void QuadPlane::update_land_positioning(void) /* run (and possibly init) xy controller */ -void QuadPlane::run_xy_controller(void) +void QuadPlane::run_xy_controller(float accel_limit) { float accel_cmss = wp_nav->get_wp_acceleration(); - if (in_vtol_land_sequence() && - (poscontrol.get_state() == QPOS_POSITION1 || - poscontrol.get_state() == QPOS_POSITION2)) { - // we allow for a bit higher accel limit than the trans decel, - // so if are less likely to overshoot the landing point - // if at some stage in the POS1 we are under velocity - const float accel_margin = 1.25; - accel_cmss = MAX(accel_cmss, accel_margin*transition_decel*100); + if (is_positive(accel_limit)) { + // allow for accel limit override + accel_cmss = MAX(accel_cmss, accel_limit*100); } const float speed_cms = wp_nav->get_default_speed_xy(); pos_control->set_max_speed_accel_xy(speed_cms, accel_cmss); @@ -2154,10 +2149,12 @@ void QuadPlane::poscontrol_init_approach(void) */ void QuadPlane::log_QPOS(void) { - AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist", "QBf", + AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc", "QBfff", AP_HAL::micros64(), poscontrol.get_state(), - plane.auto_state.wp_distance); + plane.auto_state.wp_distance, + poscontrol.target_speed, + poscontrol.target_accel); } /* @@ -2205,10 +2202,10 @@ void QuadPlane::vtol_position_controller(void) uint32_t now_ms = AP_HAL::millis(); // distance that we switch to QPOS_POSITION2 - const float position2_dist_threshold = 5.0; + const float position2_dist_threshold = 10.0; // target speed when we reach position2 threshold - const float position2_target_speed = 2.0; + const float position2_target_speed = 3.0; if (hal.util->get_soft_armed()) { poscontrol.last_run_ms = now_ms; @@ -2398,6 +2395,7 @@ void QuadPlane::vtol_position_controller(void) const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc); const float distance = diff_wp.length(); + const float groundspeed = plane.ahrs.groundspeed(); // calculate speed we should be at to reach the position2 // target speed at the position2 distance threshold, assuming @@ -2431,17 +2429,23 @@ void QuadPlane::vtol_position_controller(void) plane.nav_controller->update_waypoint(plane.current_loc, loc); Vector2f target_speed_xy_cms; + Vector2f target_accel_cms; + const float target_accel = accel_needed(distance, sq(groundspeed)); if (distance > 0.1) { - target_speed_xy_cms = diff_wp.normalized() * target_speed * 100; + Vector2f diff_wp_norm = diff_wp.normalized(); + target_speed_xy_cms = diff_wp_norm * target_speed * 100; + target_accel_cms = diff_wp_norm * (-target_accel*100); } target_speed_xy_cms += landing_velocity * 100; + poscontrol.target_speed = target_speed_xy_cms.length()*0.01; + poscontrol.target_accel = target_accel; // use input shaping and abide by accel and jerk limits - pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f()); + pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms); // run horizontal velocity controller - run_xy_controller(); + run_xy_controller(MAX(target_accel, transition_decel)*1.5); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); @@ -2481,7 +2485,7 @@ void QuadPlane::vtol_position_controller(void) update_land_positioning(); - run_xy_controller(); + run_xy_controller(transition_decel*1.5); // nav roll and pitch are controlled by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); @@ -3495,7 +3499,7 @@ bool QuadPlane::in_transition(void) const /* calculate current stopping distance for a quadplane in fixed wing flight */ -float QuadPlane::stopping_distance(float ground_speed_squared) +float QuadPlane::stopping_distance(float ground_speed_squared) const { // use v^2/(2*accel). This is only quite approximate as the drag // varies with pitch, but it gives something for the user to @@ -3503,6 +3507,14 @@ float QuadPlane::stopping_distance(float ground_speed_squared) return ground_speed_squared / (2 * transition_decel); } +/* + calculate acceleration needed to stop in the given distance given current speed + */ +float QuadPlane::accel_needed(float stop_distance, float ground_speed_squared) const +{ + return ground_speed_squared / (2 * stop_distance); +} + /* calculate current stopping distance for a quadplane in fixed wing flight */ diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 284c6f99da..5ef4039123 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -262,12 +262,13 @@ private: void update_throttle_suppression(void); void run_z_controller(void); - void run_xy_controller(void); + void run_xy_controller(float accel_limit=0.0); void setup_defaults(void); // calculate a stopping distance for fixed-wing to vtol transitions - float stopping_distance(float ground_speed_squared); + float stopping_distance(float ground_speed_squared) const; + float accel_needed(float stop_distance, float ground_speed_squared) const; float stopping_distance(void); // distance below which we don't do approach, based on stopping @@ -446,6 +447,8 @@ private: float pos1_start_speed; Vector2f velocity_match; uint32_t last_velocity_match_ms; + float target_speed; + float target_accel; private: uint32_t last_state_change_ms; enum position_control_state state;