From 382554a250fc57098ed7f72e841eab56aacf3fa5 Mon Sep 17 00:00:00 2001 From: Oliver Jowett Date: Fri, 16 Jun 2017 10:39:01 +0100 Subject: [PATCH] Tweak tracking of AP modes, emit in json & faup1090 output --- comm_b.c | 8 +++++--- dump1090.h | 16 ++++++++++------ mode_s.c | 52 +++++++++++++++++++++++++++++++++------------------- net_io.c | 51 ++++++++++++++++++++++++++++++++++++++++++++++++++- track.c | 4 ++++ track.h | 4 ++++ 6 files changed, 106 insertions(+), 29 deletions(-) diff --git a/comm_b.c b/comm_b.c index 80f9022..a5f1bfe 100644 --- a/comm_b.c +++ b/comm_b.c @@ -383,9 +383,11 @@ static int decodeBDS40(struct modesMessage *mm, bool store) } 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; + mm->intent.modes_valid = 1; + mm->intent.modes = + ((mode_raw & 4) ? INTENT_MODE_VNAV : 0) | + ((mode_raw & 2) ? INTENT_MODE_ALT_HOLD : 0) | + ((mode_raw & 1) ? INTENT_MODE_APPROACH : 0); } if (source_valid) { diff --git a/dump1090.h b/dump1090.h index 7da9819..b0fbc09 100644 --- a/dump1090.h +++ b/dump1090.h @@ -205,6 +205,14 @@ typedef enum { COMMB_HEADING_SPEED } commb_format_t; +typedef enum { + INTENT_MODE_AUTOPILOT = 1, + INTENT_MODE_VNAV = 2, + INTENT_MODE_ALT_HOLD = 4, + INTENT_MODE_APPROACH = 8, + INTENT_MODE_LNAV = 16 +} intent_modes_t; + #define MODES_NON_ICAO_ADDRESS (1<<24) // Set on addresses to indicate they are not ICAO addresses #define MODES_DEBUG_DEMOD (1<<0) @@ -526,6 +534,7 @@ struct modesMessage { unsigned fms_altitude_valid : 1; unsigned mcp_altitude_valid : 1; unsigned alt_setting_valid : 1; + unsigned modes_valid : 1; float heading; // heading, degrees (0-359) (could be magnetic or true heading; magnetic recommended) unsigned fms_altitude; // FMS selected altitude @@ -534,12 +543,7 @@ struct modesMessage { enum { INTENT_ALT_INVALID, INTENT_ALT_UNKNOWN, INTENT_ALT_AIRCRAFT, INTENT_ALT_MCP, INTENT_ALT_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_modes_t modes; } intent; }; diff --git a/mode_s.c b/mode_s.c index f5c1405..4ace63e 100644 --- a/mode_s.c +++ b/mode_s.c @@ -968,13 +968,15 @@ static void decodeESTargetStatus(struct modesMessage *mm, int check_imf) // 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->intent.modes_valid = 1; + mm->intent.modes = + (getbit(me, 48) ? INTENT_MODE_AUTOPILOT : 0) | + (getbit(me, 49) ? INTENT_MODE_VNAV : 0) | + (getbit(me, 50) ? INTENT_MODE_ALT_HOLD : 0) | + // 51: IMF + (getbit(me, 52) ? INTENT_MODE_APPROACH : 0) | + // 53: TCAS operational + (getbit(me, 54) ? INTENT_MODE_LNAV : 0); } // 55-56 reserved @@ -1342,6 +1344,28 @@ static const char *commb_format_to_string(commb_format_t format) { } } +static const char *intent_modes_to_string(intent_modes_t flags) +{ + static char buf[128]; + + buf[0] = 0; + if (flags & INTENT_MODE_AUTOPILOT) + strcat(buf, "autopilot "); + if (flags & INTENT_MODE_VNAV) + strcat(buf, "vnav "); + if (flags & INTENT_MODE_ALT_HOLD) + strcat(buf, "althold "); + if (flags & INTENT_MODE_APPROACH) + strcat(buf, "approach "); + if (flags & INTENT_MODE_LNAV) + strcat(buf, "lnav "); + + if (buf[0] != 0) + buf[strlen(buf)-1] = 0; + + return buf; +} + static void print_hex_bytes(unsigned char *data, size_t len) { size_t i; for (i = 0; i < len; ++i) { @@ -1749,18 +1773,8 @@ void displayModesMessage(struct modesMessage *mm) { } } - 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"); + if (mm->intent.modes_valid) { + printf(" Active modes: %s\n", intent_modes_to_string(mm->intent.modes)); } } diff --git a/net_io.c b/net_io.c index d72ce6e..4119d19 100644 --- a/net_io.c +++ b/net_io.c @@ -1135,6 +1135,37 @@ static char *append_flags(char *p, char *end, struct aircraft *a, datasource_t s return p; } +static struct { + intent_modes_t flag; + const char *name; +} intent_modes_names[] = { + { INTENT_MODE_AUTOPILOT, "autopilot" }, + { INTENT_MODE_VNAV, "vnav" }, + { INTENT_MODE_ALT_HOLD, "althold" }, + { INTENT_MODE_APPROACH, "approach" }, + { INTENT_MODE_LNAV, "lnav" }, + { 0, NULL } +}; + +static char *append_intent_modes(char *p, char *end, intent_modes_t flags, const char *quote, const char *sep) +{ + int first = 1; + for (int i = 0; intent_modes_names[i].name; ++i) { + if (!(flags & intent_modes_names[i].flag)) { + continue; + } + + if (!first) { + p += snprintf(p, end-p, "%s", sep); + } + + first = 0; + p += snprintf(p, end-p, "%s%s%s", quote, intent_modes_names[i].name, quote); + } + + return p; +} + static const char *addrtype_short_string(addrtype_t type) { switch (type) { case ADDR_ADSB_ICAO: @@ -1231,6 +1262,11 @@ char *generateAircraftJson(const char *url_path, int *len) { p += snprintf(p, end-p, ",\"intent_alt\":%d", a->intent_altitude); if (trackDataValid(&a->intent_heading_valid)) p += snprintf(p, end-p, ",\"intent_heading\":%.1f", a->intent_heading); + if (trackDataValid(&a->intent_modes_valid)) { + p += snprintf(p, end-p, ",\"intent_modes\":["); + p = append_intent_modes(p, end, a->intent_modes, "\"", ","); + p += snprintf(p, end-p, "]"); + } if (trackDataValid(&a->alt_setting_valid)) p += snprintf(p, end-p, ",\"alt_setting\":%.1f", a->alt_setting); @@ -1855,7 +1891,8 @@ typedef enum { TISB_CATEGORY = 8192, TISB_INTENT_ALT = 16384, TISB_INTENT_HEADING = 32768, - TISB_ALT_SETTING = 65536 + TISB_ALT_SETTING = 65536, + TISB_INTENT_MODES = 131072 } tisb_flags; struct { @@ -1879,6 +1916,7 @@ struct { { TISB_INTENT_ALT, "intent_alt" }, { TISB_INTENT_HEADING, "intent_heading" }, { TISB_ALT_SETTING, "alt_setting" }, + { TISB_INTENT_MODES, "intent_modes" }, { 0, NULL } }; @@ -1945,6 +1983,7 @@ static void writeFATSV() 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 intentModesValid = trackDataValidEx(&a->intent_modes_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_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES @@ -2004,6 +2043,9 @@ static void writeFATSV() if (intentHeadingValid && heading_difference(a->intent_heading, a->fatsv_emitted_intent_heading) > 2) { changed = immediate = 1; } + if (intentModesValid && a->intent_modes != a->fatsv_emitted_intent_modes) { + changed = immediate = 1; + } if (altSettingValid && fabs(a->alt_setting - a->fatsv_emitted_alt_setting) > 0.8) { // 0.8 is the ES message resolution changed = immediate = 1; } @@ -2212,6 +2254,13 @@ static void writeFATSV() tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_INTENT_HEADING : 0; } + if (intentModesValid && a->intent_modes_valid.updated > a->fatsv_last_emitted) { + p += snprintf(p, end-p, "\tintent_modes\t"); + p = append_intent_modes(p, end, a->intent_modes, "", " "); + a->fatsv_emitted_intent_modes = a->intent_modes; + tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_INTENT_MODES : 0; + } + 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; diff --git a/track.c b/track.c index f1b9020..2c094c6 100644 --- a/track.c +++ b/track.c @@ -652,6 +652,10 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) a->intent_heading = mm->intent.heading; } + if (mm->intent.modes_valid && accept_data(&a->intent_modes_valid, mm->source, now)) { + a->intent_modes = mm->intent.modes; + } + if (mm->intent.alt_setting_valid && accept_data(&a->alt_setting_valid, mm->source, now)) { a->alt_setting = mm->intent.alt_setting; } diff --git a/track.h b/track.h index dadebcf..c03063f 100644 --- a/track.h +++ b/track.h @@ -145,6 +145,9 @@ struct aircraft { data_validity intent_heading_valid; float intent_heading; // intent heading, degrees (0-359) + data_validity intent_modes_valid; + intent_modes_t intent_modes; // enabled modes (autopilot, vnav, etc) + data_validity cpr_odd_valid; // Last seen even CPR message cpr_type_t cpr_odd_type; unsigned cpr_odd_lat; @@ -184,6 +187,7 @@ struct aircraft { airground_t fatsv_emitted_airground; // -"- air/ground state unsigned fatsv_emitted_intent_altitude; // -"- intent altitude float fatsv_emitted_intent_heading; // -"- intent heading + intent_modes_t fatsv_emitted_intent_modes; // -"- enabled modes 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