Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed bug in rangefinder landing 

introduced with QLAND change
This commit is contained in:
Andrew Tridgell 2016-03-26 10:06:07 +11:00
parent b4c6a0e30e
commit 8a6e5ffe80
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/altitude.cpp
View File

@ -557,7 +557,6 @@ float Plane::rangefinder_correction(void)
void Plane::rangefinder_height_update(void)
{
float distance = rangefinder.distance_cm()*0.01f;
float height_estimate = 0;
if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
if (!rangefinder_state.have_initial_reading) {
rangefinder_state.have_initial_reading = true;
@ -596,14 +595,14 @@ void Plane::rangefinder_height_update(void)
if (rangefinder_state.in_range) {
// base correction is the difference between baro altitude and
// rangefinder estimate
float correction = relative_altitude() - height_estimate;
float correction = relative_altitude() - rangefinder_state.height_estimate;
#if AP_TERRAIN_AVAILABLE
// if we are terrain following then correction is based on terrain data
float terrain_altitude;
if ((target_altitude.terrain_following || g.terrain_follow) &&
terrain.height_above_terrain(terrain_altitude, true)) {
correction = terrain_altitude - height_estimate;
correction = terrain_altitude - rangefinder_state.height_estimate;
}
#endif
@ -617,7 +616,7 @@ void Plane::rangefinder_height_update(void)
if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) {
// the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar
if (rangefinder_state.in_use) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)height_estimate);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)rangefinder_state.height_estimate);
}
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
}