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;