fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 15:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: accelerate faster in auto and increase speed at 90degrees angle

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent 540c0bdafb
commit 3f73a56f5a
1 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+12 -12
View File
@@ -905,11 +905,11 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
* angle = 2 -> vel_close = min_cruising_speed */
/* middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle = 1.0 = 90degrees */
float middle_cruise_speed = 2.0f * _min_cruise_speed.get();
float middle_cruise_speed = 3.0f * (_min_cruise_speed.get() + SIGMA_NORM);
/* sanity check: make sure middle cruise speed is always in between min and max*/
middle_cruise_speed = ((_min_cruise_speed.get() < middle_cruise_speed)
&& (get_cruising_speed_xy() > middle_cruise_speed)) ? middle_cruise_speed : _min_cruise_speed.get() + SIGMA_NORM;
middle_cruise_speed = (get_cruising_speed_xy() > middle_cruise_speed) ? middle_cruise_speed : get_cruising_speed_xy() -
SIGMA_NORM;
/* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */
float a = -((middle_cruise_speed - get_cruising_speed_xy()) * (middle_cruise_speed - get_cruising_speed_xy())) /
@@ -1606,22 +1606,22 @@ void MulticopterPositionControl::control_auto(float dt)
} else {
/* we want to accelerate in half the target threshold distance */
float slope = (final_cruise_speed - vel_sp_along_track_prev) / (target_threshold);
vel_sp_along_track = slope * (vec_prev_to_pos.length()) + vel_sp_along_track_prev;
vel_sp_along_track = final_cruise_speed;
/* we want to accelerate not too fast
* TODO: change the name acceleration_hor_man to something that can
* be used by auto and manual */
float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
if (acc_track > _acceleration_hor_manual.get()) {
vel_sp_along_track = _acceleration_hor_manual.get() * dt + vel_sp_along_track_prev;
}
}
/* enforce minimum cruise speed */
vel_sp_along_track = (vel_sp_along_track < SIGMA_NORM) ? SIGMA_NORM : vel_sp_along_track;
vel_sp_along_track = (vel_sp_along_track > final_cruise_speed) ? final_cruise_speed : vel_sp_along_track;
/* take over the previous velocity setpoint along track if larger since we want to accelerate */
if (vel_sp_along_track_prev > vel_sp_along_track) {
vel_sp_along_track = vel_sp_along_track_prev;
}
} else if (close_to_current) {
/* slow down when close to current setpoint */