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

Plane: fixed a bug in POSITION1 speed thresholds

Browse Source 
we were comparing two different speeds in the threshold for going to
Q_WP_SPEED limit. The reason the two speeds were different was the
wp_nav init happened before the defaults were setup for quadplanes

this fixes both bugs
This commit is contained in:
Andrew Tridgell 2021-11-06 09:49:20 +11:00
parent bb973ac913
commit debae1381c
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/quadplane.cpp
View File

@ -680,7 +680,6 @@ bool QuadPlane::setup(void)
motors->set_update_rate(rc_speed); motors->set_update_rate(rc_speed);
motors->set_interlock(true); motors->set_interlock(true);
attitude_control->parameter_sanity_check(); attitude_control->parameter_sanity_check();
wp_nav->wp_and_spline_init();
// setup the trim of any motors used by AP_Motors so I/O board // setup the trim of any motors used by AP_Motors so I/O board
// failsafe will disable motors // failsafe will disable motors
@ -710,6 +709,9 @@ bool QuadPlane::setup(void)
AP_BoardConfig::allocation_error("transition"); AP_BoardConfig::allocation_error("transition");
} }
// init wp_nav variables after detaults are setup
wp_nav->wp_and_spline_init();
transition->force_transistion_complete(); transition->force_transistion_complete();
// param count will have changed // param count will have changed
@ -2282,7 +2284,7 @@ void QuadPlane::vtol_position_controller(void)
float target_speed = stopping_speed; float target_speed = stopping_speed;
// maximum configured VTOL speed // maximum configured VTOL speed
const float wp_speed = pos_control->get_max_speed_xy_cms() * 0.01; const float wp_speed = wp_nav->get_default_speed_xy() * 0.01;
const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared(); const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared();
const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));