diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 1292e2204fb..16848e3adb2 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -316,7 +316,6 @@ AirspeedModule::init() if (_airspeed_subs[i].copy(&airspeed)) { _airspeed_device_id[i] = airspeed.device_id; - _time_last_airspeed_update[i] = _time_now_usec; } } @@ -364,6 +363,10 @@ AirspeedModule::check_for_connected_airspeed_sensors() break; } + if (i >= _number_of_airspeed_sensors) { + _time_last_airspeed_update[i] = _time_now_usec; // newly detected: start stale-data timeout now + } + detected_airspeed_sensors = i + 1; } @@ -850,6 +853,9 @@ void AirspeedModule::select_airspeed_and_publish() events::send(events::ID("airspeed_selector_estimation_valid"), events::Log::Info, "Airspeed estimation valid"); + } else if (_prev_airspeed_src == AirspeedSource::DISABLED && airspeed_sensor_added) { + // sensor detected after boot, nothing to report + } else { mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)\t", prev_airspeed_index, valid_airspeed_index); diff --git a/test/mavsdk_tests/test_vtol_rtl.cpp b/test/mavsdk_tests/test_vtol_rtl.cpp index 0c84c524f85..e02080c10bd 100644 --- a/test/mavsdk_tests/test_vtol_rtl.cpp +++ b/test/mavsdk_tests/test_vtol_rtl.cpp @@ -202,7 +202,7 @@ TEST_CASE("Return direct home with approaches", "[vtol]") tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(4); tester.check_rtl_approaches(5., std::chrono::seconds(60)); - tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.wait_until_disarmed(std::chrono::seconds(180)); tester.check_home_within(5.0f); }