fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 00:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

adsb: warnings fixes & remove UTM_GLOBAL_POSITION (#21663)

Browse Source 
- warn about full traffic conflict buffer at 1/60hz.
- add conflict expiry for buffer.
- use only events for buffer full warning. mavlink_log_critical no longer needed.
- use icao address for conflict warnings id, stop using uas_id. UTM_GLOBAL_POSITION assumed deprecated.
- stop spamming when buffer is full
- fix warning wording if buffer is full.
- remove UTM_GLOBAL_POSITION

Fixes failing unit test:
* [adsbTest] Reduce conflict timestamps - not enough time has passed in ci
- failed ci output - (passes locally with make tests TESTFILTER=AdsbConflict)
- Timestamp: 6000000000
- Time now: 457720038
- Time since timestamp: 0
- Old Conflict warning expired: 0
- --------------------
- adsb_conflict._traffic_state 0
- ../../src/lib/adsb/AdsbConflictTest.cpp:244: Failure
- Value of: adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT
-   Actual: false
- Expected: true
This commit is contained in:
asimopunov
2024-04-22 10:14:39 +02:00
committed by GitHub
parent c7725d74b4
commit f95a2021cd
7 changed files with 130 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/adsb/AdsbConflict.cpp
+101 -92
View File
@@ -108,12 +108,14 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
{ {
_traffic_buffer.icao_address.remove(traffic_index); _traffic_buffer.icao_address.remove(traffic_index);
_traffic_buffer.timestamp.remove(traffic_index); _traffic_buffer.timestamp.remove(traffic_index);
PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size());
} }
void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address)
{ {
_traffic_buffer.timestamp.push_back(hrt_absolute_time()); _traffic_buffer.timestamp.push_back(hrt_absolute_time());
_traffic_buffer.icao_address.push_back(icao_address); _traffic_buffer.icao_address.push_back(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()
@@ -129,7 +131,6 @@ void AdsbConflict::get_traffic_state()
if (old_conflict && _conflict_detected) { if (old_conflict && _conflict_detected) {
old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT); old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT);
} }
if (new_traffic && _conflict_detected && !_traffic_buffer_full) { if (new_traffic && _conflict_detected && !_traffic_buffer_full) {
@@ -154,65 +155,56 @@ void AdsbConflict::get_traffic_state()
} }
void AdsbConflict::remove_expired_conflicts()
{
for (uint8_t traffic_index = 0; traffic_index < _traffic_buffer.timestamp.size();) {
if (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > TRAFFIC_CONFLICT_LIFETIME) {
events::send<uint32_t>(events::ID("navigator_traffic_expired"), events::Log::Notice,
"Traffic Conflict {1} Expired and removed from buffer",
_traffic_buffer.icao_address[traffic_index]);
remove_icao_address_from_conflict_list(traffic_index);
} else {
traffic_index++;
}
}
}
bool AdsbConflict::handle_traffic_conflict() bool AdsbConflict::handle_traffic_conflict()
{ {
get_traffic_state(); get_traffic_state();
char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages
//convert UAS_id byte array to char array for User Warning
for (int i = 0; i < 5; i++) {
snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]);
}
uint64_t uas_id_int = 0;
for (int i = 0; i < 8; i++) {
uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
}
bool take_action = false; bool take_action = false;
switch (_traffic_state) { switch (_traffic_state) {
case TRAFFIC_STATE::ADD_CONFLICT: case TRAFFIC_STATE::ADD_CONFLICT:
case TRAFFIC_STATE::REMIND_CONFLICT: { case TRAFFIC_STATE::REMIND_CONFLICT: {
take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f),
take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180, (int)fabsf(_crosstrack_error.distance), _transponder_report.flags,
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
_transponder_report.callsign, _transponder_report.callsign,
uas_id_int); _transponder_report.icao_address);
} }
break; break;
case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: { case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {
events::send<uint32_t>(events::ID("navigator_traffic_resolved"), events::Log::Notice,
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!", "Traffic Conflict Resolved {1}!",
_transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? _transponder_report.icao_address);
_transponder_report.callsign : uas_id); _last_traffic_warning_time = hrt_absolute_time();
events::send<uint64_t>(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved",
uas_id_int);
} }
break; break;
case TRAFFIC_STATE::BUFFER_FULL: { case TRAFFIC_STATE::BUFFER_FULL: {
if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) { if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL)
PX4_WARN("Too much traffic! Showing all messages from now on"); && (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();
} }
//stop buffering incoming conflicts //disable conflict warnings when buffer is full
take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
_transponder_report.callsign,
uas_id_int);
} }
break; break;
@@ -242,49 +234,67 @@ void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, fl
bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int) char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address)
{ {
switch (_conflict_detection_params.traffic_avoidance_mode) { switch (_conflict_detection_params.traffic_avoidance_mode) {
case 0: { case 0: {
PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
traffic_seperation,
traffic_direction); PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d",
tr_callsign,
traffic_seperation,
traffic_direction, (int)icao_address);
}
_last_traffic_warning_time = hrt_absolute_time();
break; break;
} }
case 1: { case 1: {
mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t",
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
traffic_seperation, if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
traffic_direction);
PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d",
tr_callsign,
traffic_seperation,
traffic_direction, (int)icao_address);
}
/* EVENT /* EVENT
* @description * @description
* - ID: {1} * - ICAO Address: {1}
* - Distance: {2m} * - Traffic Separation Distance: {2m}
* - Direction: {3} degrees * - Heading: {3} degrees
*/ */
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert", events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Notice,
uas_id_int, traffic_seperation, traffic_direction); "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}",
icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = hrt_absolute_time();
break; break;
} }
case 2: { case 2: {
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t",
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
traffic_seperation,
traffic_direction);
/* EVENT /* EVENT
* @description * @description
* - ID: {1} * - ICAO Address: {1}
* - Distance: {2m} * - Traffic Separation Distance: {2m}
* - Direction: {3} degrees * - Heading: {3} degrees
*/ */
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical, events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Notice,
"Traffic alert, returning home", "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home",
uas_id_int, traffic_seperation, traffic_direction); icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = hrt_absolute_time();
return true; return true;
@@ -292,19 +302,17 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper
} }
case 3: { case 3: {
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t",
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
traffic_seperation,
traffic_direction);
/* EVENT /* EVENT
* @description * @description
* - ID: {1} * - ICAO Address: {1}
* - Distance: {2m} * - Traffic Separation Distance: {2m}
* - Direction: {3} degrees * - Heading: {3} degrees
*/ */
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical, events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Notice,
"Traffic alert, landing", "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing",
uas_id_int, traffic_seperation, traffic_direction); icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = hrt_absolute_time();
return true; return true;
@@ -313,19 +321,18 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper
} }
case 4: { case 4: {
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t",
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
traffic_seperation,
traffic_direction);
/* EVENT /* EVENT
* @description * @description
* - ID: {1} * - ICAO Address: {1}
* - Distance: {2m} * - Traffic Separation Distance: {2m}
* - Direction: {3} degrees * - Heading: {3} degrees
*/ */
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical, events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Notice,
"Traffic alert, holding position", "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position",
uas_id_int, traffic_seperation, traffic_direction); icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = hrt_absolute_time();
return true; return true;
@@ -391,8 +398,7 @@ void AdsbConflict::fake_traffic(const char *callsign, float distance, float dire
void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
float &alt_uav) float &alt_uav)
{ {
//Test with buffer size of 10
/*
//first conflict //first conflict
fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
@@ -402,19 +408,20 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav); alt_uav);
fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav); alt_uav);
fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav); alt_uav);
//stop spamming //stop spamming
//new conflicts
fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
alt_uav); alt_uav);
fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
alt_uav); alt_uav);
@@ -445,6 +452,9 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
alt_uav); alt_uav);
//buffer full //buffer full
//buffer full conflicts
fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav,
alt_uav); alt_uav);
@@ -458,7 +468,7 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
alt_uav); alt_uav);
//end conflict //end conflicts
fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
alt_uav); alt_uav);
@@ -474,30 +484,29 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
alt_uav); alt_uav);
//new conflicts with space in buffer //new conflicts with space in buffer
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
alt_uav); alt_uav);
//spam
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
alt_uav); alt_uav);
//new conflict
fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
alt_uav); alt_uav);
//spam
fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
alt_uav); alt_uav);
//new conflict
fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav,
alt_uav); alt_uav);
for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) { for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i])); PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
} }
*/
} }
src/lib/adsb/AdsbConflict.h
+9 -7
View File
@@ -46,7 +46,6 @@
#include <uORB/topics/transponder_report.h> #include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <systemlib/mavlink_log.h>
#include <px4_platform_common/events.h> #include <px4_platform_common/events.h>
#include <px4_platform_common/board_common.h> #include <px4_platform_common/board_common.h>
@@ -57,14 +56,15 @@ using namespace time_literals;
static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10}; static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10};
static constexpr uint8_t UTM_GUID_MSG_LENGTH{11};
static constexpr uint8_t UTM_CALLSIGN_LENGTH{9}; static constexpr uint8_t UTM_CALLSIGN_LENGTH{9};
static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s}; static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s};
static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f}; static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f};
static constexpr uint64_t TRAFFIC_WARNING_TIMESTEP{60_s}; //limits the max warning rate when traffic conflict buffer is full
static constexpr uint64_t TRAFFIC_CONFLICT_LIFETIME{120_s}; //limits the time a conflict can be in the buffer without being seen (as a conflict)
struct traffic_data_s { struct traffic_data_s {
double lat_traffic; double lat_traffic;
@@ -118,7 +118,7 @@ public:
bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int); char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address);
transponder_report_s _transponder_report{}; transponder_report_s _transponder_report{};
@@ -132,6 +132,8 @@ public:
void run_fake_traffic(double &lat_uav, double &lon_uav, void run_fake_traffic(double &lat_uav, double &lon_uav,
float &alt_uav); float &alt_uav);
void remove_expired_conflicts();
bool _conflict_detected{false}; bool _conflict_detected{false};
TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT}; TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT};
@@ -144,15 +146,15 @@ protected:
private: private:
orb_advert_t _mavlink_log_pub{nullptr};
crosstrack_error_s _crosstrack_error{}; crosstrack_error_s _crosstrack_error{};
transponder_report_s tr{}; transponder_report_s tr{};
orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr); orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr);
TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT}; TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};
hrt_abstime _last_traffic_warning_time{0};
hrt_abstime _last_buffer_full_warning_time{0};
}; };
src/lib/adsb/AdsbConflictTest.cpp
+16 -17
View File
@@ -50,7 +50,7 @@ TEST_F(AdsbConflictTest, detectTrafficConflict)
for (uint32_t i = 0; i < traffic_dataset_size; i++) { for (uint32_t i = 0; i < traffic_dataset_size; i++) {
printf("---------------%d--------------\n", i); //printf("---------------%d--------------\n", i);
struct traffic_data_s traffic = traffic_dataset[i]; struct traffic_data_s traffic = traffic_dataset[i];
@@ -64,10 +64,10 @@ TEST_F(AdsbConflictTest, detectTrafficConflict)
adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now); adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now);
printf("conflict_detected %d \n", adsb_conflict._conflict_detected); //printf("conflict_detected %d \n", adsb_conflict._conflict_detected);
printf("------------------------------\n"); //printf("------------------------------\n");
printf("------------------------------\n \n"); //printf("------------------------------\n \n");
EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict); EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict);
} }
@@ -191,7 +191,6 @@ TEST_F(AdsbConflictTest, trafficAlerts)
TEST_F(AdsbConflictTest, trafficReminder) TEST_F(AdsbConflictTest, trafficReminder)
{ {
struct traffic_buffer_s used_buffer; struct traffic_buffer_s used_buffer;
used_buffer.icao_address.push_back(2345); used_buffer.icao_address.push_back(2345);
used_buffer.icao_address.push_back(1234); used_buffer.icao_address.push_back(1234);
@@ -201,11 +200,11 @@ TEST_F(AdsbConflictTest, trafficReminder)
used_buffer.icao_address.push_back(5000); used_buffer.icao_address.push_back(5000);
used_buffer.timestamp.push_back(3_s); used_buffer.timestamp.push_back(3_s);
used_buffer.timestamp.push_back(800_s); used_buffer.timestamp.push_back(80_s);
used_buffer.timestamp.push_back(10_s);
used_buffer.timestamp.push_back(1000_s);
used_buffer.timestamp.push_back(100_s); used_buffer.timestamp.push_back(100_s);
used_buffer.timestamp.push_back(20000_s); used_buffer.timestamp.push_back(187_s);
used_buffer.timestamp.push_back(6000_s);
used_buffer.timestamp.push_back(6587_s);
struct traffic_buffer_s full_buffer; struct traffic_buffer_s full_buffer;
full_buffer.icao_address.push_back(2345); full_buffer.icao_address.push_back(2345);
@@ -220,15 +219,15 @@ TEST_F(AdsbConflictTest, trafficReminder)
full_buffer.icao_address.push_back(99999); full_buffer.icao_address.push_back(99999);
full_buffer.timestamp.push_back(1_s); full_buffer.timestamp.push_back(1_s);
full_buffer.timestamp.push_back(800_s); full_buffer.timestamp.push_back(80_s);
full_buffer.timestamp.push_back(100_s); full_buffer.timestamp.push_back(10_s);
full_buffer.timestamp.push_back(20000_s);
full_buffer.timestamp.push_back(6000_s);
full_buffer.timestamp.push_back(19000_s);
full_buffer.timestamp.push_back(5000_s);
full_buffer.timestamp.push_back(2_s);
full_buffer.timestamp.push_back(1000_s); full_buffer.timestamp.push_back(1000_s);
full_buffer.timestamp.push_back(58943_s); full_buffer.timestamp.push_back(100_s);
full_buffer.timestamp.push_back(900_s);
full_buffer.timestamp.push_back(500_s);
full_buffer.timestamp.push_back(2_s);
full_buffer.timestamp.push_back(100_s);
full_buffer.timestamp.push_back(5843_s);
TestAdsbConflict adsb_conflict; TestAdsbConflict adsb_conflict;
src/modules/mavlink/mavlink_main.cpp
-5
View File
@@ -1440,7 +1440,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYS_STATUS", 1.0f);
configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
configure_stream_local("UTM_GLOBAL_POSITION", 0.5f);
configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.1f); configure_stream_local("VIBRATION", 0.1f);
configure_stream_local("WIND_COV", 0.5f); configure_stream_local("WIND_COV", 0.5f);
@@ -1510,7 +1509,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f);
configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VFR_HUD", 10.0f);
configure_stream_local("VIBRATION", 0.5f); configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 10.0f); configure_stream_local("WIND_COV", 10.0f);
@@ -1576,7 +1574,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYS_STATUS", 5.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.5f); configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 1.0f); configure_stream_local("WIND_COV", 1.0f);
@@ -1675,7 +1672,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYS_STATUS", 1.0f);
configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f);
configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
configure_stream_local("VFR_HUD", 20.0f); configure_stream_local("VFR_HUD", 20.0f);
configure_stream_local("VIBRATION", 2.5f); configure_stream_local("VIBRATION", 2.5f);
configure_stream_local("WIND_COV", 10.0f); configure_stream_local("WIND_COV", 10.0f);
@@ -1760,7 +1756,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("SYSTEM_TIME", 2.0f);
configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.5f); configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 1.0f); configure_stream_local("WIND_COV", 1.0f);
src/modules/mavlink/mavlink_messages.cpp
-4
View File
@@ -150,7 +150,6 @@
# include "streams/SCALED_PRESSURE3.hpp" # include "streams/SCALED_PRESSURE3.hpp"
# include "streams/UAVIONIX_ADSB_OUT_CFG.hpp" # include "streams/UAVIONIX_ADSB_OUT_CFG.hpp"
# include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp" # include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp"
# include "streams/UTM_GLOBAL_POSITION.hpp"
#endif // !CONSTRAINED_FLASH #endif // !CONSTRAINED_FLASH
// ensure PX4 rotation enum and MAV_SENSOR_ROTATION align // ensure PX4 rotation enum and MAV_SENSOR_ROTATION align
@@ -419,9 +418,6 @@ static const StreamListItem streams_list[] = {
#if defined(ADSB_VEHICLE_HPP) #if defined(ADSB_VEHICLE_HPP)
create_stream_list_item<MavlinkStreamADSBVehicle>(), create_stream_list_item<MavlinkStreamADSBVehicle>(),
#endif // ADSB_VEHICLE_HPP #endif // ADSB_VEHICLE_HPP
#if defined(UTM_GLOBAL_POSITION_HPP)
create_stream_list_item<MavlinkStreamUTMGlobalPosition>(),
#endif // UTM_GLOBAL_POSITION_HPP
#if defined(COLLISION_HPP) #if defined(COLLISION_HPP)
create_stream_list_item<MavlinkStreamCollision>(), create_stream_list_item<MavlinkStreamCollision>(),
#endif // COLLISION_HPP #endif // COLLISION_HPP
src/modules/mavlink/mavlink_receiver.cpp
-89
View File
@@ -224,10 +224,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_adsb_vehicle(msg); handle_message_adsb_vehicle(msg);
break; break;
case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION:
handle_message_utm_global_position(msg);
break;
case MAVLINK_MSG_ID_COLLISION: case MAVLINK_MSG_ID_COLLISION:
handle_message_collision(msg); handle_message_collision(msg);
break; break;
@@ -2540,91 +2536,6 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
_transponder_report_pub.publish(t); _transponder_report_pub.publish(t);
} }
void
MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
{
mavlink_utm_global_position_t utm_pos;
mavlink_msg_utm_global_position_decode(msg, &utm_pos);
bool is_self_published = false;
#ifndef BOARD_HAS_NO_UUID
px4_guid_t px4_guid;
board_get_px4_guid(px4_guid);
is_self_published = sizeof(px4_guid) == sizeof(utm_pos.uas_id)
&& memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) == 0;
#else
is_self_published = msg->sysid == _mavlink->get_system_id();
#endif /* BOARD_HAS_NO_UUID */
//Ignore selfpublished UTM messages
if (is_self_published) {
return;
}
// Convert cm/s to m/s
float vx = utm_pos.vx / 100.0f;
float vy = utm_pos.vy / 100.0f;
float vz = utm_pos.vz / 100.0f;
transponder_report_s t{};
t.timestamp = hrt_absolute_time();
mav_array_memcpy(t.uas_id, utm_pos.uas_id, PX4_GUID_BYTE_LENGTH);
t.icao_address = msg->sysid;
t.lat = utm_pos.lat * 1e-7;
t.lon = utm_pos.lon * 1e-7;
t.altitude = utm_pos.alt / 1000.0f;
t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
// UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s.
t.heading = atan2f(vy, vx);
t.hor_velocity = sqrtf(vy * vy + vx * vx);
t.ver_velocity = -vz;
// TODO: Callsign
// For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null
// terminator could cause problems.
memset(&t.callsign[0], 0, sizeof(t.callsign));
t.emitter_type = ADSB_EMITTER_TYPE_UAV; // TODO: Is this correct?x2?
// The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of
// an 8-bit int, or if this is the first communication.
// Here, I assume that if this is the first communication, tslc = 0.
// If tslc > 255, then tslc = 255.
unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000;
if (_last_utm_global_pos_com == 0) {
time_passed = 0;
} else if (time_passed > UINT8_MAX) {
time_passed = UINT8_MAX;
}
t.tslc = (uint8_t) time_passed;
t.flags = 0;
if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
}
if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
}
if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) {
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
}
// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
// provide these.
_transponder_report_pub.publish(t);
_last_utm_global_pos_com = t.timestamp;
}
void void
MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
{ {
src/modules/navigator/navigator_main.cpp
+4 -1
View File
@@ -1273,6 +1273,9 @@ void Navigator::check_traffic()
} }
} }
} }
_adsb_conflict.remove_expired_conflicts();
} }
bool Navigator::abort_landing() bool Navigator::abort_landing()
@@ -1544,7 +1547,7 @@ controller.
PRINT_MODULE_USAGE_NAME("navigator", "controller"); PRINT_MODULE_USAGE_NAME("navigator", "controller");
PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt"); PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt");
PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 4 fake transponder_report_s uORB messages"); PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 24 fake transponder_report_s uORB messages");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0; return 0;