diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index 9a31e0fea59..358a1bb6b7c 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -82,7 +82,9 @@ void UavcanRangefinderBridge::range_sub_cb(const return; } - if (!_inited) { + const int8_t channel_idx = get_channel_index_for_node(msg.getSrcNodeID().get()); + + if (channel_idx >= 0 && !_channel_initialized[channel_idx]) { uint8_t rangefinder_type = 0; @@ -107,7 +109,7 @@ void UavcanRangefinderBridge::range_sub_cb(const rangefinder->set_min_distance(_range_min_m); rangefinder->set_max_distance(_range_max_m); - _inited = true; + _channel_initialized[channel_idx] = true; } // TOO_CLOSE, TOO_FAR and UNDEFINED readings do not carry a usable range diff --git a/src/drivers/uavcan/sensors/rangefinder.hpp b/src/drivers/uavcan/sensors/rangefinder.hpp index 7c50bcdcb24..4fd61980ca6 100644 --- a/src/drivers/uavcan/sensors/rangefinder.hpp +++ b/src/drivers/uavcan/sensors/rangefinder.hpp @@ -70,6 +70,6 @@ private: float _range_min_m{0.0f}; float _range_max_m{0.0f}; - bool _inited{false}; + bool _channel_initialized[DEFAULT_MAX_CHANNELS] {}; };