Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Improve surface tracking dynamics

Browse Source 
This little fix will allow much higher sonar_gain without the
oscillating effect it used to bring.
This commit is contained in:
Ju1ien 2014-06-06 15:26:58 +09:00 committed by Randy Mackay
parent 4f75cbcce0
commit fd9502d3fa
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Attitude.pde
View File

@ -208,8 +208,8 @@ static float get_throttle_surface_tracking(int16_t target_rate, float current_al
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z());
// calc desired velocity correction from target sonar alt vs actual sonar alt
distance_error = target_sonar_alt-sonar_alt;
// calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_loc.alt);
velocity_correction = distance_error * g.sonar_gain;
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);