diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index 8fa11a8f2a..0852414826 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -118,9 +118,8 @@ void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } -void AdsbConflict::get_traffic_state() +void AdsbConflict::get_traffic_state(hrt_abstime now) { - const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address); const bool old_conflict = (traffic_index >= 0); @@ -130,7 +129,7 @@ void AdsbConflict::get_traffic_state() bool old_conflict_warning_expired = false; if (old_conflict && _conflict_detected) { - old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT); + old_conflict_warning_expired = now > _traffic_buffer.timestamp[traffic_index] + CONFLICT_WARNING_TIMEOUT; } if (new_traffic && _conflict_detected && !_traffic_buffer_full) { @@ -142,7 +141,7 @@ void AdsbConflict::get_traffic_state() } else if (old_conflict && _conflict_detected && old_conflict_warning_expired) { - _traffic_buffer.timestamp[traffic_index] = hrt_absolute_time(); + _traffic_buffer.timestamp[traffic_index] = now; _traffic_state = TRAFFIC_STATE::REMIND_CONFLICT; } else if (old_conflict && !_conflict_detected) { @@ -152,7 +151,6 @@ void AdsbConflict::get_traffic_state() } else { _traffic_state = TRAFFIC_STATE::NO_CONFLICT; } - } void AdsbConflict::remove_expired_conflicts() @@ -172,8 +170,9 @@ void AdsbConflict::remove_expired_conflicts() bool AdsbConflict::handle_traffic_conflict() { + const hrt_abstime now = hrt_absolute_time(); - get_traffic_state(); + get_traffic_state(now); bool take_action = false; @@ -192,7 +191,7 @@ bool AdsbConflict::handle_traffic_conflict() events::send(events::ID("navigator_traffic_resolved"), events::Log::Notice, "Traffic Conflict Resolved {1}!", _transponder_report.icao_address); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; } break; @@ -201,7 +200,7 @@ bool AdsbConflict::handle_traffic_conflict() if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) && (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) { events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on"); - _last_buffer_full_warning_time = hrt_absolute_time(); + _last_buffer_full_warning_time = now; } //disable conflict warnings when buffer is full diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index 9942a6b921..ebe1588161 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -111,7 +111,7 @@ public: void add_icao_address_from_conflict_list(uint32_t icao_address); - void get_traffic_state(); + void get_traffic_state(hrt_abstime now); void set_conflict_detection_params(float crosstrack_separation, float vertical_separation, int collision_time_threshold, uint8_t traffic_avoidance_mode); diff --git a/src/lib/adsb/AdsbConflictTest.cpp b/src/lib/adsb/AdsbConflictTest.cpp index 5d6b6ccbe6..f0ddea70d1 100644 --- a/src/lib/adsb/AdsbConflictTest.cpp +++ b/src/lib/adsb/AdsbConflictTest.cpp @@ -126,17 +126,19 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(used_buffer); bool conflict_detected = false; + hrt_abstime now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 00001; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); conflict_detected = true; + now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); @@ -144,9 +146,10 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(empty_buffer); conflict_detected = true; + now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); @@ -154,25 +157,28 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(full_buffer); conflict_detected = true; + now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL); conflict_detected = false; + now = 2_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); conflict_detected = false; + now = 3_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); @@ -180,9 +186,10 @@ TEST_F(AdsbConflictTest, trafficAlerts) adsb_conflict.set_traffic_buffer(used_buffer); conflict_detected = false; + now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); @@ -235,17 +242,19 @@ TEST_F(AdsbConflictTest, trafficReminder) adsb_conflict.set_traffic_buffer(used_buffer); bool conflict_detected = true; + hrt_abstime now = 200_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); conflict_detected = true; + now = 201_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); @@ -253,33 +262,37 @@ TEST_F(AdsbConflictTest, trafficReminder) adsb_conflict.set_traffic_buffer(full_buffer); conflict_detected = true; + now = 400_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); conflict_detected = true; + now = 401_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); conflict_detected = false; + now = 600_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); conflict_detected = true; + now = 700_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);