From 98d64483d6739365c6b5af18d89059096bb42584 Mon Sep 17 00:00:00 2001 From: Oliver Jowett Date: Thu, 15 Jun 2017 18:07:40 +0100 Subject: [PATCH] WIP: More Comm-B & ADS-B decoding. --- Makefile | 6 +- comm_b.c | 735 ++++++++++++++++++++++++++++++++++++++++++++++++++ comm_b.h | 26 ++ dump1090.h | 123 +++++---- interactive.c | 12 +- mode_s.c | 376 ++++++++++++-------------- mode_s.h | 100 +++++++ net_io.c | 319 ++++++++++++++-------- track.c | 114 +++++--- track.h | 73 +++-- 10 files changed, 1446 insertions(+), 438 deletions(-) create mode 100644 comm_b.c create mode 100644 comm_b.h create mode 100644 mode_s.h diff --git a/Makefile b/Makefile index a7d4c04..93db17a 100644 --- a/Makefile +++ b/Makefile @@ -45,13 +45,13 @@ all: dump1090 view1090 %.o: %.c *.h $(CC) $(CPPFLAGS) $(CFLAGS) -c $< -o $@ -dump1090: dump1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o demod_2400.o stats.o cpr.o icao_filter.o track.o util.o convert.o sdr_ifile.o sdr.o $(SDR_OBJ) $(COMPAT) +dump1090: dump1090.o anet.o interactive.o mode_ac.o mode_s.o comm_b.o net_io.o crc.o demod_2400.o stats.o cpr.o icao_filter.o track.o util.o convert.o sdr_ifile.o sdr.o $(SDR_OBJ) $(COMPAT) $(CC) -g -o $@ $^ $(LDFLAGS) $(LIBS) $(LIBS_SDR) -lncurses -view1090: view1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o stats.o cpr.o icao_filter.o track.o util.o $(COMPAT) +view1090: view1090.o anet.o interactive.o mode_ac.o mode_s.o comm_b.o net_io.o crc.o stats.o cpr.o icao_filter.o track.o util.o $(COMPAT) $(CC) -g -o $@ $^ $(LDFLAGS) $(LIBS) -lncurses -faup1090: faup1090.o anet.o mode_ac.o mode_s.o net_io.o crc.o stats.o cpr.o icao_filter.o track.o util.o $(COMPAT) +faup1090: faup1090.o anet.o mode_ac.o mode_s.o comm_b.o net_io.o crc.o stats.o cpr.o icao_filter.o track.o util.o $(COMPAT) $(CC) -g -o $@ $^ $(LDFLAGS) $(LIBS) clean: diff --git a/comm_b.c b/comm_b.c new file mode 100644 index 0000000..b03ea43 --- /dev/null +++ b/comm_b.c @@ -0,0 +1,735 @@ +// Part of dump1090, a Mode S message decoder for RTLSDR devices. +// +// comm_b.c: Comm-B message decoding +// +// Copyright (c) 2017 FlightAware, LLC +// Copyright (c) 2017 Oliver Jowett +// +// This file is free software: you may copy, redistribute and/or modify it +// under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 2 of the License, or (at your +// option) any later version. +// +// This file is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include "dump1090.h" + +typedef int (*CommBDecoderFn)(struct modesMessage *,bool); + +static int decodeEmptyResponse(struct modesMessage *mm, bool store); +static int decodeBDS10(struct modesMessage *mm, bool store); +static int decodeBDS17(struct modesMessage *mm, bool store); +static int decodeBDS20(struct modesMessage *mm, bool store); +static int decodeBDS30(struct modesMessage *mm, bool store); +static int decodeBDS40(struct modesMessage *mm, bool store); +static int decodeBDS50(struct modesMessage *mm, bool store); +static int decodeBDS60(struct modesMessage *mm, bool store); + +static CommBDecoderFn comm_b_decoders[] = { + &decodeEmptyResponse, + &decodeBDS10, + &decodeBDS20, + &decodeBDS30, + &decodeBDS17, + &decodeBDS40, + &decodeBDS50, + &decodeBDS60 +}; + +void decodeCommB(struct modesMessage *mm) +{ + mm->commb_format = COMMB_UNKNOWN; + + // If DR or UM are set, this message is _probably_ noise + // as nothing really seems to use the multisite broadcast stuff? + // Also skip anything that had errors corrected + if (mm->DR != 0 || mm->UM != 0 || mm->correctedbits > 0) { + return; + } + + // This is a bit hairy as we don't know what the requested register was + int bestScore = 0; + CommBDecoderFn bestDecoder = NULL; + + for (unsigned i = 0; i < (sizeof(comm_b_decoders) / sizeof(comm_b_decoders[0])); ++i) { + int score = comm_b_decoders[i](mm, false); + if (score > bestScore) { + bestScore = score; + bestDecoder = comm_b_decoders[i]; + } + } + + if (bestDecoder) { + // decode it + bestDecoder(mm, true); + } +} + +static int decodeEmptyResponse(struct modesMessage *mm, bool store) +{ + for (unsigned i = 0; i < 7; ++i) { + if (mm->MB[i] != 0) { + return 0; + } + } + + if (store) { + mm->commb_format = COMMB_EMPTY_RESPONSE; + } + + return 56; +} + +// BDS1,0 Datalink capabilities +static int decodeBDS10(struct modesMessage *mm, bool store) +{ + unsigned char *msg = mm->MB; + + // BDS identifier + if (msg[0] != 0x10) { + return 0; + } + + // Reserved bits + if (getbits(msg, 10, 15) != 0) { + return 0; + } + + // Looks plausible. + + if (store) { + mm->commb_format = COMMB_DATALINK_CAPS; + } + + return 56; +} + +// BDS1,7 Common usage GICB capability report +static int decodeBDS17(struct modesMessage *mm, bool store) +{ + unsigned char *msg = mm->MB; + + // reserved bits + if (getbits(msg, 25, 56) != 0) { + return 0; + } + + int score = 0; + if (getbit(msg, 7)) { + score += 1; // 2,0 aircraft identification + } else { + // BDS2,0 is on almost everything + score -= 2; + } + + // unlikely bits + if (getbit(msg, 10)) { // 4,1 next waypoint identifier + score -= 2; + } + if (getbit(msg, 11)) { // 4,2 next waypoint position + score -= 2; + } + if (getbit(msg, 12)) { // 4,3 next waypoint information + score -= 2; + } + if (getbit(msg, 13)) { // 4,4 meterological routine report + score -= 2; + } + if (getbit(msg, 14)) { // 4,4 meterological hazard report + score -= 2; + } + if (getbit(msg, 20)) { // 5,4 waypoint 1 + score -= 2; + } + if (getbit(msg, 21)) { // 5,5 waypoint 2 + score -= 2; + } + if (getbit(msg, 22)) { // 5,6 waypoint 3 + score -= 2; + } + + if (getbit(msg, 1) && getbit(msg, 2) && getbit(msg, 3) && getbit(msg, 4) && getbit(msg, 5)) { + // looks like ES capable + score += 5; + if (getbit(msg, 6)) { + // ES EDI + score += 1; + } + } else if (!getbit(msg, 1) && !getbit(msg, 2) && !getbit(msg, 3) && !getbit(msg, 4) && !getbit(msg, 5) && !getbit(msg, 6)) { + // not ES capable + score += 1; + } else { + // partial ES support, unlikely + score -= 12; + } + + if (getbit(msg, 16) && getbit(msg, 24)) { + // track/turn, heading/speed + score += 2; + if (getbit(msg, 9)) { + // vertical intent + score += 1; + } + } else if (!getbit(msg, 16) && !getbit(msg, 24) && !getbit(msg, 9)) { + // neither + score += 1; + } else { + // unlikely + score -= 6; + } + + if (store) { + mm->commb_format = COMMB_GICB_CAPS; + } + + return score; +} + +// BDS2,0 Aircraft identification +static int decodeBDS20(struct modesMessage *mm, bool store) +{ + char callsign[9]; + unsigned char *msg = mm->MB; + + // BDS identifier + if (msg[0] != 0x20) { + return 0; + } + + callsign[0] = ais_charset[getbits(msg, 9, 14)]; + callsign[1] = ais_charset[getbits(msg, 15, 20)]; + callsign[2] = ais_charset[getbits(msg, 21, 26)]; + callsign[3] = ais_charset[getbits(msg, 27, 32)]; + callsign[4] = ais_charset[getbits(msg, 33, 38)]; + callsign[5] = ais_charset[getbits(msg, 39, 44)]; + callsign[6] = ais_charset[getbits(msg, 45, 50)]; + callsign[7] = ais_charset[getbits(msg, 51, 56)]; + callsign[8] = 0; + + // score based on number of valid non-space characters + int score = 8; + for (unsigned i = 0; i < 8; ++i) { + if ((callsign[i] >= 'A' && callsign[i] <= 'Z') || (callsign[i] >= '0' && callsign[i] <= '9')) { + score += 6; + } else if (callsign[i] == ' ') { + // Valid, but not informative + } else { + // Invalid + return 0; + } + } + + if (store) { + mm->commb_format = COMMB_AIRCRAFT_IDENT; + memcpy(mm->callsign, callsign, sizeof(mm->callsign)); + mm->callsign_valid = 1; + } + + return score; +} + +// BDS3,0 ACAS RA +static int decodeBDS30(struct modesMessage *mm, bool store) +{ + unsigned char *msg = mm->MB; + + // BDS identifier + if (msg[0] != 0x30) { + return 0; + } + + if (store) { + mm->commb_format = COMMB_ACAS_RA; + } + + // just accept it. + return 56; +} + +// BDS4,0 Selected vertical intention +static int decodeBDS40(struct modesMessage *mm, bool store) +{ + unsigned char *msg = mm->MB; + + unsigned mcp_valid = getbit(msg, 1); + unsigned mcp_raw = getbits(msg, 2, 13); + unsigned fms_valid = getbit(msg, 14); + unsigned fms_raw = getbits(msg, 15, 26); + unsigned baro_valid = getbit(msg, 27); + unsigned baro_raw = getbits(msg, 28, 39); + unsigned reserved_1 = getbits(msg, 40, 47); + unsigned mode_valid = getbit(msg, 48); + unsigned mode_raw = getbits(msg, 49, 51); + unsigned reserved_2 = getbits(msg, 52, 53); + unsigned source_valid = getbit(msg, 54); + unsigned source_raw = getbits(msg, 55, 56); + + if (!mcp_valid && !fms_valid && !baro_valid && !mode_valid && !source_valid) { + return 0; + } + + int score = 0; + + unsigned mcp_alt = 0; + if (mcp_valid && mcp_raw != 0) { + mcp_alt = mcp_raw * 16; + if (mcp_alt >= 1000 && mcp_alt <= 50000) { + score += 13; + } + } else if (!mcp_valid && mcp_raw == 0) { + score += 1; + } else { + score -= 130; + } + + unsigned fms_alt = 0; + if (fms_valid && fms_raw != 0) { + fms_alt = fms_raw * 16; + if (fms_alt >= 1000 && fms_alt <= 50000) { + score += 13; + } + } else if (!fms_valid && fms_raw == 0) { + score += 1; + } else { + score -= 130; + } + + float baro_setting = 0; + if (baro_valid && baro_raw != 0) { + baro_setting = 800 + baro_raw * 0.1; + if (baro_setting >= 900 && baro_setting <= 1100) { + score += 13; + } + } else if (!baro_valid && baro_raw == 0) { + score += 1; + } else { + score -= 130; + } + + if (reserved_1 != 0) { + score -= 80; + } + + if (mode_valid) { + score += 4; + } else if (!mode_valid && mode_raw == 0) { + score += 1; + } else { + score -= 40; + } + + if (reserved_2 != 0) { + score -= 20; + } + + if (source_valid) { + score += 3; + } else if (!source_valid && source_raw == 0) { + score += 1; + } else { + score -= 30; + } + + // small bonuses for consistent data + if (mcp_valid && fms_valid && mcp_alt == fms_alt) { + score += 2; + } + + if (baro_valid && baro_raw == 2132) { + // 1013.2mb, standard pressure + score += 2; + } + + if (mcp_valid) { + unsigned remainder = mcp_alt % 500; + if (remainder < 16 || remainder > 484) { + // mcp altitude is a multiple of 500 + score += 2; + } + } + + if (fms_valid) { + unsigned remainder = fms_alt % 500; + if (remainder < 16 || remainder > 484) { + // fms altitude is a multiple of 500 + score += 2; + } + } + + if (store) { + mm->commb_format = COMMB_VERTICAL_INTENT; + + mm->intent.valid = 1; + + if (mcp_valid) { + mm->intent.mcp_altitude_valid = 1; + mm->intent.mcp_altitude = mcp_alt; + } + + if (fms_valid) { + mm->intent.fms_altitude_valid = 1; + mm->intent.fms_altitude = fms_alt; + } + + if (baro_valid) { + mm->intent.alt_setting_valid = 1; + mm->intent.alt_setting = baro_setting; + } + + if (mode_valid) { + mm->intent.mode_vnav = (mode_raw & 4) ? 1 : 0; + mm->intent.mode_alt_hold = (mode_raw & 2) ? 1 : 0; + mm->intent.mode_approach = (mode_raw & 1) ? 1 : 0; + } + + if (source_valid) { + switch (source_raw) { + case 0: + mm->intent.altitude_source = TARGET_UNKNOWN; + break; + case 1: + mm->intent.altitude_source = TARGET_AIRCRAFT; + break; + case 2: + mm->intent.altitude_source = TARGET_MCP; + break; + case 3: + mm->intent.altitude_source = TARGET_FMS; + break; + default: + mm->intent.altitude_source = TARGET_INVALID; + break; + } + } else { + mm->intent.altitude_source = TARGET_INVALID; + } + } + + return score; +} + +// BDS5,0 Track and turn report +static int decodeBDS50(struct modesMessage *mm, bool store) +{ + unsigned char *msg = mm->MB; + + unsigned roll_valid = getbit(msg, 1); + unsigned roll_sign = getbit(msg, 2); + unsigned roll_raw = getbits(msg, 3, 11); + + unsigned track_valid = getbit(msg, 12); + unsigned track_sign = getbit(msg, 13); + unsigned track_raw = getbits(msg, 14, 23); + + unsigned gs_valid = getbit(msg, 24); + unsigned gs_raw = getbits(msg, 25, 34); + + unsigned track_rate_valid = getbit(msg, 35); + unsigned track_rate_sign = getbit(msg, 36); + unsigned track_rate_raw = getbits(msg, 37, 45); + + unsigned tas_valid = getbit(msg, 46); + unsigned tas_raw = getbits(msg, 47, 56); + + if (!roll_valid && !track_valid && !gs_valid && !track_rate_valid && !tas_valid) { + return 0; + } + + int score = 0; + + float roll = 0; + if (roll_valid) { + roll = roll_raw * 45.0 / 256.0; + if (roll_sign) { + roll -= 90.0; + } + + if (roll >= -40 && roll < 40) { + score += 11; + } else { + score -= 5; + } + } else if (!roll_valid && roll_raw == 0 && !roll_sign) { + score += 1; + } else { + score -= 110; + } + + float track = 0; + if (track_valid) { + score += 12; + track = track_raw * 90.0 / 512.0; + if (track_sign) { + track += 180.0; + } + } else if (!track_valid && track_raw == 0 && !track_sign) { + score += 1; + } else { + score -= 120; + } + + unsigned gs = 0; + if (gs_valid && gs_raw != 0) { + gs = gs_raw * 2; + + if (gs >= 50 && gs <= 700) { + score += 11; + } else { + score -= 5; + } + } else if (!gs_valid && gs_raw == 0) { + score += 1; + } else { + score -= 110; + } + + float track_rate = 0; + if (track_rate_valid) { + track_rate = track_rate_raw * 8.0 / 256.0; + if (track_rate_sign) { + track_rate -= 16; + } + + if (track_rate >= -10.0 && track_rate <= 10.0) { + score += 11; + } else { + score -= 5; + } + } else if (!track_rate_valid && track_rate_raw == 0 && !track_rate_sign) { + score += 1; + } else { + score -= 110; + } + + unsigned tas = 0; + if (tas_valid && tas_raw != 0) { + tas = tas_raw * 2; + + if (tas >= 50 && tas <= 700) { + score += 11; + } else { + score -= 5; + } + } else if (!tas_valid && tas_raw == 0) { + score += 1; + } else { + score -= 110; + } + + // small bonuses for consistent data + if (gs_valid && tas_valid) { + int delta = abs((int)gs_valid - (int)tas_valid); + if (delta < 50) { + score += 5; + } else if (delta > 150) { + score -= 5; + } + } + + // compute the theoretical turn rate and compare to track angle rate + if (roll_valid && tas_valid && tas > 0 && track_rate_valid) { + double turn_rate = 68625 * tan(roll * M_PI / 180.0) / (tas * 20 * M_PI); + double delta = fabs(turn_rate - track_rate); + if (delta < 0.5) { + score += 5; + } else if (delta > 2.0) { + score -= 5; + } + } + + if (store) { + mm->commb_format = COMMB_TRACK_TURN; + + if (roll_valid) { + mm->roll_valid = 1; + mm->roll = roll; + } + + if (track_valid) { + mm->track_valid = 1; + mm->track = track; + } + + if (gs_valid) { + mm->gs_valid = 1; + mm->gs = gs; + } + + if (track_rate_valid) { + mm->track_rate_valid = 1; + mm->track_rate = track_rate; + } + + if (tas_valid) { + mm->tas_valid = 1; + mm->tas = tas; + } + } + + return score; +} + +// BDS6,0 Heading and speed report +static int decodeBDS60(struct modesMessage *mm, bool store) +{ + unsigned char *msg = mm->MB; + + unsigned heading_valid = getbit(msg, 1); + unsigned heading_sign = getbit(msg, 2); + unsigned heading_raw = getbits(msg, 3, 12); + + unsigned ias_valid = getbit(msg, 13); + unsigned ias_raw = getbits(msg, 14, 23); + + unsigned mach_valid = getbit(msg, 24); + unsigned mach_raw = getbits(msg, 25, 34); + + unsigned baro_rate_valid = getbit(msg, 35); + unsigned baro_rate_sign = getbit(msg, 36); + unsigned baro_rate_raw = getbits(msg, 37, 45); + + unsigned inertial_rate_valid = getbit(msg, 46); + unsigned inertial_rate_sign = getbit(msg, 47); + unsigned inertial_rate_raw = getbits(msg, 48, 56); + + if (!heading_valid && !ias_valid && !mach_valid && !baro_rate_valid && !inertial_rate_valid) { + return 0; + } + + int score = 0; + + float heading = 0; + if (heading_valid) { + heading = heading_raw * 90.0 / 512.0; + if (heading_sign) { + heading += 180.0; + } + score += 12; + } else if (!heading_valid && heading_raw == 0 && !heading_sign) { + score += 1; + } else { + score -= 120; + } + + unsigned ias = 0; + if (ias_valid && ias_raw != 0) { + ias = ias_raw; + if (ias >= 50 && ias <= 700) { + score += 11; + } else { + score -= 5; + } + } else if (!ias_valid && ias_raw == 0) { + score += 1; + } else { + score -= 110; + } + + float mach = 0; + if (mach_valid && mach_raw != 0) { + mach = mach_raw * 2.048 / 512; + if (mach >= 0.1 && mach <= 0.9) { + score += 11; + } else { + score -= 5; + } + } else if (!mach_valid && mach_raw == 0) { + score += 1; + } else { + score -= 110; + } + + int baro_rate = 0; + if (baro_rate_valid) { + baro_rate = baro_rate_raw * 32; + if (baro_rate_sign) { + baro_rate -= 16384; + } + + if (baro_rate >= -6000 && baro_rate <= 6000) { + score += 11; + } else { + score -= 5; + } + } else if (!baro_rate_valid && baro_rate_raw == 0) { + score += 1; + } else { + score -= 110; + } + + int inertial_rate = 0; + if (inertial_rate_valid) { + inertial_rate = inertial_rate_raw * 32; + if (inertial_rate_sign) { + inertial_rate -= 16384; + } + + if (inertial_rate >= -6000 && inertial_rate <= 6000) { + score += 11; + } else { + score -= 5; + } + } else if (!inertial_rate_valid && inertial_rate_raw == 0) { + score += 1; + } else { + score -= 110; + } + + // small bonuses for consistent data + if (ias_valid && mach_valid) { + double delta = fabs(ias / 666.0 - mach); + if (delta < 0.1) { + score += 5; + } else if (delta > 0.25) { + score -= 5; + } + } + + if (baro_rate_valid && inertial_rate_valid) { + int delta = abs(baro_rate - inertial_rate); + if (delta < 500) { + score += 5; + } else if (delta > 200) { + score -= 5; + } + } + + if (store) { + mm->commb_format = COMMB_HEADING_SPEED; + + if (heading_valid) { + mm->mag_heading_valid = 1; + mm->mag_heading = heading; + } + + if (ias_valid) { + mm->ias_valid = 1; + mm->ias = ias; + } + + if (mach_valid) { + mm->mach_valid = 1; + mm->mach = mach; + } + + if (baro_rate_valid) { + mm->baro_rate_valid = 1; + mm->baro_rate = baro_rate; + } + + if (inertial_rate_valid) { + // INS-derived data is treated as a "geometric rate" / "geometric altitude" + // elsewhere, so do the same here. + mm->geom_rate_valid = 1; + mm->geom_rate = inertial_rate; + } + } + + return score; +} diff --git a/comm_b.h b/comm_b.h new file mode 100644 index 0000000..e7cc888 --- /dev/null +++ b/comm_b.h @@ -0,0 +1,26 @@ +// Part of dump1090, a Mode S message decoder for RTLSDR devices. +// +// comm_b.h: Comm-B message decoding (prototypes) +// +// Copyright (c) 2017 FlightAware, LLC +// Copyright (c) 2017 Oliver Jowett +// +// This file is free software: you may copy, redistribute and/or modify it +// under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 2 of the License, or (at your +// option) any later version. +// +// This file is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef COMM_B_H +#define COMM_B_H + +void decodeCommB(struct modesMessage *mm); + +#endif diff --git a/dump1090.h b/dump1090.h index 07c2247..d74c547 100644 --- a/dump1090.h +++ b/dump1090.h @@ -167,7 +167,7 @@ typedef enum { typedef enum { ALTITUDE_BARO, - ALTITUDE_GNSS + ALTITUDE_GEOM } altitude_source_t; typedef enum { @@ -177,12 +177,6 @@ typedef enum { AG_UNCERTAIN } airground_t; -typedef enum { - SPEED_GROUNDSPEED, - SPEED_IAS, - SPEED_TAS -} speed_source_t; - typedef enum { HEADING_TRUE, HEADING_MAGNETIC @@ -196,6 +190,18 @@ typedef enum { CPR_SURFACE, CPR_AIRBORNE, CPR_COARSE } cpr_type_t; +typedef enum { + COMMB_UNKNOWN, + COMMB_EMPTY_RESPONSE, + COMMB_DATALINK_CAPS, + COMMB_GICB_CAPS, + COMMB_AIRCRAFT_IDENT, + COMMB_ACAS_RA, + COMMB_VERTICAL_INTENT, + COMMB_TRACK_TURN, + COMMB_HEADING_SPEED +} commb_format_t; + #define MODES_NON_ICAO_ADDRESS (1<<24) // Set on addresses to indicate they are not ICAO addresses #define MODES_DEBUG_DEMOD (1<<0) @@ -406,19 +412,24 @@ struct modesMessage { // Decoded data unsigned altitude_valid : 1; - unsigned heading_valid : 1; - unsigned speed_valid : 1; - unsigned vert_rate_valid : 1; + unsigned track_valid : 1; + unsigned track_rate_valid : 1; + unsigned mag_heading_valid : 1; + unsigned roll_valid : 1; + unsigned gs_valid : 1; + unsigned ias_valid : 1; + unsigned tas_valid : 1; + unsigned mach_valid : 1; + unsigned baro_rate_valid : 1; + unsigned geom_rate_valid : 1; unsigned squawk_valid : 1; unsigned callsign_valid : 1; - unsigned ew_velocity_valid : 1; - unsigned ns_velocity_valid : 1; unsigned cpr_valid : 1; unsigned cpr_odd : 1; unsigned cpr_decoded : 1; unsigned cpr_relative : 1; unsigned category_valid : 1; - unsigned gnss_delta_valid : 1; + unsigned geom_delta_valid : 1; unsigned from_mlat : 1; unsigned from_tisb : 1; unsigned spi_valid : 1; @@ -429,26 +440,27 @@ struct modesMessage { unsigned metype; // DF17/18 ME type unsigned mesub; // DF17/18 ME subtype + commb_format_t commb_format; // Inferred format of a comm-b message + // valid if altitude_valid: int altitude; // Altitude in either feet or meters altitude_unit_t altitude_unit; // the unit used for altitude - altitude_source_t altitude_source; // whether the altitude is a barometric altude or a GNSS height - // valid if gnss_delta_valid: - int gnss_delta; // difference between GNSS and baro alt - // valid if heading_valid: - unsigned heading; // Reported by aircraft, or computed from from EW and NS velocity - heading_source_t heading_source; // what "heading" is measuring (true or magnetic heading) - // valid if speed_valid: - unsigned speed; // in kts, reported by aircraft, or computed from from EW and NS velocity - speed_source_t speed_source; // what "speed" is measuring (groundspeed / IAS / TAS) - // valid if vert_rate_valid: - int vert_rate; // vertical rate in feet/minute - altitude_source_t vert_rate_source; // the altitude source used for vert_rate - // valid if squawk_valid: - unsigned squawk; // 13 bits identity (Squawk), encoded as 4 hex digits - // valid if callsign_valid - char callsign[9]; // 8 chars flight number - // valid if category_valid + altitude_source_t altitude_source; // whether the altitude is a barometric altitude or a geometric height + + // following fields are valid if the corresponding _valid field is set: + int geom_delta; // Difference between geometric and baro alt + float track; // True ground track, degrees (0-359). Reported directly or computed from from EW and NS velocity + float track_rate; // Rate of change of track, degrees/second + float mag_heading; // Magnetic heading, degrees (0-359) + float roll; // Roll, degrees, negative is left roll + unsigned gs; // Groundspeed, kts, reported directly or computed from from EW and NS velocity + unsigned ias; // Indicated airspeed, kts + unsigned tas; // True airspeed, kts + double mach; // Mach number + int baro_rate; // Rate of change of barometric altitude, feet/minute + int geom_rate; // Rate of change of geometric (GNSS / INS) altitude, feet/minute + unsigned squawk; // 13 bits identity (Squawk), encoded as 4 hex digits + char callsign[9]; // 8 chars flight number, NUL-terminated unsigned category; // A0 - D7 encoded as a single hex byte // valid if cpr_valid cpr_type_t cpr_type; // The encoding type used (surface, airborne, coarse TIS-B) @@ -500,32 +512,37 @@ struct modesMessage { unsigned cc_antenna_offset; } opstatus; - // Target State & Status (ADS-B V2 only) + // combined: + // Target State & Status (ADS-B V2 only) + // Comm-B BDS4,0 Vertical Intent struct { unsigned valid : 1; - unsigned altitude_valid : 1; - unsigned baro_valid : 1; - unsigned heading_valid : 1; - unsigned mode_valid : 1; - unsigned mode_autopilot : 1; - unsigned mode_vnav : 1; - unsigned mode_alt_hold : 1; - unsigned mode_approach : 1; - unsigned acas_operational : 1; - unsigned nac_p : 4; - unsigned nic_baro : 1; - unsigned sil : 2; - sil_type_t sil_type; - enum { TSS_ALTITUDE_MCP, TSS_ALTITUDE_FMS } altitude_type; - unsigned altitude; - float baro; - unsigned heading; - } tss; + unsigned heading_valid : 1; + unsigned fms_altitude_valid : 1; + unsigned mcp_altitude_valid : 1; + unsigned alt_setting_valid : 1; + + float heading; // heading, degrees (0-359) (could be magnetic or true heading; magnetic recommended) + unsigned fms_altitude; // FMS selected altitude + unsigned mcp_altitude; // MCP/FCU selected altitude + float alt_setting; // altimeter setting (QFE or QNH/QNE), millibars + + enum { TARGET_INVALID, TARGET_UNKNOWN, TARGET_AIRCRAFT, TARGET_MCP, TARGET_FMS } altitude_source; + + unsigned mode_autopilot : 1; // Autopilot engaged + unsigned mode_vnav : 1; // Vertical Navigation Mode active + unsigned mode_alt_hold : 1; // Altitude Hold Mode active + unsigned mode_approach : 1; // Approach Mode active + unsigned mode_lnav : 1; // Lateral Navigation Mode active + + } intent; }; // This one needs modesMessage: #include "track.h" +#include "mode_s.h" +#include "comm_b.h" // ======================== function declarations ========================= @@ -542,14 +559,6 @@ void modeACInit(); int modeAToModeC (unsigned int modeA); unsigned modeCToModeA (int modeC); -// -// Functions exported from mode_s.c -// -int modesMessageLenByType(int type); -int scoreModesMessage(unsigned char *msg, int validbits); -int decodeModesMessage (struct modesMessage *mm, unsigned char *msg); -void displayModesMessage(struct modesMessage *mm); -void useModesMessage (struct modesMessage *mm); // // Functions exported from interactive.c // diff --git a/interactive.c b/interactive.c index 85554b2..09fcd1b 100644 --- a/interactive.c +++ b/interactive.c @@ -130,12 +130,12 @@ void interactiveShowData(void) { snprintf(strSquawk,5,"%04x", a->squawk); } - if (trackDataValid(&a->speed_valid)) { - snprintf (strGs, 5,"%3d", convert_speed(a->speed)); + if (trackDataValid(&a->gs_valid)) { + snprintf (strGs, 5,"%3d", convert_speed(a->gs)); } - if (trackDataValid(&a->heading_valid)) { - snprintf (strTt, 5,"%03d", a->heading); + if (trackDataValid(&a->track_valid)) { + snprintf (strTt, 5,"%03.0f", a->track); } if (msgs > 99999) { @@ -160,8 +160,8 @@ void interactiveShowData(void) { if (trackDataValid(&a->airground_valid) && a->airground == AG_GROUND) { snprintf(strFl, 7," grnd"); - } else if (Modes.use_gnss && trackDataValid(&a->altitude_gnss_valid)) { - snprintf(strFl, 7, "%5dH", convert_altitude(a->altitude_gnss)); + } else if (Modes.use_gnss && trackDataValid(&a->altitude_geom_valid)) { + snprintf(strFl, 7, "%5dH", convert_altitude(a->altitude_geom)); } else if (trackDataValid(&a->altitude_valid)) { snprintf(strFl, 7, "%5d ", convert_altitude(a->altitude)); } diff --git a/mode_s.c b/mode_s.c index be41840..f14f0f1 100644 --- a/mode_s.c +++ b/mode_s.c @@ -53,8 +53,6 @@ /* for PRIX64 */ #include -#include - // // ===================== Mode S detection and decoding =================== // @@ -220,67 +218,6 @@ static int correct_aa_field(uint32_t *addr, struct errorinfo *ei) return addr_errors; } -// The first bit (MSB of the first byte) is numbered 1, for consistency -// with how the specs number them. - -// Extract one bit from a message. -static inline __attribute__((always_inline)) unsigned getbit(unsigned char *data, unsigned bitnum) -{ - unsigned bi = bitnum - 1; - unsigned by = bi >> 3; - unsigned mask = 1 << (7 - (bi & 7)); - - return (data[by] & mask) != 0; -} - -// Extract some bits (firstbit .. lastbit inclusive) from a message. -static inline __attribute__((always_inline)) unsigned getbits(unsigned char *data, unsigned firstbit, unsigned lastbit) -{ - unsigned fbi = firstbit - 1; - unsigned lbi = lastbit - 1; - unsigned nbi = (lastbit - firstbit + 1); - - unsigned fby = fbi >> 3; - unsigned lby = lbi >> 3; - unsigned nby = (lby - fby) + 1; - - unsigned shift = 7 - (lbi & 7); - unsigned topmask = 0xFF >> (fbi & 7); - - assert (fbi <= lbi); - assert (nbi <= 32); - assert (nby <= 5); - - if (nby == 5) { - return - ((data[fby] & topmask) << (32 - shift)) | - (data[fby + 1] << (24 - shift)) | - (data[fby + 2] << (16 - shift)) | - (data[fby + 3] << (8 - shift)) | - (data[fby + 4] >> shift); - } else if (nby == 4) { - return - ((data[fby] & topmask) << (24 - shift)) | - (data[fby + 1] << (16 - shift)) | - (data[fby + 2] << (8 - shift)) | - (data[fby + 3] >> shift); - } else if (nby == 3) { - return - ((data[fby] & topmask) << (16 - shift)) | - (data[fby + 1] << (8 - shift)) | - (data[fby + 2] >> shift); - } else if (nby == 2) { - return - ((data[fby] & topmask) << (8 - shift)) | - (data[fby + 1] >> shift); - } else if (nby == 1) { - return - (data[fby] & topmask) >> shift; - } else { - return 0; - } -} - // Score how plausible this ModeS message looks. // The more positive, the more reliable the message is @@ -417,8 +354,8 @@ int scoreModesMessage(unsigned char *msg, int validbits) // static void decodeExtendedSquitter(struct modesMessage *mm); -static void decodeCommB(struct modesMessage *mm); -static char *ais_charset = "@ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_ !\"#$%&'()*+,-./0123456789:;<=>?"; + +char ais_charset[64] = "@ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_ !\"#$%&'()*+,-./0123456789:;<=>?"; // return 0 if all OK // -1: message might be valid, but we couldn't validate the CRC against a known ICAO @@ -737,33 +674,6 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg) return 0; } -// Decode BDS2,0 carried in Comm-B or ES -static void decodeBDS20(struct modesMessage *mm) -{ - unsigned char *msg = mm->msg; - - mm->callsign[0] = ais_charset[getbits(msg, 41, 46)]; - mm->callsign[1] = ais_charset[getbits(msg, 47, 52)]; - mm->callsign[2] = ais_charset[getbits(msg, 53, 58)]; - mm->callsign[3] = ais_charset[getbits(msg, 59, 64)]; - mm->callsign[4] = ais_charset[getbits(msg, 65, 70)]; - mm->callsign[5] = ais_charset[getbits(msg, 71, 76)]; - mm->callsign[6] = ais_charset[getbits(msg, 77, 82)]; - mm->callsign[7] = ais_charset[getbits(msg, 83, 88)]; - mm->callsign[8] = 0; - - // Catch possible bad decodings since BDS2,0 is not - // 100% reliable: accept only alphanumeric data - mm->callsign_valid = 1; - for (int i = 0; i < 8; ++i) { - if (! ((mm->callsign[i] >= 'A' && mm->callsign[i] <= 'Z') || - (mm->callsign[i] >= '0' && mm->callsign[i] <= '9') || - mm->callsign[i] == ' ') ) { - mm->callsign_valid = 0; - break; - } - } -} static void decodeESIdentAndCategory(struct modesMessage *mm) { @@ -828,13 +738,18 @@ static void decodeESAirborneVelocity(struct modesMessage *mm, int check_imf) return; unsigned vert_rate = getbits(me, 38, 46); + unsigned vert_rate_is_geom = getbit(me, 36); if (vert_rate) { - mm->vert_rate = (vert_rate - 1) * (getbit(me, 37) ? -64 : 64); - mm->vert_rate_valid = 1; + int rate = (vert_rate - 1) * (getbit(me, 37) ? -64 : 64); + if (vert_rate_is_geom) { + mm->geom_rate = rate; + mm->geom_rate_valid = 1; + } else { + mm->baro_rate = rate; + mm->baro_rate_valid = 1; + } } - mm->vert_rate_source = (getbit(me, 36) ? ALTITUDE_GNSS : ALTITUDE_BARO); - switch (mm->mesub) { case 1: case 2: { @@ -846,20 +761,17 @@ static void decodeESAirborneVelocity(struct modesMessage *mm, int check_imf) int ns_vel = (ns_raw - 1) * (getbit(me, 25) ? -1 : 1) * ((mm->mesub == 2) ? 4 : 1); // Compute velocity and angle from the two speed components - mm->speed = (unsigned) sqrt((ns_vel * ns_vel) + (ew_vel * ew_vel) + 0.5); - mm->speed_valid = 1; + mm->gs = (unsigned) sqrt((ns_vel * ns_vel) + (ew_vel * ew_vel) + 0.5); + mm->gs_valid = 1; - if (mm->speed) { - int heading = (int) (atan2(ew_vel, ns_vel) * 180.0 / M_PI + 0.5); + if (mm->gs) { + float heading = atan2(ew_vel, ns_vel) * 180.0 / M_PI; // We don't want negative values but a 0-360 scale if (heading < 0) heading += 360; - mm->heading = (unsigned) heading; - mm->heading_source = HEADING_TRUE; - mm->heading_valid = 1; + mm->track = heading; + mm->track_valid = 1; } - - mm->speed_source = SPEED_GROUNDSPEED; } break; } @@ -868,15 +780,19 @@ static void decodeESAirborneVelocity(struct modesMessage *mm, int check_imf) { unsigned airspeed = getbits(me, 26, 35); if (airspeed) { - mm->speed = (airspeed - 1) * (mm->mesub == 4 ? 4 : 1); - mm->speed_source = getbit(me, 25) ? SPEED_TAS : SPEED_IAS; - mm->speed_valid = 1; + unsigned speed = (airspeed - 1) * (mm->mesub == 4 ? 4 : 1); + if (getbit(me, 25)) { + mm->tas_valid = 1; + mm->tas = speed; + } else { + mm->ias_valid = 1; + mm->ias = speed; + } } if (getbit(me, 14)) { - mm->heading = getbits(me, 15, 24); - mm->heading_source = HEADING_MAGNETIC; - mm->heading_valid = 1; + mm->mag_heading_valid = 1; + mm->mag_heading = getbits(me, 15, 24) * 360.0 / 1024.0; } break; } @@ -884,8 +800,8 @@ static void decodeESAirborneVelocity(struct modesMessage *mm, int check_imf) unsigned raw_delta = getbits(me, 50, 56); if (raw_delta) { - mm->gnss_delta_valid = 1; - mm->gnss_delta = (raw_delta - 1) * (getbit(me, 49) ? -25 : 25); + mm->geom_delta_valid = 1; + mm->geom_delta = (raw_delta - 1) * (getbit(me, 49) ? -25 : 25); } } @@ -907,15 +823,13 @@ static void decodeESSurfacePosition(struct modesMessage *mm, int check_imf) unsigned movement = getbits(me, 6, 12); if (movement > 0 && movement < 125) { - mm->speed_valid = 1; - mm->speed = decodeMovementField(movement); - mm->speed_source = SPEED_GROUNDSPEED; + mm->gs_valid = 1; + mm->gs = decodeMovementField(movement); } if (getbit(me, 13)) { - mm->heading_valid = 1; - mm->heading_source = HEADING_TRUE; - mm->heading = getbits(me, 14, 20) * 360 / 128; + mm->track_valid = 1; + mm->track = getbits(me, 14, 20) * 360.0 / 128.0; } } @@ -967,7 +881,7 @@ static void decodeESAirbornePosition(struct modesMessage *mm, int check_imf) mm->altitude_valid = 1; } - mm->altitude_source = (mm->metype == 20 || mm->metype == 21 || mm->metype == 22) ? ALTITUDE_GNSS : ALTITUDE_BARO; + mm->altitude_source = (mm->metype == 20 || mm->metype == 21 || mm->metype == 22) ? ALTITUDE_GEOM : ALTITUDE_BARO; } } @@ -1017,45 +931,50 @@ static void decodeESTargetStatus(struct modesMessage *mm, int check_imf) if (mm->mesub == 0) { // Target state and status, V1 // TODO: need RTCA/DO-260A } else if (mm->mesub == 1) { // Target state and status, V2 - mm->tss.valid = 1; - mm->tss.sil_type = getbit(me, 8) ? SIL_PER_SAMPLE : SIL_PER_HOUR; - mm->tss.altitude_type = getbit(me, 9) ? TSS_ALTITUDE_FMS : TSS_ALTITUDE_MCP; + mm->intent.valid = 1; + + // 8: SIL + unsigned is_fms = getbit(me, 9); unsigned alt_bits = getbits(me, 10, 20); - if (alt_bits == 0) { - mm->tss.altitude_valid = 0; - } else { - mm->tss.altitude_valid = 1; - mm->tss.altitude = (alt_bits - 1) * 32; + if (alt_bits != 0) { + if (is_fms) { + mm->intent.fms_altitude_valid = 1; + mm->intent.fms_altitude = (alt_bits - 1) * 32; + } else { + mm->intent.mcp_altitude_valid = 1; + mm->intent.mcp_altitude = (alt_bits - 1) * 32; + } } unsigned baro_bits = getbits(me, 21, 29); - if (baro_bits == 0) { - mm->tss.baro_valid = 0; - } else { - mm->tss.baro_valid = 1; - mm->tss.baro = 800.0 + (baro_bits - 1) * 0.8; + if (baro_bits != 0) { + mm->intent.alt_setting_valid = 1; + mm->intent.alt_setting = 800.0 + (baro_bits - 1) * 0.8; } - mm->tss.heading_valid = getbit(me, 30); - if (mm->tss.heading_valid) { + if (getbit(me, 30)) { + mm->intent.heading_valid = 1; // two's complement -180..+180, which is conveniently // also the same as unsigned 0..360 - mm->tss.heading = getbits(me, 31, 39) * 180 / 256; + mm->intent.heading = getbits(me, 31, 39) * 180.0 / 256.0; } - mm->tss.nac_p = getbits(me, 40, 43); - mm->tss.nic_baro = getbit(me, 44); - mm->tss.sil = getbits(me, 45, 46); - mm->tss.mode_valid = getbit(me, 47); - if (mm->tss.mode_valid) { - mm->tss.mode_autopilot = getbit(me, 48); - mm->tss.mode_vnav = getbit(me, 49); - mm->tss.mode_alt_hold = getbit(me, 50); - mm->tss.mode_approach = getbit(me, 52); + // 40-43: NACp + // 44: NICbaro + // 45-46: SIL + + if (getbit(me, 47)) { + mm->intent.mode_autopilot = getbit(me, 48); + mm->intent.mode_vnav = getbit(me, 49); + mm->intent.mode_alt_hold = getbit(me, 50); + // 51: IMF + mm->intent.mode_approach = getbit(me, 52); + // 53: TCAS operational + mm->intent.mode_lnav = getbit(me, 54); } - mm->tss.acas_operational = getbit(me, 53); + // 55-56 reserved } } @@ -1230,7 +1149,7 @@ static void decodeExtendedSquitter(struct modesMessage *mm) case 0: // Airborne position, baro altitude only case 9: case 10: case 11: case 12: case 13: case 14: case 15: case 16: case 17: case 18: // Airborne position, baro - case 20: case 21: case 22: // Airborne position, GNSS altitude (HAE or MSL) + case 20: case 21: case 22: // Airborne position, geometric altitude (HAE or MSL) decodeESAirbornePosition(mm, check_imf); break; @@ -1261,16 +1180,6 @@ static void decodeExtendedSquitter(struct modesMessage *mm) } } -static void decodeCommB(struct modesMessage *mm) -{ - unsigned char *msg = mm->msg; - - // This is a bit hairy as we don't know what the requested register was - if (getbits(msg, 33, 40) == 0x20) { // BDS 2,0 Aircraft Identification - decodeBDS20(mm); - } -} - static const char *df_names[33] = { /* 0 */ "Short Air-Air Surveillance", /* 1 */ NULL, @@ -1330,8 +1239,8 @@ static const char *altitude_source_to_string(altitude_source_t source) { switch (source) { case ALTITUDE_BARO: return "barometric"; - case ALTITUDE_GNSS: - return "GNSS"; + case ALTITUDE_GEOM: + return "geometric"; default: return "(unknown altitude source)"; } @@ -1352,19 +1261,6 @@ static const char *airground_to_string(airground_t airground) { } } -static const char *speed_source_to_string(speed_source_t speed) { - switch (speed) { - case SPEED_GROUNDSPEED: - return "groundspeed"; - case SPEED_IAS: - return "IAS"; - case SPEED_TAS: - return "TAS"; - default: - return "(unknown speed type)"; - } -} - static const char *addrtype_to_string(addrtype_t type) { switch (type) { case ADDR_ADSB_ICAO: @@ -1403,6 +1299,29 @@ static const char *cpr_type_to_string(cpr_type_t type) { } } +static const char *commb_format_to_string(commb_format_t format) { + switch (format) { + case COMMB_EMPTY_RESPONSE: + return "empty response"; + case COMMB_DATALINK_CAPS: + return "BDS1,0 Datalink capabilities"; + case COMMB_GICB_CAPS: + return "BDS1,7 Common usage GICB capabilities"; + case COMMB_AIRCRAFT_IDENT: + return "BDS2,0 Aircraft identification"; + case COMMB_ACAS_RA: + return "BDS3,0 ACAS resolution advisory"; + case COMMB_VERTICAL_INTENT: + return "BDS4,0 Selected vertical intention"; + case COMMB_TRACK_TURN: + return "BDS5,0 Track and turn report"; + case COMMB_HEADING_SPEED: + return "BDS6,0 Heading and speed report"; + default: + return "unknown format"; + } +} + static void print_hex_bytes(unsigned char *data, size_t len) { size_t i; for (i = 0; i < len; ++i) { @@ -1455,7 +1374,7 @@ static const char *esTypeName(unsigned metype, unsigned mesub) } case 20: case 21: case 22: - return "Airborne position (GNSS altitude)"; + return "Airborne position (geometric altitude)"; case 23: switch (mesub) { @@ -1639,6 +1558,10 @@ void displayModesMessage(struct modesMessage *mm) { } printf("\n"); + if (mm->msgtype == 20 || mm->msgtype == 21) { + printf(" Comm-B format: %s\n", commb_format_to_string(mm->commb_format)); + } + if (mm->addr & MODES_NON_ICAO_ADDRESS) { printf(" Other Address: %06X (%s)\n", mm->addr & 0xFFFFFF, addrtype_to_string(mm->addrtype)); } else { @@ -1657,25 +1580,49 @@ void displayModesMessage(struct modesMessage *mm) { altitude_source_to_string(mm->altitude_source)); } - if (mm->gnss_delta_valid) { - printf(" GNSS delta: %d ft\n", - mm->gnss_delta); + if (mm->geom_delta_valid) { + printf(" Geom - baro: %d ft\n", + mm->geom_delta); } - if (mm->heading_valid) { - printf(" Heading: %u\n", mm->heading); + if (mm->track_valid) { + printf(" Track: %.1f\n", mm->track); } - if (mm->speed_valid) { - printf(" Speed: %u kt %s\n", - mm->speed, - speed_source_to_string(mm->speed_source)); + if (mm->mag_heading_valid) { + printf(" Mag heading: %.1f\n", mm->mag_heading); } - if (mm->vert_rate_valid) { - printf(" Vertical rate: %d ft/min %s\n", - mm->vert_rate, - altitude_source_to_string(mm->vert_rate_source)); + if (mm->track_rate_valid) { + printf(" Track rate: %.2f deg/sec %s\n", mm->track_rate, mm->track_rate < 0 ? "left" : mm->track_rate > 0 ? "right" : ""); + } + + if (mm->roll_valid) { + printf(" Roll: %.1f degrees %s\n", mm->roll, mm->roll < -0.05 ? "left" : mm->roll > 0.05 ? "right" : ""); + } + + if (mm->gs_valid) { + printf(" Groundspeed: %u kt\n", mm->gs); + } + + if (mm->ias_valid) { + printf(" IAS: %u kt\n", mm->ias); + } + + if (mm->tas_valid) { + printf(" TAS: %u kt\n", mm->tas); + } + + if (mm->mach_valid) { + printf(" Mach number: %.3f\n", mm->mach); + } + + if (mm->baro_rate_valid) { + printf(" Baro rate: %d ft/min\n", mm->baro_rate); + } + + if (mm->geom_rate_valid) { + printf(" Geom rate: %d ft/min\n", mm->geom_rate); } if (mm->squawk_valid) { @@ -1758,26 +1705,47 @@ void displayModesMessage(struct modesMessage *mm) { printf(" Heading reference: %s\n", (mm->opstatus.hrd == HEADING_TRUE ? "true north" : "magnetic north")); } - if (mm->tss.valid) { - printf(" Target State and Status:\n"); - if (mm->tss.altitude_valid) - printf(" Target altitude: %s, %d ft\n", (mm->tss.altitude_type == TSS_ALTITUDE_MCP ? "MCP" : "FMS"), mm->tss.altitude); - if (mm->tss.baro_valid) - printf(" Altimeter setting: %.1f millibars\n", mm->tss.baro); - if (mm->tss.heading_valid) - printf(" Target heading: %d\n", mm->tss.heading); - if (mm->tss.mode_valid) { - printf(" Active modes: "); - if (mm->tss.mode_autopilot) printf("autopilot "); - if (mm->tss.mode_vnav) printf("VNAV "); - if (mm->tss.mode_alt_hold) printf("altitude-hold "); - if (mm->tss.mode_approach) printf("approach "); + if (mm->intent.valid) { + printf(" Intent:\n"); + if (mm->intent.heading_valid) + printf(" Selected heading: %.1f\n", mm->intent.heading); + if (mm->intent.fms_altitude_valid) + printf(" FMS selected altitude: %u ft\n", mm->intent.fms_altitude); + if (mm->intent.mcp_altitude_valid) + printf(" MCP selected altitude: %u ft\n", mm->intent.mcp_altitude); + if (mm->intent.alt_setting_valid) + printf(" Altimeter setting: %.1f millibars\n", mm->intent.alt_setting); + + if (mm->intent.altitude_source != TARGET_INVALID) { + printf(" Target altitude source: "); + switch (mm->intent.altitude_source) { + case TARGET_AIRCRAFT: + printf("aircraft altitude\n"); + break; + case TARGET_MCP: + printf("MCP selected altitude\n"); + break; + case TARGET_FMS: + printf("FMS selected altitude\n"); + break; + default: + printf("unknown\n"); + } + } + + if (mm->intent.mode_autopilot || + mm->intent.mode_vnav || + mm->intent.mode_alt_hold || + mm->intent.mode_approach || + mm->intent.mode_lnav) { + printf(" Active modes: "); + if (mm->intent.mode_autopilot) printf("autopilot "); + if (mm->intent.mode_vnav) printf("VNAV "); + if (mm->intent.mode_alt_hold) printf("altitude-hold "); + if (mm->intent.mode_approach) printf("approach "); + if (mm->intent.mode_lnav) printf("LNAV "); printf("\n"); } - printf(" ACAS: %s\n", mm->tss.acas_operational ? "operational" : "NOT operational"); - printf(" NACp: %d\n", mm->tss.nac_p); - printf(" NICbaro: %d\n", mm->tss.nic_baro); - printf(" SIL: %d (%s)\n", mm->tss.sil, (mm->opstatus.sil_type == SIL_PER_HOUR ? "per hour" : "per sample")); } printf("\n"); diff --git a/mode_s.h b/mode_s.h new file mode 100644 index 0000000..4fbc983 --- /dev/null +++ b/mode_s.h @@ -0,0 +1,100 @@ +// Part of dump1090, a Mode S message decoder for RTLSDR devices. +// +// mode_s.h: Mode S message decoding (prototypes) +// +// Copyright (c) 2017 FlightAware, LLC +// Copyright (c) 2017 Oliver Jowett +// +// This file is free software: you may copy, redistribute and/or modify it +// under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 2 of the License, or (at your +// option) any later version. +// +// This file is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef MODE_S_H +#define MODE_S_H + +#include + +// +// Functions exported from mode_s.c +// +int modesMessageLenByType(int type); +int scoreModesMessage(unsigned char *msg, int validbits); +int decodeModesMessage (struct modesMessage *mm, unsigned char *msg); +void displayModesMessage(struct modesMessage *mm); +void useModesMessage (struct modesMessage *mm); + +// datafield extraction helpers + +// The first bit (MSB of the first byte) is numbered 1, for consistency +// with how the specs number them. + +// Extract one bit from a message. +static inline __attribute__((always_inline)) unsigned getbit(unsigned char *data, unsigned bitnum) +{ + unsigned bi = bitnum - 1; + unsigned by = bi >> 3; + unsigned mask = 1 << (7 - (bi & 7)); + + return (data[by] & mask) != 0; +} + +// Extract some bits (firstbit .. lastbit inclusive) from a message. +static inline __attribute__((always_inline)) unsigned getbits(unsigned char *data, unsigned firstbit, unsigned lastbit) +{ + unsigned fbi = firstbit - 1; + unsigned lbi = lastbit - 1; + unsigned nbi = (lastbit - firstbit + 1); + + unsigned fby = fbi >> 3; + unsigned lby = lbi >> 3; + unsigned nby = (lby - fby) + 1; + + unsigned shift = 7 - (lbi & 7); + unsigned topmask = 0xFF >> (fbi & 7); + + assert (fbi <= lbi); + assert (nbi <= 32); + assert (nby <= 5); + + if (nby == 5) { + return + ((data[fby] & topmask) << (32 - shift)) | + (data[fby + 1] << (24 - shift)) | + (data[fby + 2] << (16 - shift)) | + (data[fby + 3] << (8 - shift)) | + (data[fby + 4] >> shift); + } else if (nby == 4) { + return + ((data[fby] & topmask) << (24 - shift)) | + (data[fby + 1] << (16 - shift)) | + (data[fby + 2] << (8 - shift)) | + (data[fby + 3] >> shift); + } else if (nby == 3) { + return + ((data[fby] & topmask) << (16 - shift)) | + (data[fby + 1] << (8 - shift)) | + (data[fby + 2] >> shift); + } else if (nby == 2) { + return + ((data[fby] & topmask) << (8 - shift)) | + (data[fby + 1] >> shift); + } else if (nby == 1) { + return + (data[fby] & topmask) >> shift; + } else { + return 0; + } +} + +extern char ais_charset[64]; + +#endif diff --git a/net_io.c b/net_io.c index 35e1e27..66fce2b 100644 --- a/net_io.c +++ b/net_io.c @@ -582,18 +582,18 @@ static void modesSendSBSOutput(struct modesMessage *mm, struct aircraft *a) { // Field 12 is the altitude (if we have it) if (mm->altitude_valid) { if (Modes.use_gnss) { - if (mm->altitude_source == ALTITUDE_GNSS) { + if (mm->altitude_source == ALTITUDE_GEOM) { p += sprintf(p, ",%dH", mm->altitude); - } else if (trackDataValid(&a->gnss_delta_valid)) { - p += sprintf(p, ",%dH", mm->altitude + a->gnss_delta); + } else if (trackDataValid(&a->geom_delta_valid)) { + p += sprintf(p, ",%dH", mm->altitude + a->geom_delta); } else { p += sprintf(p, ",%d", mm->altitude); } } else { if (mm->altitude_source == ALTITUDE_BARO) { p += sprintf(p, ",%d", mm->altitude); - } else if (trackDataValid(&a->gnss_delta_valid)) { - p += sprintf(p, ",%d", mm->altitude - a->gnss_delta); + } else if (trackDataValid(&a->geom_delta_valid)) { + p += sprintf(p, ",%d", mm->altitude - a->geom_delta); } else { p += sprintf(p, ","); } @@ -603,15 +603,15 @@ static void modesSendSBSOutput(struct modesMessage *mm, struct aircraft *a) { } // Field 13 is the ground Speed (if we have it) - if (mm->speed_valid && mm->speed_source == SPEED_GROUNDSPEED) { - p += sprintf(p, ",%d", mm->speed); + if (mm->gs_valid) { + p += sprintf(p, ",%d", mm->gs); } else { p += sprintf(p, ","); } - // Field 14 is the ground Heading (if we have it) - if (mm->heading_valid && mm->heading_source == HEADING_TRUE) { - p += sprintf(p, ",%d", mm->heading); + // Field 14 is the ground Heading (if we have it) + if (mm->track_valid) { + p += sprintf(p, ",%.0f", mm->track); } else { p += sprintf(p, ","); } @@ -624,10 +624,22 @@ static void modesSendSBSOutput(struct modesMessage *mm, struct aircraft *a) { } // Field 17 is the VerticalRate (if we have it) - if (mm->vert_rate_valid) { - p += sprintf(p, ",%d", mm->vert_rate); + if (Modes.use_gnss) { + if (mm->geom_rate_valid) { + p += sprintf(p, ",%dH", mm->geom_rate); + } else if (mm->baro_rate_valid) { + p += sprintf(p, ",%d", mm->baro_rate); + } else { + p += sprintf(p, ","); + } } else { - p += sprintf(p, ","); + if (mm->baro_rate_valid) { + p += sprintf(p, ",%d", mm->baro_rate); + } else if (mm->geom_rate_valid) { + p += sprintf(p, ",%d", mm->geom_rate); + } else { + p += sprintf(p, ","); + } } // Field 18 is the Squawk (if we have it) @@ -1099,12 +1111,14 @@ static char *append_flags(char *p, char *end, struct aircraft *a, datasource_t s p += snprintf(p, end-p, "\"lat\",\"lon\","); if (a->altitude_valid.source == source) p += snprintf(p, end-p, "\"altitude\","); - if (a->heading_valid.source == source) + if (a->track_valid.source == source) p += snprintf(p, end-p, "\"track\","); - if (a->speed_valid.source == source) + if (a->gs_valid.source == source) p += snprintf(p, end-p, "\"speed\","); - if (a->vert_rate_valid.source == source) - p += snprintf(p, end-p, "\"vert_rate\","); + if (a->baro_rate_valid.source == source) + p += snprintf(p, end-p, "\"baro_rate\","); + if (a->geom_rate_valid.source == source) + p += snprintf(p, end-p, "\"geom_rate\","); if (a->category_valid.source == source) p += snprintf(p, end-p, "\"category\","); if (p[-1] != '[') @@ -1173,14 +1187,30 @@ char *generateAircraftJson(const char *url_path, int *len) { p += snprintf(p, end-p, ",\"lat\":%f,\"lon\":%f,\"nucp\":%u,\"seen_pos\":%.1f", a->lat, a->lon, a->pos_nuc, (now - a->position_valid.updated)/1000.0); if (trackDataValid(&a->airground_valid) && a->airground_valid.source >= SOURCE_MODE_S_CHECKED && a->airground == AG_GROUND) p += snprintf(p, end-p, ",\"altitude\":\"ground\""); - else if (trackDataValid(&a->altitude_valid)) - p += snprintf(p, end-p, ",\"altitude\":%d", a->altitude); - if (trackDataValid(&a->vert_rate_valid)) - p += snprintf(p, end-p, ",\"vert_rate\":%d", a->vert_rate); - if (trackDataValid(&a->heading_valid)) - p += snprintf(p, end-p, ",\"track\":%d", a->heading); - if (trackDataValid(&a->speed_valid)) - p += snprintf(p, end-p, ",\"speed\":%d", a->speed); + else { + if (trackDataValid(&a->altitude_valid)) + p += snprintf(p, end-p, ",\"altitude\":%d", a->altitude); + if (trackDataValid(&a->altitude_geom_valid)) + p += snprintf(p, end-p, ",\"altitude_geom\":%d", a->altitude_geom); + } + if (trackDataValid(&a->baro_rate_valid)) + p += snprintf(p, end-p, ",\"baro_rate\":%d", a->baro_rate); + if (trackDataValid(&a->geom_rate_valid)) + p += snprintf(p, end-p, ",\"geom_rate\":%d", a->geom_rate); + if (trackDataValid(&a->track_valid)) + p += snprintf(p, end-p, ",\"track\":%.1f", a->track); + if (trackDataValid(&a->track_rate_valid)) + p += snprintf(p, end-p, ",\"track_rate\":%.2f", a->track_rate); + if (trackDataValid(&a->gs_valid)) + p += snprintf(p, end-p, ",\"gs\":%u", a->gs); + if (trackDataValid(&a->ias_valid)) + p += snprintf(p, end-p, ",\"ias\":%u", a->ias); + if (trackDataValid(&a->tas_valid)) + p += snprintf(p, end-p, ",\"tas\":%u", a->tas); + if (trackDataValid(&a->mach_valid)) + p += snprintf(p, end-p, ",\"mach\":%.3f", a->mach); + if (trackDataValid(&a->roll_valid)) + p += snprintf(p, end-p, ",\"roll\":%.1f", a->roll); if (trackDataValid(&a->category_valid)) p += snprintf(p, end-p, ",\"category\":\"%02X\"", a->category); @@ -1662,7 +1692,7 @@ static void modesReadFromClient(struct client *c) { } } -#define TSV_MAX_PACKET_SIZE 275 +#define TSV_MAX_PACKET_SIZE 400 static void writeFATSVPositionUpdate(float lat, float lon, float alt) { @@ -1747,23 +1777,28 @@ static void writeFATSVEvent(struct modesMessage *mm, struct aircraft *a) switch (mm->msgtype) { case 20: case 21: - if (mm->correctedbits > 0) - break; // only messages we trust a little more - // DF 20/21: Comm-B: emit if they've changed since we last sent them - // - // BDS 1,0: data link capability report - // BDS 3,0: ACAS RA report - if (mm->MB[0] == 0x10 && memcmp(mm->MB, a->fatsv_emitted_bds_10, 7) != 0) { - memcpy(a->fatsv_emitted_bds_10, mm->MB, 7); - writeFATSVEventMessage(mm, "datalink_caps", mm->MB, 7); - } + switch (mm->commb_format) { + case COMMB_DATALINK_CAPS: + // BDS 1,0: data link capability report + if (memcmp(mm->MB, a->fatsv_emitted_bds_10, 7) != 0) { + memcpy(a->fatsv_emitted_bds_10, mm->MB, 7); + writeFATSVEventMessage(mm, "datalink_caps", mm->MB, 7); + } + break; - else if (mm->MB[0] == 0x30 && memcmp(mm->MB, a->fatsv_emitted_bds_30, 7) != 0) { - memcpy(a->fatsv_emitted_bds_30, mm->MB, 7); - writeFATSVEventMessage(mm, "commb_acas_ra", mm->MB, 7); - } + case COMMB_ACAS_RA: + // BDS 3,0: ACAS RA report + if (memcmp(mm->MB, a->fatsv_emitted_bds_30, 7) != 0) { + memcpy(a->fatsv_emitted_bds_30, mm->MB, 7); + writeFATSVEventMessage(mm, "commb_acas_ra", mm->MB, 7); + } + break; + default: + // nothing + break; + } break; case 17: @@ -1778,10 +1813,6 @@ static void writeFATSVEvent(struct modesMessage *mm, struct aircraft *a) // aircraft operational status memcpy(a->fatsv_emitted_es_status, mm->ME, 7); writeFATSVEventMessage(mm, "es_op_status", mm->ME, 7); - } else if (mm->metype == 29 && (mm->mesub == 0 || mm->mesub == 1) && memcmp(mm->ME, a->fatsv_emitted_es_target, 7) != 0) { - // target state and status - memcpy(a->fatsv_emitted_es_target, mm->ME, 7); - writeFATSVEventMessage(mm, "es_target", mm->ME, 7); } break; } @@ -1791,13 +1822,13 @@ typedef enum { TISB_IDENT = 1, TISB_SQUAWK = 2, TISB_ALTITUDE = 4, - TISB_ALTITUDE_GNSS = 8, - TISB_SPEED = 16, - TISB_SPEED_IAS = 32, - TISB_SPEED_TAS = 64, + TISB_ALTITUDE_GEOM = 8, + TISB_GS = 16, + TISB_IAS = 32, + TISB_TAS = 64, TISB_POSITION = 128, - TISB_HEADING = 256, - TISB_HEADING_MAGNETIC = 512, + TISB_TRACK = 256, + TISB_MAG_HEADING = 512, TISB_AIRGROUND = 1024, TISB_CATEGORY = 2048 } tisb_flags; @@ -1807,9 +1838,9 @@ static inline unsigned unsigned_difference(unsigned v1, unsigned v2) return (v1 > v2) ? (v1 - v2) : (v2 - v1); } -static inline unsigned heading_difference(unsigned h1, unsigned h2) +static inline float heading_difference(float h1, float h2) { - unsigned d = unsigned_difference(h1, h2); + float d = fabs(h1 - h2); return (d < 180) ? d : (360 - d); } @@ -1832,17 +1863,6 @@ static void writeFATSV() next_update = now + 1000; for (a = Modes.aircrafts; a; a = a->next) { - int altValid = 0; - int altGNSSValid = 0; - int positionValid = 0; - int speedValid = 0; - int speedIASValid = 0; - int speedTASValid = 0; - int headingValid = 0; - int headingMagValid = 0; - int airgroundValid = 0; - int categoryValid = 0; - uint64_t minAge; int useful = 0; @@ -1859,16 +1879,25 @@ static void writeFATSV() continue; } - altValid = trackDataValidEx(&a->altitude_valid, now, 15000, SOURCE_MODE_S); // for non-ADS-B transponders, DF0/4/16/20 are the only sources of altitude data - altGNSSValid = trackDataValidEx(&a->altitude_gnss_valid, now, 15000, SOURCE_MODE_S_CHECKED); - airgroundValid = trackDataValidEx(&a->airground_valid, now, 15000, SOURCE_MODE_S_CHECKED); // for non-ADS-B transponders, only trust DF11 CA field - positionValid = trackDataValidEx(&a->position_valid, now, 15000, SOURCE_MODE_S_CHECKED); - headingValid = trackDataValidEx(&a->heading_valid, now, 15000, SOURCE_MODE_S_CHECKED); - headingMagValid = trackDataValidEx(&a->heading_magnetic_valid, now, 15000, SOURCE_MODE_S_CHECKED); - speedValid = trackDataValidEx(&a->speed_valid, now, 15000, SOURCE_MODE_S_CHECKED); - speedIASValid = trackDataValidEx(&a->speed_ias_valid, now, 15000, SOURCE_MODE_S_CHECKED); - speedTASValid = trackDataValidEx(&a->speed_tas_valid, now, 15000, SOURCE_MODE_S_CHECKED); - categoryValid = trackDataValidEx(&a->category_valid, now, 15000, SOURCE_MODE_S_CHECKED); + int altValid = trackDataValidEx(&a->altitude_valid, now, 15000, SOURCE_MODE_S); // for non-ADS-B transponders, DF0/4/16/20 are the only sources of altitude data + int altGeomValid = trackDataValidEx(&a->altitude_geom_valid, now, 15000, SOURCE_MODE_S_CHECKED); + int airgroundValid = trackDataValidEx(&a->airground_valid, now, 15000, SOURCE_MODE_S_CHECKED); // for non-ADS-B transponders, only trust DF11 CA field + int baroRateValid = trackDataValidEx(&a->baro_rate_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int geomRateValid = trackDataValidEx(&a->geom_rate_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int positionValid = trackDataValidEx(&a->position_valid, now, 15000, SOURCE_MODE_S_CHECKED); + int trackValid = trackDataValidEx(&a->track_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int trackRateValid = trackDataValidEx(&a->track_rate_valid, now, 15000, SOURCE_MODE_S); // Comm-B + int rollValid = trackDataValidEx(&a->roll_valid, now, 15000, SOURCE_MODE_S); // Comm-B + int magHeadingValid = trackDataValidEx(&a->mag_heading_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int gsValid = trackDataValidEx(&a->gs_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int iasValid = trackDataValidEx(&a->ias_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int tasValid = trackDataValidEx(&a->tas_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int machValid = trackDataValidEx(&a->mach_valid, now, 15000, SOURCE_MODE_S); // Comm-B + int categoryValid = trackDataValidEx(&a->category_valid, now, 15000, SOURCE_MODE_S_CHECKED); + int intentAltValid = trackDataValidEx(&a->intent_altitude_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int intentHeadingValid = trackDataValidEx(&a->intent_heading_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int altSettingValid = trackDataValidEx(&a->alt_setting_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + int callsignValid = trackDataValidEx(&a->callsign, now, 15000, SOURCE_MODE_S); // Comm-B or ES // If we are definitely on the ground, suppress any unreliable altitude info. // When on the ground, ADS-B transponders don't emit an ADS-B message that includes @@ -1883,22 +1912,49 @@ static void writeFATSV() if (altValid && abs(a->altitude - a->fatsv_emitted_altitude) >= 50) { changed = 1; } - if (altGNSSValid && abs(a->altitude_gnss - a->fatsv_emitted_altitude_gnss) >= 50) { + if (altGeomValid && abs(a->altitude_geom - a->fatsv_emitted_altitude_gnss) >= 50) { changed = 1; } - if (headingValid && heading_difference(a->heading, a->fatsv_emitted_heading) >= 2) { + if (baroRateValid && abs(a->baro_rate - a->fatsv_emitted_baro_rate) > 500) { changed = 1; } - if (headingMagValid && heading_difference(a->heading_magnetic, a->fatsv_emitted_heading_magnetic) >= 2) { + if (geomRateValid && abs(a->geom_rate - a->fatsv_emitted_geom_rate) > 500) { changed = 1; } - if (speedValid && unsigned_difference(a->speed, a->fatsv_emitted_speed) >= 25) { + if (trackValid && heading_difference(a->track, a->fatsv_emitted_heading) >= 2) { changed = 1; } - if (speedIASValid && unsigned_difference(a->speed_ias, a->fatsv_emitted_speed_ias) >= 25) { + if (trackRateValid && fabs(a->track_rate - a->fatsv_emitted_track_rate) >= 0.5) { changed = 1; } - if (speedTASValid && unsigned_difference(a->speed_tas, a->fatsv_emitted_speed_tas) >= 25) { + if (rollValid && fabs(a->roll - a->fatsv_emitted_roll) >= 5.0) { + changed = 1; + } + if (magHeadingValid && heading_difference(a->mag_heading, a->fatsv_emitted_heading_magnetic) >= 2) { + changed = 1; + } + if (gsValid && unsigned_difference(a->gs, a->fatsv_emitted_speed) >= 25) { + changed = 1; + } + if (iasValid && unsigned_difference(a->ias, a->fatsv_emitted_speed_ias) >= 25) { + changed = 1; + } + if (tasValid && unsigned_difference(a->tas, a->fatsv_emitted_speed_tas) >= 25) { + changed = 1; + } + if (machValid && fabs(a->mach - a->fatsv_emitted_mach) >= 0.02) { + changed = 1; + } + if (intentAltValid && unsigned_difference(a->intent_altitude, a->fatsv_emitted_intent_altitude) > 50) { + changed = 1; + } + if (intentHeadingValid && heading_difference(a->intent_heading, a->fatsv_emitted_intent_heading) > 2) { + changed = 1; + } + if (altSettingValid && fabs(a->alt_setting - a->fatsv_emitted_alt_setting) > 0.8) { // 0.8 is the ES message resolution + changed = 1; + } + if (callsignValid && strcmp(a->callsign, a->fatsv_emitted_callsign) != 0) { changed = 1; } @@ -1910,8 +1966,8 @@ static void writeFATSV() // don't send mode S very often minAge = 30000; } else if ((airgroundValid && a->airground == AG_GROUND) || - (altValid && a->altitude < 500 && (!speedValid || a->speed < 200)) || - (speedValid && a->speed < 100 && (!altValid || a->altitude < 1000))) { + (altValid && a->altitude < 500 && (!gsValid || a->gs < 200)) || + (gsValid && a->gs < 100 && (!altValid || a->altitude < 1000))) { // we are probably on the ground, increase the update rate minAge = 1000; } else if (!altValid || a->altitude < 10000) { @@ -1944,7 +2000,7 @@ static void writeFATSV() p += snprintf(p, bufsize(p, end), "\taddrtype\t%s", addrtype_short_string(a->addrtype)); } - if (trackDataValidEx(&a->callsign_valid, now, 35000, SOURCE_MODE_S_CHECKED) && strcmp(a->callsign, " ") != 0 && a->callsign_valid.updated > a->fatsv_last_emitted) { + if (trackDataValidEx(&a->callsign_valid, now, 35000, SOURCE_MODE_S) && strcmp(a->callsign, " ") != 0 && a->callsign_valid.updated > a->fatsv_last_emitted) { p += snprintf(p, bufsize(p,end), "\tident\t%s", a->callsign); switch (a->callsign_valid.source) { case SOURCE_MODE_S: @@ -1971,7 +2027,7 @@ static void writeFATSV() tisb |= (a->squawk_valid.source == SOURCE_TISB) ? TISB_SQUAWK : 0; } - // only emit alt, speed, latlon, track if they have been received since the last time + // only emit alt, speed, latlon, track etc if they have been received since the last time // and are not stale if (altValid && a->altitude_valid.updated > a->fatsv_last_emitted) { @@ -1981,32 +2037,50 @@ static void writeFATSV() tisb |= (a->altitude_valid.source == SOURCE_TISB) ? TISB_ALTITUDE : 0; } - if (altGNSSValid && a->altitude_gnss_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\talt_gnss\t%d", a->altitude_gnss); - a->fatsv_emitted_altitude_gnss = a->altitude_gnss; + if (altGeomValid && a->altitude_geom_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\talt_geom\t%d", a->altitude_geom); + a->fatsv_emitted_altitude_gnss = a->altitude_geom; useful = 1; - tisb |= (a->altitude_gnss_valid.source == SOURCE_TISB) ? TISB_ALTITUDE_GNSS : 0; + tisb |= (a->altitude_geom_valid.source == SOURCE_TISB) ? TISB_ALTITUDE_GEOM : 0; } - if (speedValid && a->speed_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tspeed\t%d", a->speed); - a->fatsv_emitted_speed = a->speed; + if (baroRateValid && a->baro_rate_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tbaro_rate\t%d", a->baro_rate); + a->fatsv_emitted_baro_rate = a->baro_rate; useful = 1; - tisb |= (a->speed_valid.source == SOURCE_TISB) ? TISB_SPEED : 0; } - if (speedIASValid && a->speed_ias_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tspeed_ias\t%d", a->speed_ias); - a->fatsv_emitted_speed_ias = a->speed_ias; + if (geomRateValid && a->geom_rate_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tgeom_rate\t%d", a->geom_rate); + a->fatsv_emitted_geom_rate = a->geom_rate; useful = 1; - tisb |= (a->speed_ias_valid.source == SOURCE_TISB) ? TISB_SPEED_IAS : 0; } - if (speedTASValid && a->speed_tas_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tspeed_tas\t%d", a->speed_tas); - a->fatsv_emitted_speed_tas = a->speed_tas; + if (gsValid && a->gs_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tgs\t%u", a->gs); + a->fatsv_emitted_speed = a->gs; + useful = 1; + tisb |= (a->gs_valid.source == SOURCE_TISB) ? TISB_GS : 0; + } + + if (iasValid && a->ias_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tias\t%u", a->ias); + a->fatsv_emitted_speed_ias = a->ias; + useful = 1; + tisb |= (a->ias_valid.source == SOURCE_TISB) ? TISB_IAS : 0; + } + + if (tasValid && a->tas_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\ttas\t%u", a->tas); + a->fatsv_emitted_speed_tas = a->tas; + useful = 1; + tisb |= (a->tas_valid.source == SOURCE_TISB) ? TISB_TAS : 0; + } + + if (machValid && a->mach_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tmach\t%.3f", a->mach); + a->fatsv_emitted_mach = a->mach; useful = 1; - tisb |= (a->speed_tas_valid.source == SOURCE_TISB) ? TISB_SPEED_TAS : 0; } if (positionValid && a->position_valid.updated > a->fatsv_last_emitted) { @@ -2015,18 +2089,30 @@ static void writeFATSV() tisb |= (a->position_valid.source == SOURCE_TISB) ? TISB_POSITION : 0; } - if (headingValid && a->heading_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\theading\t%d", a->heading); - a->fatsv_emitted_heading = a->heading; + if (trackValid && a->track_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\ttrack\t%.0f", a->track); + a->fatsv_emitted_heading = a->track; useful = 1; - tisb |= (a->heading_valid.source == SOURCE_TISB) ? TISB_HEADING : 0; + tisb |= (a->track_valid.source == SOURCE_TISB) ? TISB_TRACK : 0; } - if (headingMagValid && a->heading_magnetic_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\theading_magnetic\t%d", a->heading_magnetic); - a->fatsv_emitted_heading_magnetic = a->heading_magnetic; + if (trackRateValid && a->track_rate_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\ttrack_rate\t%.2f", a->track_rate); + a->fatsv_emitted_track_rate = a->track_rate; useful = 1; - tisb |= (a->heading_magnetic_valid.source == SOURCE_TISB) ? TISB_HEADING_MAGNETIC : 0; + } + + if (rollValid && a->roll_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\troll\t%.1f", a->roll); + a->fatsv_emitted_roll = a->roll; + useful = 1; + } + + if (magHeadingValid && a->mag_heading_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tmag_heading\t%.0f", a->mag_heading); + a->fatsv_emitted_heading_magnetic = a->mag_heading; + useful = 1; + tisb |= (a->mag_heading_valid.source == SOURCE_TISB) ? TISB_MAG_HEADING : 0; } if (airgroundValid && (a->airground == AG_GROUND || a->airground == AG_AIRBORNE) && a->airground_valid.updated > a->fatsv_last_emitted) { @@ -2043,6 +2129,25 @@ static void writeFATSV() tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_CATEGORY : 0; } + if (intentAltValid && a->intent_altitude_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tintent_alt\t%u", a->intent_altitude); + a->fatsv_emitted_intent_altitude = a->intent_altitude; + useful = 1; + } + + if (intentHeadingValid && a->intent_heading_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\tintent_heading\t%.0f", a->intent_heading); + a->fatsv_emitted_intent_heading = a->intent_heading; + useful = 1; + } + + if (altSettingValid && a->alt_setting_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, bufsize(p,end), "\talt_setting\t%.1f", a->alt_setting); + a->fatsv_emitted_alt_setting = a->alt_setting; + useful = 1; + } + + // if we didn't get anything interesting, bail out. // We don't need to do anything special to unwind prepareWrite(). if (!useful) { diff --git a/track.c b/track.c index 24e6348..ea98ab1 100644 --- a/track.c +++ b/track.c @@ -209,12 +209,12 @@ static int speed_check(struct aircraft *a, double lat, double lon, uint64_t now, elapsed = trackDataAge(&a->position_valid, now); - if (trackDataValid(&a->speed_valid)) - speed = a->speed; - else if (trackDataValid(&a->speed_ias_valid)) - speed = a->speed_ias * 4 / 3; - else if (trackDataValid(&a->speed_tas_valid)) - speed = a->speed_tas * 4 / 3; + if (trackDataValid(&a->gs_valid)) + speed = a->gs; + else if (trackDataValid(&a->tas_valid)) + speed = a->tas * 4 / 3; + else if (trackDataValid(&a->ias_valid)) + speed = a->ias * 2; else speed = surface ? 100 : 600; // guess @@ -427,7 +427,7 @@ static void updatePosition(struct aircraft *a, struct modesMessage *mm, uint64_t ++Modes.stats_current.cpr_surface; // Surface: 25 seconds if >25kt or speed unknown, 50 seconds otherwise - if (mm->speed_valid && mm->speed <= 25) + if (mm->gs_valid && mm->gs <= 25) max_elapsed = 50000; else max_elapsed = 25000; @@ -562,37 +562,52 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) a->squawk = mm->squawk; } - if (mm->altitude_valid && mm->altitude_source == ALTITUDE_GNSS && accept_data(&a->altitude_gnss_valid, mm->source, now)) { - a->altitude_gnss = mm->altitude; + if (mm->altitude_valid && mm->altitude_source == ALTITUDE_GEOM && accept_data(&a->altitude_geom_valid, mm->source, now)) { + a->altitude_geom = mm->altitude; } - if (mm->gnss_delta_valid && accept_data(&a->gnss_delta_valid, mm->source, now)) { - a->gnss_delta = mm->gnss_delta; + if (mm->geom_delta_valid && accept_data(&a->geom_delta_valid, mm->source, now)) { + a->geom_delta = mm->geom_delta; } - if (mm->heading_valid && mm->heading_source == HEADING_TRUE && accept_data(&a->heading_valid, mm->source, now)) { - a->heading = mm->heading; + if (mm->track_valid && accept_data(&a->track_valid, mm->source, now)) { + a->track = mm->track; } - if (mm->heading_valid && mm->heading_source == HEADING_MAGNETIC && accept_data(&a->heading_magnetic_valid, mm->source, now)) { - a->heading_magnetic = mm->heading; + if (mm->track_rate_valid && accept_data(&a->track_rate_valid, mm->source, now)) { + a->track_rate = mm->track_rate; } - if (mm->speed_valid && mm->speed_source == SPEED_GROUNDSPEED && accept_data(&a->speed_valid, mm->source, now)) { - a->speed = mm->speed; + if (mm->roll_valid && accept_data(&a->roll_valid, mm->source, now)) { + a->roll = mm->roll; } - if (mm->speed_valid && mm->speed_source == SPEED_IAS && accept_data(&a->speed_ias_valid, mm->source, now)) { - a->speed_ias = mm->speed; + if (mm->mag_heading_valid && accept_data(&a->mag_heading_valid, mm->source, now)) { + a->mag_heading = mm->mag_heading; } - if (mm->speed_valid && mm->speed_source == SPEED_TAS && accept_data(&a->speed_tas_valid, mm->source, now)) { - a->speed_tas = mm->speed; + if (mm->gs_valid && accept_data(&a->gs_valid, mm->source, now)) { + a->gs = mm->gs; } - if (mm->vert_rate_valid && accept_data(&a->vert_rate_valid, mm->source, now)) { - a->vert_rate = mm->vert_rate; - a->vert_rate_source = mm->vert_rate_source; + if (mm->ias_valid && accept_data(&a->ias_valid, mm->source, now)) { + a->ias = mm->ias; + } + + if (mm->tas_valid && accept_data(&a->tas_valid, mm->source, now)) { + a->tas = mm->tas; + } + + if (mm->mach_valid && accept_data(&a->mach_valid, mm->source, now)) { + a->mach = mm->mach; + } + + if (mm->baro_rate_valid && accept_data(&a->baro_rate_valid, mm->source, now)) { + a->baro_rate = mm->baro_rate; + } + + if (mm->geom_rate_valid && accept_data(&a->geom_rate_valid, mm->source, now)) { + a->geom_rate = mm->geom_rate; } if (mm->category_valid && accept_data(&a->category_valid, mm->source, now)) { @@ -607,6 +622,22 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) memcpy(a->callsign, mm->callsign, sizeof(a->callsign)); } + if (mm->intent.valid) { + if (mm->intent.mcp_altitude_valid && accept_data(&a->intent_altitude_valid, mm->source, now)) { + a->intent_altitude = mm->intent.mcp_altitude; + } else if (mm->intent.fms_altitude_valid && accept_data(&a->intent_altitude_valid, mm->source, now)) { + a->intent_altitude = mm->intent.fms_altitude; + } + + if (mm->intent.heading_valid && accept_data(&a->intent_heading_valid, mm->source, now)) { + a->intent_heading = mm->intent.heading; + } + + if (mm->intent.alt_setting_valid && accept_data(&a->alt_setting_valid, mm->source, now)) { + a->alt_setting = mm->intent.alt_setting; + } + } + // CPR, even if (mm->cpr_valid && !mm->cpr_odd && accept_data(&a->cpr_even_valid, mm->source, now)) { a->cpr_even_type = mm->cpr_type; @@ -625,12 +656,12 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) // Now handle derived data - // derive GNSS if we have baro + delta - if (compare_validity(&a->altitude_valid, &a->altitude_gnss_valid, now) > 0 && - compare_validity(&a->gnss_delta_valid, &a->altitude_gnss_valid, now) > 0) { - // Baro and delta are both more recent than GNSS, derive GNSS from baro + delta - a->altitude_gnss = a->altitude + a->gnss_delta; - combine_validity(&a->altitude_gnss_valid, &a->altitude_valid, &a->gnss_delta_valid); + // derive geometric altitude if we have baro + delta + if (compare_validity(&a->altitude_valid, &a->altitude_geom_valid, now) > 0 && + compare_validity(&a->geom_delta_valid, &a->altitude_geom_valid, now) > 0) { + // Baro and delta are both more recent than geometric, derive geometric from baro + delta + a->altitude_geom = a->altitude + a->geom_delta; + combine_validity(&a->altitude_geom_valid, &a->altitude_valid, &a->geom_delta_valid); } // If we've got a new cprlat or cprlon @@ -752,17 +783,24 @@ static void trackRemoveStaleAircraft(uint64_t now) #define EXPIRE(_f) do { if (a->_f##_valid.source != SOURCE_INVALID && now >= a->_f##_valid.expires) { a->_f##_valid.source = SOURCE_INVALID; } } while (0) EXPIRE(callsign); EXPIRE(altitude); - EXPIRE(altitude_gnss); - EXPIRE(gnss_delta); - EXPIRE(speed); - EXPIRE(speed_ias); - EXPIRE(speed_tas); - EXPIRE(heading); - EXPIRE(heading_magnetic); - EXPIRE(vert_rate); + EXPIRE(altitude_geom); + EXPIRE(geom_delta); + EXPIRE(gs); + EXPIRE(ias); + EXPIRE(tas); + EXPIRE(mach); + EXPIRE(track); + EXPIRE(track_rate); + EXPIRE(roll); + EXPIRE(mag_heading); + EXPIRE(baro_rate); + EXPIRE(geom_rate); EXPIRE(squawk); EXPIRE(category); EXPIRE(airground); + EXPIRE(alt_setting); + EXPIRE(intent_altitude); + EXPIRE(intent_heading); EXPIRE(cpr_odd); EXPIRE(cpr_even); EXPIRE(position); diff --git a/track.h b/track.h index 57be9f9..dc441bd 100644 --- a/track.h +++ b/track.h @@ -88,30 +88,41 @@ struct aircraft { data_validity altitude_valid; int altitude; // Altitude (Baro) - data_validity altitude_gnss_valid; - int altitude_gnss; // Altitude (GNSS) + data_validity altitude_geom_valid; + int altitude_geom; // Altitude (Geometric) - data_validity gnss_delta_valid; - int gnss_delta; // Difference between GNSS and Baro altitudes + data_validity geom_delta_valid; + int geom_delta; // Difference between Geometric and Baro altitudes - data_validity speed_valid; - unsigned speed; + data_validity gs_valid; + unsigned gs; - data_validity speed_ias_valid; - unsigned speed_ias; + data_validity ias_valid; + unsigned ias; - data_validity speed_tas_valid; - unsigned speed_tas; + data_validity tas_valid; + unsigned tas; - data_validity heading_valid; - unsigned heading; // Heading (OK it's really the track) + data_validity mach_valid; + float mach; - data_validity heading_magnetic_valid; - unsigned heading_magnetic; // Heading + data_validity track_valid; + float track; // Ground track - data_validity vert_rate_valid; - int vert_rate; // Vertical rate - altitude_source_t vert_rate_source; + data_validity track_rate_valid; + float track_rate; // Rate of change of ground track, degrees/second + + data_validity roll_valid; + float roll; // Roll angle, degrees right + + data_validity mag_heading_valid; + float mag_heading; // Magnetic heading + + data_validity baro_rate_valid; + int baro_rate; // Vertical rate (barometric) + + data_validity geom_rate_valid; + int geom_rate; // Vertical rate (geometric) data_validity squawk_valid; unsigned squawk; // Squawk @@ -122,6 +133,15 @@ struct aircraft { data_validity airground_valid; airground_t airground; // air/ground status + data_validity alt_setting_valid; + float alt_setting; // Altimeter setting (QNH/QFE), millibars + + data_validity intent_altitude_valid; + unsigned intent_altitude; // intent altitude (FMS or FCU selected altitude) + + data_validity intent_heading_valid; + float intent_heading; // intent heading, degrees (0-359) + data_validity cpr_odd_valid; // Last seen even CPR message cpr_type_t cpr_odd_type; unsigned cpr_odd_lat; @@ -143,16 +163,23 @@ struct aircraft { int fatsv_emitted_altitude; // last FA emitted altitude int fatsv_emitted_altitude_gnss; // -"- GNSS altitude - int fatsv_emitted_heading; // -"- true track - int fatsv_emitted_heading_magnetic; // -"- magnetic heading - int fatsv_emitted_speed; // -"- groundspeed - int fatsv_emitted_speed_ias; // -"- IAS - int fatsv_emitted_speed_tas; // -"- TAS + int fatsv_emitted_baro_rate; // -"- barometric rate + int fatsv_emitted_geom_rate; // -"- geometric rate + float fatsv_emitted_heading; // -"- true track + float fatsv_emitted_heading_magnetic; // -"- magnetic heading + float fatsv_emitted_track_rate; // -"- track rate of change + float fatsv_emitted_roll; // -"- roll angle + unsigned fatsv_emitted_speed; // -"- groundspeed + unsigned fatsv_emitted_speed_ias; // -"- IAS + unsigned fatsv_emitted_speed_tas; // -"- TAS + float fatsv_emitted_mach; // -"- Mach number airground_t fatsv_emitted_airground; // -"- air/ground state + unsigned fatsv_emitted_intent_altitude; // -"- intent altitude + float fatsv_emitted_intent_heading; // -"- intent heading + float fatsv_emitted_alt_setting; // -"- altimeter setting unsigned char fatsv_emitted_bds_10[7]; // -"- BDS 1,0 message unsigned char fatsv_emitted_bds_30[7]; // -"- BDS 3,0 message unsigned char fatsv_emitted_es_status[7]; // -"- ES operational status message - unsigned char fatsv_emitted_es_target[7]; // -"- ES target status message unsigned char fatsv_emitted_es_acas_ra[7]; // -"- ES ACAS RA report message uint64_t fatsv_last_emitted; // time (millis) aircraft was last FA emitted