From 3f73a56f5a0af0810c86c9ccb4b4647c6001b00c Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 1 Jun 2017 14:59:17 +0200 Subject: [PATCH] mc_pos_control: accelerate faster in auto and increase speed at 90degrees angle --- .../mc_pos_control/mc_pos_control_main.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 20e0a606c8..03ce9cc47e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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 */