aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: improved POSITION1 speed profile

Browse Source 
allow acceleration up to the Q_WP_SPEED, and assume a decel profile
matching Q_TRANS_DECEL for approach speed
This commit is contained in:
Andrew Tridgell 2021-06-10 10:41:35 +10:00
parent 5f8e90cd6f
commit 74ac9964f1
2 changed files with 26 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
ArduPlane/quadplane.cpp
View File

@ -2591,9 +2591,6 @@ 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;
}
}
/*
@ -2608,6 +2605,12 @@ void QuadPlane::vtol_position_controller(void)
const Location &loc = plane.next_WP_loc;
uint32_t now_ms = AP_HAL::millis();
// distance that we switch to QPOS_POSITION2
const float position2_dist_threshold = 5.0;
// target speed when we reach position2 threshold
const float position2_target_speed = 2.0;
check_attitude_relax();
// horizontal position control
@ -2760,47 +2763,31 @@ void QuadPlane::vtol_position_controller(void)
const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
const float distance = diff_wp.length();
Vector2f groundspeed = ahrs.groundspeed_vector();
float speed_towards_target = distance>1?(diff_wp.normalized() * groundspeed):0;
if (poscontrol.speed_scale <= 0) {
// initialise scaling so we start off targeting our
// current linear speed towards the target. If this is
// less than the wpnav speed then the wpnav speed is used
// land_speed_scale is then used to linearly change
// velocity as we approach the waypoint, aiming for zero
// speed at the waypoint
// setup land_speed_scale so at current distance we
// maintain speed towards target, and slow down as we
// approach
// max_speed will control how fast we will fly. It will always decrease
poscontrol.max_speed = MAX(speed_towards_target, wp_nav->get_default_speed_xy() * 0.01);
poscontrol.speed_scale = poscontrol.max_speed / MAX(distance, 1);
// calculate speed we should be at to reach the position2
// target speed at the position2 distance threshold, assuming
// Q_TRANS_DECEL is correct
const float stopping_speed = safe_sqrt(MAX(0, distance-position2_dist_threshold) * 2 * transition_decel) + position2_target_speed;
float target_speed = stopping_speed;
// maximum configured VTOL speed
const float wp_speed = pos_control->get_max_speed_xy_cms() * 0.01;
const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared();
if (current_speed_sq < sq(wp_speed)) {
// if we are below the WP speed then don't ask for more
// than WP speed. The POSITION1 controller can be used
// when faster than WP speed if we are coming from fixed
// wing flight, but we don't want to accelerate to speeds
// beyond the configured max VTOL speed
target_speed = MIN(target_speed, wp_speed);
}
// run fixed wing navigation
plane.nav_controller->update_waypoint(plane.current_loc, loc);
/*
calculate target velocity, not dropping it below 2m/s
*/
const float final_speed = 2.0f;
Vector2f target_speed_xy = diff_wp * poscontrol.speed_scale;
float target_speed = target_speed_xy.length();
if (distance < 1) {
// prevent numerical error before switching to POSITION2
target_speed_xy = {0.1, 0.1};
}
if (target_speed < final_speed) {
// until we enter the loiter we always aim for at least 2m/s
target_speed_xy = target_speed_xy.normalized() * final_speed;
poscontrol.max_speed = final_speed;
} else if (target_speed > poscontrol.max_speed) {
// we never speed up during landing approaches
target_speed_xy = target_speed_xy.normalized() * poscontrol.max_speed;
} else {
poscontrol.max_speed = target_speed;
}
Vector2f target_speed_xy = diff_wp.normalized() * target_speed;
pos_control->set_vel_desired_xy_cms(target_speed_xy * 100);
// reset position controller xy target to current position
@ -2841,7 +2828,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_distance < 5) {
if (plane.auto_state.wp_distance < position2_dist_threshold) {
poscontrol.set_state(QPOS_POSITION2);
poscontrol.pilot_correction_done = false;
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f",

3
ArduPlane/quadplane.h
View File

@ -477,9 +477,6 @@ private:
uint32_t time_since_state_start_ms() const {
return AP_HAL::millis() - last_state_change_ms;
}
float speed_scale;
Vector2f target_velocity;
float max_speed;
Vector3f target_cm;
Vector3f target_vel_cms;
bool slow_descent:1;