Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Don't use ADSB messages with undefined fields in navigator #8900

Merged
merged 4 commits into from
Feb 18, 2018
Merged

Don't use ADSB messages with undefined fields in navigator #8900

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
740ab24
Don't use ADSB messages with undefined fields in navigator
svpcom Feb 16, 2018
9afb7e5
Fix style
svpcom Feb 17, 2018
8550589
Split PX4 and mavlink definition of ADSB message flags
svpcom Feb 17, 2018
dcc80e5
Fix style. Move flags definitions to uORB message
svpcom Feb 18, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions msg/transponder_report.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,13 @@ uint8 tslc # Time since last communication in seconds
uint16 flags # Flags to indicate various statuses including valid data fields
uint16 squawk # Squawk code

# ADSB flags
uint16 PX4_ADSB_FLAGS_VALID_COORDS = 1
uint16 PX4_ADSB_FLAGS_VALID_ALTITUDE = 2
uint16 PX4_ADSB_FLAGS_VALID_HEADING = 4
uint16 PX4_ADSB_FLAGS_VALID_VELOCITY = 8
uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16
uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32
uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256

uint32 ORB_QUEUE_LENGTH = 10
17 changes: 16 additions & 1 deletion src/modules/mavlink/mavlink_messages.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1346,6 +1346,8 @@ class MavlinkStreamADSBVehicle : public MavlinkStream
while (_pos_sub->update(&_pos_time, &pos)) {
mavlink_adsb_vehicle_t msg = {};

if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) { continue; }

msg.ICAO_address = pos.ICAO_address;
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
Expand All @@ -1357,9 +1359,22 @@ class MavlinkStreamADSBVehicle : public MavlinkStream
memcpy(&msg.callsign[0], &pos.callsign[0], sizeof(msg.callsign));
msg.emitter_type = pos.emitter_type;
msg.tslc = pos.tslc;
msg.flags = pos.flags;
msg.squawk = pos.squawk;

msg.flags = 0;

if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS) { msg.flags |= ADSB_FLAGS_VALID_COORDS; }

if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE) { msg.flags |= ADSB_FLAGS_VALID_ALTITUDE; }

if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING) { msg.flags |= ADSB_FLAGS_VALID_HEADING; }

if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY) { msg.flags |= ADSB_FLAGS_VALID_VELOCITY; }

if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) { msg.flags |= ADSB_FLAGS_VALID_CALLSIGN; }

if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK) { msg.flags |= ADSB_FLAGS_VALID_SQUAWK; }

mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
sent = true;
}
Expand Down
15 changes: 14 additions & 1 deletion src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2079,9 +2079,22 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
memcpy(&t.callsign[0], &adsb.callsign[0], sizeof(t.callsign));
t.emitter_type = adsb.emitter_type;
t.tslc = adsb.tslc;
t.flags = adsb.flags;
t.squawk = adsb.squawk;

t.flags = transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE; //Unset in receiver already broadcast its messages

if (adsb.flags & ADSB_FLAGS_VALID_COORDS) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; }

if (adsb.flags & ADSB_FLAGS_VALID_ALTITUDE) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; }

if (adsb.flags & ADSB_FLAGS_VALID_HEADING) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; }

if (adsb.flags & ADSB_FLAGS_VALID_VELOCITY) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; }

if (adsb.flags & ADSB_FLAGS_VALID_CALLSIGN) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; }

if (adsb.flags & ADSB_FLAGS_VALID_SQUAWK) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK; }

//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);

if (_transponder_report_pub == nullptr) {
Expand Down
1 change: 1 addition & 0 deletions src/modules/navigator/navigator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 11


class Navigator : public control::SuperBlock
{
public:
Expand Down
25 changes: 21 additions & 4 deletions src/modules/navigator/navigator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -963,7 +963,10 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign));
tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum
tr.tslc = 2; // Time since last communication in seconds
tr.flags = 0; // Flags to indicate various statuses including valid data fields
tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
tr.squawk = 6667;

orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
Expand Down Expand Up @@ -993,10 +996,20 @@ void Navigator::check_traffic()
transponder_report_s tr;
orb_copy(ORB_ID(transponder_report), _traffic_sub, &tr);

uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;

if ((tr.flags & required_flags) != required_flags) {
orb_check(_traffic_sub, &changed);
continue;
}

float d_hor, d_vert;
get_distance_to_point_global_wgs84(lat, lon, alt,
tr.lat, tr.lon, tr.altitude, &d_hor, &d_vert);


// predict final altitude (positive is up) in prediction time frame
float end_alt = tr.altitude + (d_vert / tr.hor_velocity) * tr.ver_velocity;

Expand Down Expand Up @@ -1031,18 +1044,22 @@ void Navigator::check_traffic()

case 0: {
/* ignore */
PX4_WARN("TRAFFIC %s, hdg: %d", tr.callsign, traffic_direction);
PX4_WARN("TRAFFIC %s, hdg: %d", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign :
"unknown",
traffic_direction);
break;
}

case 1: {
mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately", tr.callsign,
mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
traffic_direction);
break;
}

case 2: {
mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home", tr.callsign,
mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home",
tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown",
traffic_direction);

// set the return altitude to minimum
Expand Down