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

Plane: fixed bug in rangefinder landing

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