Break up decodeExtendedSquiiter, it was getting pretty large.

mode_s.c

@@ -181,7 +181,7 @@ static int decodeAC12Field(int AC12Field, altitude_unit_t *unit) {
 //
 // Decode the 7 bit ground movement field PWL exponential style scale
 //
-static int decodeMovementField(int movement) {
+static unsigned decodeMovementField(unsigned movement) {
     int gspeed;
 
     // Note : movement codes 0,125,126,127 are all invalid, but they are 
@@ -762,59 +762,15 @@ static void decodeBDS20(struct modesMessage *mm)
     mm->callsign[8] = '\0';
 }
 
-static void decodeExtendedSquitter(struct modesMessage *mm)
+static void decodeESIdentAndCategory(struct modesMessage *mm)
 {
-    unsigned char *me = mm->ME;
-    int metype = mm->metype = getbits(me, 1, 5);
-    int mesub  = mm->mesub  = (metype == 29 ? getbits(me, 6, 7) : getbits(me, 6, 8));   // Extended squitter message subtype
-
-    int check_imf = 0;
-
-    // Check CF on DF18 to work out the format of the ES and whether we need to look for an IMF bit
-    if (mm->msgtype == 18) {
-        switch (mm->CF) {
-        case 0: //   ADS-B ES/NT devices that report the ICAO 24-bit address in the AA field
-            break;
-
-        case 1: //   Reserved for ADS-B for ES/NT devices that use other addressing techniques in the AA field
-            mm->addr |= MODES_NON_ICAO_ADDRESS;
-            break;
-
-        case 2: //   Fine TIS-B message (formats are close enough to DF17 for our purposes)
-            mm->source = SOURCE_TISB;
-            check_imf = 1;
-            break;
-
-        case 3: //   Coarse TIS-B airborne position and velocity.
-            // TODO: decode me.
-            // For now we only look at the IMF bit.
-            mm->source = SOURCE_TISB;
-            if (getbit(me, 1))
-                mm->addr |= MODES_NON_ICAO_ADDRESS;
-            return;
-
-        case 5: //   TIS-B messages that relay ADS-B Messages using anonymous 24-bit addresses (format not explicitly defined, but it seems to follow DF17)
-            mm->source = SOURCE_TISB;
-            mm->addr |= MODES_NON_ICAO_ADDRESS;
-            break;
-
-        case 6: //   ADS-B rebroadcast using the same type codes and message formats as defined for DF = 17 ADS-B messages
-            check_imf = 1;
-            break;
-
-        default:    // All others, we don't know the format.
-            mm->addr |= MODES_NON_ICAO_ADDRESS; // assume non-ICAO
-            return;
-        }
-    }
-
-    switch (metype) {
-    case 1: case 2: case 3: case 4: {
     // Aircraft Identification and Category
-        uint32_t chars1, chars2;
+    unsigned char *me = mm->ME;
 
-        chars1 = getbits(me, 9, 32);
+    mm->mesub = getbits(me, 6, 8);
-        chars2 = getbits(me, 33, 56);
+
+    unsigned chars1 = getbits(me, 9, 32);
+    unsigned chars2 = getbits(me, 33, 56);
 
     // A common failure mode seems to be to intermittently send
     // all zeros. Catch that here.
@@ -834,36 +790,40 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
         mm->callsign[8] = '\0';
     }
 
-        mm->category = ((0x0E - metype) << 4) | mesub;
+    mm->category = ((0x0E - mm->metype) << 4) | mm->mesub;
     mm->category_valid = 1;
-        break;
 }
 
-    case 19: { // Airborne Velocity Message
+static void decodeESAirborneVelocity(struct modesMessage *mm, int check_imf)
+{
+    // Airborne Velocity Message
+    unsigned char *me = mm->ME;
+
+    mm->mesub = getbits(me, 6, 8);
+
     if (check_imf && getbit(me, 9))
         mm->addr |= MODES_NON_ICAO_ADDRESS;
 
-        if ( (mesub >= 1) && (mesub <= 4) ) {
+    if (mm->mesub < 1 || mm->mesub > 4)
-            int vert_rate = getbits(me, 38, 46);
+        return;
+
+    unsigned vert_rate = getbits(me, 38, 46);
     if (vert_rate) {
-                --vert_rate;
+        mm->vert_rate =  (vert_rate - 1) * (getbit(me, 37) ? -64 : 64);
-                if (getbit(me, 37)) {
-                    vert_rate = 0 - vert_rate;
-                }
-                mm->vert_rate =  vert_rate * 64;
         mm->vert_rate_valid = 1;
     }
 
     mm->vert_rate_source = (getbit(me, 36) ? ALTITUDE_GNSS : ALTITUDE_BARO);
-        }
 
-        if ((mesub == 1) || (mesub == 2)) {
+    switch (mm->mesub) {
+    case 1: case 2:
+        {
             unsigned ew_raw = getbits(me, 15, 24);
             unsigned ns_raw = getbits(me, 26, 35);
 
             if (ew_raw && ns_raw) {
-                int ew_vel = (ew_raw - 1) * (getbit(me, 14) ? -1 : 1) * ((mesub == 2) ? 4 : 1);
+                int ew_vel = (ew_raw - 1) * (getbit(me, 14) ? -1 : 1) * ((mm->mesub == 2) ? 4 : 1);
-                int ns_vel = (ns_raw - 1) * (getbit(me, 25) ? -1 : 1) * ((mesub == 2) ? 4 : 1);
+                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);
@@ -881,14 +841,14 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
 
                 mm->speed_source = SPEED_GROUNDSPEED;
             }
-        } else if (mesub == 3 || mesub == 4) {
+            break;
+        }
+
+    case 3: case 4:
+        {
             unsigned airspeed = getbits(me, 26, 35);
             if (airspeed) {
-                --airspeed;
+                mm->speed = (airspeed - 1) * (mm->mesub == 4 ? 4 : 1);
-                if (mesub == 4) { // If (supersonic) unit is 4 kts
-                    airspeed *= 4;
-                }
-                mm->speed = airspeed;
                 mm->speed_source = getbit(me, 25) ? SPEED_TAS : SPEED_IAS;
                 mm->speed_valid = 1;
             }
@@ -898,6 +858,8 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
                 mm->heading_source = HEADING_MAGNETIC;
                 mm->heading_valid = 1;
             }
+            break;
+        }
     }
 
     unsigned raw_delta = getbits(me, 50, 56);
@@ -905,13 +867,12 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
         mm->gnss_delta_valid = 1;
         mm->gnss_delta = (raw_delta - 1) * (getbit(me, 49) ? -25 : 25);
     }
-
-        break;
 }
 
-    case 5: case 6: case 7: case 8: {
+static void decodeESSurfacePosition(struct modesMessage *mm, int check_imf)
-        // Ground position
+{
-        int movement;
+    // Surface position and movement
+    unsigned char *me = mm->ME;
 
     if (check_imf && getbit(me, 21))
         mm->addr |= MODES_NON_ICAO_ADDRESS;
@@ -920,11 +881,11 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
     mm->cpr_lat = getbits(me, 23, 39);
     mm->cpr_lon = getbits(me, 40, 56);
     mm->cpr_odd = getbit(me, 22);
-        mm->cpr_nucp = (14 - metype);
+    mm->cpr_nucp = (14 - mm->metype);
     mm->cpr_valid = 1;
 
-        movement = getbits(me, 6, 12);
+    unsigned movement = getbits(me, 6, 12);
-        if ((movement) && (movement < 125)) {
+    if (movement > 0 && movement < 125) {
         mm->speed_valid = 1;
         mm->speed = decodeMovementField(movement);
         mm->speed_source = SPEED_GROUNDSPEED;
@@ -935,19 +896,19 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
         mm->heading_source = HEADING_TRUE;
         mm->heading = getbits(me, 14, 20) * 360 / 128;
     }
-
-        break;
 }
 
-    case 0: // Airborne position, baro altitude only
+static void decodeESAirbornePosition(struct modesMessage *mm, int check_imf)
-    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)
+    // Airborne position and altitude
-        int AC12Field = getbits(me, 9, 20);
+    unsigned char *me = mm->ME;
 
     if (check_imf && getbit(me, 8))
         mm->addr |= MODES_NON_ICAO_ADDRESS;
 
-        if (metype == 0) {
+    unsigned AC12Field = getbits(me, 9, 20);
+
+    if (mm->metype == 0) {
         mm->cpr_nucp = 0;
     } else {
         // Catch some common failure modes and don't mark them as valid
@@ -969,12 +930,12 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
             mm->cpr_valid = 1;
             mm->cpr_odd = getbit(me, 22);
 
-                if (metype == 18 || metype == 22)
+            if (mm->metype == 18 || mm->metype == 22)
                 mm->cpr_nucp = 0;
-                else if (metype < 18)
+            else if (mm->metype < 18)
-                    mm->cpr_nucp = (18 - metype);
+                mm->cpr_nucp = (18 - mm->metype);
             else
-                    mm->cpr_nucp = (29 - metype);
+                mm->cpr_nucp = (29 - mm->metype);
         }
     }
 
@@ -984,28 +945,33 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
             mm->altitude_valid = 1;
         }
 
-            mm->altitude_source = (metype == 20 || metype == 21 || metype == 22) ? ALTITUDE_GNSS : ALTITUDE_BARO;
+        mm->altitude_source = (mm->metype == 20 || mm->metype == 21 || mm->metype == 22) ? ALTITUDE_GNSS : ALTITUDE_BARO;
+    }
 }
 
-        break;
+static void decodeESTestMessage(struct modesMessage *mm)
-    }
+{
+    unsigned char *me = mm->ME;
 
-    case 23: { // Test message
+    mm->mesub = getbits(me, 6, 8);
-        if (mesub == 7) {               // (see 1090-WP-15-20)
+
+    if (mm->mesub == 7) {               // (see 1090-WP-15-20)
         int ID13Field = getbits(me, 9, 21);
         if (ID13Field) {
             mm->squawk_valid = 1;
             mm->squawk   = decodeID13Field(ID13Field);
         }
     }
-        break;
 }
 
-    case 24: // Reserved for Surface System Status
+static void decodeESAircraftStatus(struct modesMessage *mm, int check_imf)
-        break;
+{
+    // Extended Squitter Aircraft Status
+    unsigned char *me = mm->ME;
 
-    case 28: { // Extended Squitter Aircraft Status
+    mm->mesub = getbits(me, 6, 8);
-        if (mesub == 1) {      // Emergency status squawk field
+
+    if (mm->mesub == 1) {      // Emergency status squawk field
         int ID13Field = getbits(me, 12, 24);
         if (ID13Field) {
             mm->squawk_valid = 1;
@@ -1015,16 +981,20 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
         if (check_imf && getbit(me, 56))
             mm->addr |= MODES_NON_ICAO_ADDRESS;
     }
-        break;
 }
 
-    case 29: // Aircraft Trajectory Intent
+static void decodeESTargetStatus(struct modesMessage *mm, int check_imf)
+{
+    unsigned char *me = mm->ME;
+
+    mm->mesub = getbits(me, 6, 7); // an unusual message: only 2 bits of subtype
+
     if (check_imf && getbit(me, 51))
         mm->addr |= MODES_NON_ICAO_ADDRESS;
 
-        if (mesub == 0) { // Target state and status, V1
+    if (mm->mesub == 0) { // Target state and status, V1
         // TODO: need RTCA/DO-260A
-        } else if (mesub == 1) { // Target state and status, V2
+    } 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;
@@ -1065,12 +1035,15 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
 
         mm->tss.acas_operational = getbit(me, 53);
     }
-        break;
+}
 
-    case 30: // Aircraft Operational Coordination
+static void decodeESOperationalStatus(struct modesMessage *mm, int check_imf)
-        break;
+{
+    unsigned char *me = mm->ME;
 
-    case 31: // Aircraft Operational Status
+    mm->mesub = getbits(me, 6, 8);
+
+    // Aircraft Operational Status
     if (check_imf && getbit(me, 56))
         mm->addr |= MODES_NON_ICAO_ADDRESS;
 
@@ -1161,6 +1134,91 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
             break;
         }
     }
+}
+
+static void decodeExtendedSquitter(struct modesMessage *mm)
+{
+    unsigned char *me = mm->ME;
+    unsigned metype = mm->metype = getbits(me, 1, 5);
+    unsigned check_imf = 0;
+
+    // Check CF on DF18 to work out the format of the ES and whether we need to look for an IMF bit
+    if (mm->msgtype == 18) {
+        switch (mm->CF) {
+        case 0: //   ADS-B ES/NT devices that report the ICAO 24-bit address in the AA field
+            break;
+
+        case 1: //   Reserved for ADS-B for ES/NT devices that use other addressing techniques in the AA field
+            mm->addr |= MODES_NON_ICAO_ADDRESS;
+            break;
+
+        case 2: //   Fine TIS-B message (formats are close enough to DF17 for our purposes)
+            mm->source = SOURCE_TISB;
+            check_imf = 1;
+            break;
+
+        case 3: //   Coarse TIS-B airborne position and velocity.
+            // TODO: decode me.
+            // For now we only look at the IMF bit.
+            mm->source = SOURCE_TISB;
+            if (getbit(me, 1))
+                mm->addr |= MODES_NON_ICAO_ADDRESS;
+            return;
+
+        case 5: //   TIS-B messages that relay ADS-B Messages using anonymous 24-bit addresses (format not explicitly defined, but it seems to follow DF17)
+            mm->source = SOURCE_TISB;
+            mm->addr |= MODES_NON_ICAO_ADDRESS;
+            break;
+
+        case 6: //   ADS-B rebroadcast using the same type codes and message formats as defined for DF = 17 ADS-B messages
+            check_imf = 1;
+            break;
+
+        default:    // All others, we don't know the format.
+            mm->addr |= MODES_NON_ICAO_ADDRESS; // assume non-ICAO
+            return;
+        }
+    }
+
+    switch (metype) {
+    case 1: case 2: case 3: case 4:
+        decodeESIdentAndCategory(mm);
+        break;
+
+    case 19:
+        decodeESAirborneVelocity(mm, check_imf);
+        break;
+
+    case 5: case 6: case 7: case 8:
+        decodeESSurfacePosition(mm, check_imf);
+        break;
+
+    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)
+        decodeESAirbornePosition(mm, check_imf);
+        break;
+
+    case 23:
+        decodeESTestMessage(mm);
+        break;
+
+    case 24: // Reserved for Surface System Status
+        break;
+
+    case 28:
+        decodeESAircraftStatus(mm, check_imf);
+        break;
+
+    case 29:
+        decodeESTargetStatus(mm, check_imf);
+        break;
+
+    case 30: // Aircraft Operational Coordination
+        break;
+
+    case 31:
+        decodeESOperationalStatus(mm, check_imf);
         break;
 
     default: