From 21ff3853f11c25b46bf0af61ec77b982c19730a3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Jan 2013 21:17:43 +0900
Subject: [PATCH] Copter: bug fix to set sonar_alt_health to zero when disabled
---
ArduCopter/ArduCopter.pde | 6 ++++--
ArduCopter/sensors.pde | 6 ++++++
2 files changed, 10 insertions(+), 2 deletions(-)
diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index ef6dd9286..91a1e91fe 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -2056,8 +2056,10 @@ static void update_altitude()
baro_rate = constrain(baro_rate, -500, 500);
// read in sonar altitude and calculate sonar rate
- if(g.sonar_enabled) {
- sonar_alt = read_sonar();
+ sonar_alt = read_sonar();
+ // start calculating the sonar_rate as soon as valid sonar readings start coming in so that we are ready when the sonar_alt_health becomes 3
+ // Note: post 2.9.1 release we will remove the sonar_rate variable completely
+ if(sonar_alt_health > 1) {
sonar_rate = (sonar_alt - old_sonar_alt) * 10;
sonar_rate = constrain(sonar_rate, -150, 150);
}
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 5f4b607e6..0b5324bad 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -35,6 +35,12 @@ static int32_t read_barometer(void)
static int16_t read_sonar(void)
{
#if CONFIG_SONAR == ENABLED
+ // exit immediately if sonar is disabled
+ if( !g.sonar_enabled ) {
+ sonar_alt_health = 0;
+ return 0;
+ }
+
int16_t temp_alt = sonar.read();
if(temp_alt >= sonar.min_distance && temp_alt <= sonar.max_distance * 0.70) {
--
GitLab