diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index f052965c6f..0e0fc0a605 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -214,14 +214,10 @@ void AP_Proximity::init(void) void AP_Proximity::update(void) { for (uint8_t i=0; iupdate(); + if (!valid_instance(i)) { + continue; } + drivers[i]->update(); } // work out primary instance - first sensor returning good data @@ -235,7 +231,7 @@ void AP_Proximity::update(void) // return sensor orientation uint8_t AP_Proximity::get_orientation(uint8_t instance) const { - if (instance >= PROXIMITY_MAX_INSTANCES) { + if (!valid_instance(instance)) { return 0; } @@ -245,7 +241,7 @@ uint8_t AP_Proximity::get_orientation(uint8_t instance) const // return sensor yaw correction int16_t AP_Proximity::get_yaw_correction(uint8_t instance) const { - if (instance >= PROXIMITY_MAX_INSTANCES) { + if (!valid_instance(instance)) { return 0; } @@ -256,7 +252,7 @@ int16_t AP_Proximity::get_yaw_correction(uint8_t instance) const AP_Proximity::Proximity_Status AP_Proximity::get_status(uint8_t instance) const { // sanity check instance number - if (instance >= num_instances) { + if (!valid_instance(instance)) { return Proximity_NotConnected; }