mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
adsb: warnings fixes & remove UTM_GLOBAL_POSITION (#21663)
- 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
GitHub
parent
c7725d74b4
commit
f95a2021cd
@ -108,12 +108,14 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
|
||||
{
|
||||
_traffic_buffer.icao_address.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)
|
||||
{
|
||||
_traffic_buffer.timestamp.push_back(hrt_absolute_time());
|
||||
_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()
|
||||
@ -129,7 +131,6 @@ void AdsbConflict::get_traffic_state()
|
||||
|
||||
if (old_conflict && _conflict_detected) {
|
||||
old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT);
|
||||
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
|
||||
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;
|
||||
|
||||
switch (_traffic_state) {
|
||||
|
||||
case TRAFFIC_STATE::ADD_CONFLICT:
|
||||
case TRAFFIC_STATE::REMIND_CONFLICT: {
|
||||
|
||||
take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
|
||||
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
|
||||
take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f),
|
||||
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags,
|
||||
_transponder_report.callsign,
|
||||
uas_id_int);
|
||||
|
||||
_transponder_report.icao_address);
|
||||
}
|
||||
break;
|
||||
|
||||
case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!",
|
||||
_transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ?
|
||||
_transponder_report.callsign : uas_id);
|
||||
|
||||
events::send<uint64_t>(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved",
|
||||
uas_id_int);
|
||||
|
||||
events::send<uint32_t>(events::ID("navigator_traffic_resolved"), events::Log::Notice,
|
||||
"Traffic Conflict Resolved {1}!",
|
||||
_transponder_report.icao_address);
|
||||
_last_traffic_warning_time = hrt_absolute_time();
|
||||
}
|
||||
break;
|
||||
|
||||
case TRAFFIC_STATE::BUFFER_FULL: {
|
||||
|
||||
if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) {
|
||||
PX4_WARN("Too much traffic! Showing all messages from now on");
|
||||
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();
|
||||
}
|
||||
|
||||
//stop buffering incoming conflicts
|
||||
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);
|
||||
|
||||
//disable conflict warnings when buffer is full
|
||||
}
|
||||
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,
|
||||
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) {
|
||||
|
||||
case 0: {
|
||||
PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
|
||||
tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
|
||||
traffic_seperation,
|
||||
traffic_direction);
|
||||
|
||||
if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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,
|
||||
traffic_direction);
|
||||
|
||||
|
||||
if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
|
||||
|
||||
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
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
* - ICAO Address: {1}
|
||||
* - Traffic Separation Distance: {2m}
|
||||
* - Heading: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Notice,
|
||||
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}",
|
||||
icao_address, traffic_seperation, traffic_direction);
|
||||
|
||||
_last_traffic_warning_time = hrt_absolute_time();
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
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
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
* - ICAO Address: {1}
|
||||
* - Traffic Separation Distance: {2m}
|
||||
* - Heading: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical,
|
||||
"Traffic alert, returning home",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Notice,
|
||||
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home",
|
||||
icao_address, traffic_seperation, traffic_direction);
|
||||
|
||||
_last_traffic_warning_time = hrt_absolute_time();
|
||||
|
||||
return true;
|
||||
|
||||
@ -292,19 +302,17 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper
|
||||
}
|
||||
|
||||
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
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
* - ICAO Address: {1}
|
||||
* - Traffic Separation Distance: {2m}
|
||||
* - Heading: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical,
|
||||
"Traffic alert, landing",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Notice,
|
||||
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing",
|
||||
icao_address, traffic_seperation, traffic_direction);
|
||||
|
||||
_last_traffic_warning_time = hrt_absolute_time();
|
||||
|
||||
return true;
|
||||
|
||||
@ -313,19 +321,18 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper
|
||||
}
|
||||
|
||||
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
|
||||
* @description
|
||||
* - ID: {1}
|
||||
* - Distance: {2m}
|
||||
* - Direction: {3} degrees
|
||||
* - ICAO Address: {1}
|
||||
* - Traffic Separation Distance: {2m}
|
||||
* - Heading: {3} degrees
|
||||
*/
|
||||
events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical,
|
||||
"Traffic alert, holding position",
|
||||
uas_id_int, traffic_seperation, traffic_direction);
|
||||
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Notice,
|
||||
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position",
|
||||
icao_address, traffic_seperation, traffic_direction);
|
||||
|
||||
_last_traffic_warning_time = hrt_absolute_time();
|
||||
|
||||
|
||||
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,
|
||||
float &alt_uav)
|
||||
{
|
||||
|
||||
/*
|
||||
//Test with buffer size of 10
|
||||
//first conflict
|
||||
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,
|
||||
@ -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,
|
||||
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_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,
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
//stop spamming
|
||||
|
||||
//new conflicts
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
|
||||
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,
|
||||
alt_uav);
|
||||
@ -445,6 +452,9 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
|
||||
alt_uav);
|
||||
|
||||
//buffer full
|
||||
|
||||
//buffer full conflicts
|
||||
|
||||
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,
|
||||
alt_uav);
|
||||
@ -458,7 +468,7 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
|
||||
alt_uav);
|
||||
|
||||
|
||||
//end conflict
|
||||
//end conflicts
|
||||
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,
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
|
||||
//new conflicts with space in buffer
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
//spam
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
//new conflict
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
//spam
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
//new conflict
|
||||
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,
|
||||
alt_uav);
|
||||
|
||||
|
||||
for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
|
||||
PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
@ -46,7 +46,6 @@
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <px4_platform_common/events.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 UTM_GUID_MSG_LENGTH{11};
|
||||
|
||||
static constexpr uint8_t UTM_CALLSIGN_LENGTH{9};
|
||||
|
||||
static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s};
|
||||
|
||||
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 {
|
||||
double lat_traffic;
|
||||
@ -118,7 +118,7 @@ public:
|
||||
|
||||
|
||||
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{};
|
||||
|
||||
@ -132,6 +132,8 @@ public:
|
||||
void run_fake_traffic(double &lat_uav, double &lon_uav,
|
||||
float &alt_uav);
|
||||
|
||||
void remove_expired_conflicts();
|
||||
|
||||
bool _conflict_detected{false};
|
||||
|
||||
TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT};
|
||||
@ -144,15 +146,15 @@ protected:
|
||||
|
||||
private:
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
crosstrack_error_s _crosstrack_error{};
|
||||
|
||||
|
||||
transponder_report_s tr{};
|
||||
|
||||
orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr);
|
||||
|
||||
TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};
|
||||
|
||||
hrt_abstime _last_traffic_warning_time{0};
|
||||
|
||||
hrt_abstime _last_buffer_full_warning_time{0};
|
||||
};
|
||||
|
||||
@ -50,7 +50,7 @@ TEST_F(AdsbConflictTest, detectTrafficConflict)
|
||||
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];
|
||||
|
||||
@ -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);
|
||||
|
||||
printf("conflict_detected %d \n", adsb_conflict._conflict_detected);
|
||||
//printf("conflict_detected %d \n", adsb_conflict._conflict_detected);
|
||||
|
||||
printf("------------------------------\n");
|
||||
printf("------------------------------\n \n");
|
||||
//printf("------------------------------\n");
|
||||
//printf("------------------------------\n \n");
|
||||
|
||||
EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict);
|
||||
}
|
||||
@ -191,7 +191,6 @@ TEST_F(AdsbConflictTest, trafficAlerts)
|
||||
|
||||
TEST_F(AdsbConflictTest, trafficReminder)
|
||||
{
|
||||
|
||||
struct traffic_buffer_s used_buffer;
|
||||
used_buffer.icao_address.push_back(2345);
|
||||
used_buffer.icao_address.push_back(1234);
|
||||
@ -201,11 +200,11 @@ TEST_F(AdsbConflictTest, trafficReminder)
|
||||
used_buffer.icao_address.push_back(5000);
|
||||
|
||||
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(20000_s);
|
||||
used_buffer.timestamp.push_back(6000_s);
|
||||
used_buffer.timestamp.push_back(6587_s);
|
||||
used_buffer.timestamp.push_back(187_s);
|
||||
|
||||
struct traffic_buffer_s full_buffer;
|
||||
full_buffer.icao_address.push_back(2345);
|
||||
@ -220,15 +219,15 @@ TEST_F(AdsbConflictTest, trafficReminder)
|
||||
full_buffer.icao_address.push_back(99999);
|
||||
|
||||
full_buffer.timestamp.push_back(1_s);
|
||||
full_buffer.timestamp.push_back(800_s);
|
||||
full_buffer.timestamp.push_back(100_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(80_s);
|
||||
full_buffer.timestamp.push_back(10_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;
|
||||
|
||||
@ -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("SYS_STATUS", 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("VIBRATION", 0.1f);
|
||||
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("TIME_ESTIMATE_TO_TARGET", 1.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("VIBRATION", 0.5f);
|
||||
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("SYS_STATUS", 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("VIBRATION", 0.5f);
|
||||
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("SYSTEM_TIME", 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("VIBRATION", 2.5f);
|
||||
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("TIME_ESTIMATE_TO_TARGET", 1.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("VIBRATION", 0.5f);
|
||||
configure_stream_local("WIND_COV", 1.0f);
|
||||
|
||||
@ -150,7 +150,6 @@
|
||||
# include "streams/SCALED_PRESSURE3.hpp"
|
||||
# include "streams/UAVIONIX_ADSB_OUT_CFG.hpp"
|
||||
# include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp"
|
||||
# include "streams/UTM_GLOBAL_POSITION.hpp"
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
// ensure PX4 rotation enum and MAV_SENSOR_ROTATION align
|
||||
@ -419,9 +418,6 @@ static const StreamListItem streams_list[] = {
|
||||
#if defined(ADSB_VEHICLE_HPP)
|
||||
create_stream_list_item<MavlinkStreamADSBVehicle>(),
|
||||
#endif // ADSB_VEHICLE_HPP
|
||||
#if defined(UTM_GLOBAL_POSITION_HPP)
|
||||
create_stream_list_item<MavlinkStreamUTMGlobalPosition>(),
|
||||
#endif // UTM_GLOBAL_POSITION_HPP
|
||||
#if defined(COLLISION_HPP)
|
||||
create_stream_list_item<MavlinkStreamCollision>(),
|
||||
#endif // COLLISION_HPP
|
||||
|
||||
@ -224,10 +224,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
handle_message_adsb_vehicle(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION:
|
||||
handle_message_utm_global_position(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_COLLISION:
|
||||
handle_message_collision(msg);
|
||||
break;
|
||||
@ -2540,91 +2536,6 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
|
||||
_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
|
||||
MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
|
||||
{
|
||||
|
||||
@ -1273,6 +1273,9 @@ void Navigator::check_traffic()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_adsb_conflict.remove_expired_conflicts();
|
||||
|
||||
}
|
||||
|
||||
bool Navigator::abort_landing()
|
||||
@ -1544,7 +1547,7 @@ controller.
|
||||
PRINT_MODULE_USAGE_NAME("navigator", "controller");
|
||||
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("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();
|
||||
|
||||
return 0;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user