From e489c3184c2526d091a3eae533d26e43f918fc31 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 11 May 2016 15:31:58 +0900 Subject: [PATCH] Copter: rangefinder.enabled false if no range finders are configured --- ArduCopter/Copter.h | 2 +- ArduCopter/sensors.cpp | 2 ++ ArduCopter/switches.cpp | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f923afc235..3d0f958a80 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -177,7 +177,7 @@ private: int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder uint32_t last_healthy_ms; LowPassFilterFloat alt_cm_filt; // altitude filter - } rangefinder_state = { true, false, 0, 0 }; + } rangefinder_state = { false, false, 0, 0 }; #endif AP_RPM rpm_sensor; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 8f12e9ca44..1875a511c2 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -31,6 +31,7 @@ void Copter::init_rangefinder(void) #if RANGEFINDER_ENABLED == ENABLED rangefinder.init(); rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); + rangefinder_state.enabled = (rangefinder.num_sensors() >= 1); #endif } @@ -68,6 +69,7 @@ void Copter::read_rangefinder(void) wp_nav.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); #else + rangefinder_state.enabled = false; rangefinder_state.alt_healthy = false; rangefinder_state.alt_cm = 0; #endif diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index f00e7078f5..f235ef485d 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -348,7 +348,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_RANGEFINDER: // enable or disable the rangefinder #if RANGEFINDER_ENABLED == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { + if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) { rangefinder_state.enabled = true; }else{ rangefinder_state.enabled = false;