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

Copter: upward surface tracking never closer than avoidance margin

Browse Source
This commit is contained in:
Randy Mackay 2019-09-20 11:07:53 +09:00
parent 0d032ecd86
commit a5d0f55bdc
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/surface_tracking.cpp
View File

@ -39,6 +39,12 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
}
valid_for_logging = true;
// upward facing terrain following never gets closer than avoidance margin
if (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) {
const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f;
target_dist_cm = MAX(target_dist_cm, margin_cm);
}
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error);
float velocity_correction = dir * distance_error * copter.g.rangefinder_gain;