Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: setup target accel in POSITION1 state

This commit is contained in:
Andrew Tridgell 2022-03-03 13:57:00 +11:00 committed by Randy Mackay
parent 5119808e8b
commit 281377065c
2 changed files with 35 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduPlane/quadplane.cpp
View File

@ -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
*/

7
ArduPlane/quadplane.h
View File

@ -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;