Line data    Source code

       1             : /**
       2             :  * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
       3             :  *
       4             :  * @file   sgDecodeSVR.c
       5             :  * @author jimb
       6             :  *
       7             :  * @date   Feb 10, 2021
       8             :  *
       9             :  * This file receives a raw ADS-B target state vector report message buffer and
      10             :  * parses the payload into a data struct.
      11             :  */
      12             : 
      13             : #include <string.h>
      14             : #include <stdbool.h>
      15             : #include <string.h>
      16             : #include <math.h>
      17             : 
      18             : #include "sg.h"
      19             : #include "sgUtil.h"
      20             : #include "target.h"
      21             : 
      22             : // airborne surface
      23             : // -------- --------
      24             : #define SV_PARAM_TOA_EPOS (1 << 3) // x
      25             : #define SV_PARAM_TOA_POS (1 << 2)  // x        x
      26             : #define SV_PARAM_TOA_VEL (1 << 1)  // x        x
      27             : #define SV_PARAM_LATLON (1 << 0)   // x        x
      28             : 
      29             : #define SV_PARAM_GEOALT (1 << 7)    // x
      30             : #define SV_PARAM_VEL (1 << 6)       // x
      31             : #define SV_PARAM_SURF_GS (1 << 5)   //          x
      32             : #define SV_PARAM_SURF_HEAD (1 << 4) //          x
      33             : #define SV_PARAM_BAROALT (1 << 3)   // x
      34             : #define SV_PARAM_VRATE (1 << 2)     // x
      35             : #define SV_PARAM_NIC (1 << 1)       // x        x
      36             : #define SV_PARAM_ESTLAT (1 << 0)    // x
      37             : 
      38             : #define SV_PARAM_ESTLON (1 << 7) // x
      39             : #define SV_PARAM_ESTNVEL (1 << 6)
      40             : #define SV_PARAM_ESTEVAL (1 << 5)
      41             : #define SV_PARAM_SURV (1 << 4)   // x        x
      42             : #define SV_PARAM_REPORT (1 << 3) // x        x
      43             : 
      44             : /// the payload offset.
      45             : #define PBASE 4
      46             : 
      47             : /*
      48             :  * Documented in the header file.
      49             :  */
      50        4155 : bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr)
      51             : {
      52             :     //   memset(svr, 0, sizeof(sg_svr_t));
      53             : 
      54             :     uint8_t fields[3];
      55        4155 :     memcpy(&fields, &buffer[PBASE + 0], 3);
      56             : 
      57        4155 :     svr->type = buffer[PBASE + 0] == 0x1F ? svrAirborne : svrSurface;
      58        4155 :     svr->flags = buffer[PBASE + 3];
      59        4155 :     svr->eflags = buffer[PBASE + 4];
      60        4155 :     svr->addr = toInt32(&buffer[PBASE + 5]) & 0xFFFFFF;
      61        4155 :     svr->addrType = buffer[PBASE + 8] & 0xFF;
      62             : 
      63        4155 :     uint8_t ofs = 9;
      64             : 
      65        4155 :     if (fields[0] & SV_PARAM_TOA_EPOS)
      66             :     {
      67        4155 :         svr->toaEst = toTOA(&buffer[PBASE + ofs]);
      68        4155 :         ofs += 2;
      69             :     }
      70        4155 :     if (fields[0] & SV_PARAM_TOA_POS)
      71             :     {
      72        4155 :         svr->toaPosition = toTOA(&buffer[PBASE + ofs]);
      73        4155 :         ofs += 2;
      74             :     }
      75        4155 :     if (fields[0] & SV_PARAM_TOA_VEL)
      76             :     {
      77        4155 :         svr->toaSpeed = toTOA(&buffer[PBASE + ofs]);
      78        4155 :         ofs += 2;
      79             :     }
      80             : 
      81        4155 :     if (fields[0] & SV_PARAM_LATLON)
      82             :     {
      83        4155 :         if (svr->validity.position)
      84             :         {
      85        4155 :             svr->lat = toLatLon(&buffer[PBASE + ofs + 0]);
      86        4155 :             svr->lon = toLatLon(&buffer[PBASE + ofs + 3]);
      87             :         }
      88             :         else
      89             :         {
      90           0 :             svr->lat = 0.0;
      91           0 :             svr->lon = 0.0;
      92             :         }
      93             : 
      94        4155 :         ofs += 6;
      95             :     }
      96             : 
      97        4155 :     if (svr->type == svrAirborne)
      98             :     {
      99        4155 :         if (fields[1] & SV_PARAM_GEOALT)
     100             :         {
     101        4155 :             if (svr->validity.geoAlt)
     102             :             {
     103        4155 :                 svr->airborne.geoAlt = toAlt(&buffer[PBASE + ofs]);
     104             :             }
     105             :             else
     106             :             {
     107           0 :                 svr->airborne.geoAlt = 0;
     108             :             }
     109             : 
     110        4155 :             ofs += 3;
     111             :         }
     112             : 
     113        4155 :         if (fields[1] & SV_PARAM_VEL)
     114             :         {
     115        4155 :             if (svr->validity.airSpeed)
     116             :             {
     117        4155 :                 int16_t nvel = toVel(&buffer[PBASE + ofs + 0]);
     118        4155 :                 int16_t evel = toVel(&buffer[PBASE + ofs + 2]);
     119             : 
     120        4155 :                 svr->airborne.heading = toHeading2((double)nvel, (double)evel);
     121        4155 :                 svr->airborne.speed = sqrt(nvel * nvel + evel * evel);
     122        4155 :                 svr->airborne.velEW = evel;
     123        4155 :                 svr->airborne.velNS = nvel;
     124             :             }
     125             :             else
     126             :             {
     127           0 :                 svr->airborne.heading = 0;
     128           0 :                 svr->airborne.speed = 0;
     129           0 :                 svr->airborne.velEW = 0;
     130           0 :                 svr->airborne.velNS = 0;
     131             :             }
     132             : 
     133        4155 :             ofs += 4;
     134             :         }
     135             : 
     136        4155 :         if (fields[1] & SV_PARAM_BAROALT)
     137             :         {
     138           0 :             if (svr->validity.baroAlt)
     139             :             {
     140           0 :                 svr->airborne.baroAlt = toAlt(&buffer[PBASE + ofs]);
     141             :             }
     142             :             else
     143             :             {
     144           0 :                 svr->airborne.baroAlt = 0;
     145             :             }
     146             : 
     147           0 :             ofs += 3;
     148             :         }
     149             : 
     150        4155 :         if (fields[1] & SV_PARAM_VRATE)
     151             :         {
     152        4155 :             if (svr->validity.baroVRate || svr->validity.geoVRate)
     153             :             {
     154        4155 :                 svr->airborne.vrate = toInt16(&buffer[PBASE + ofs]);
     155             :             }
     156             :             else
     157             :             {
     158           0 :                 svr->airborne.vrate = 0;
     159             :             }
     160             : 
     161        4155 :             ofs += 2;
     162             :         }
     163             :     }
     164             :     else
     165             :     {
     166           0 :         if (fields[1] & SV_PARAM_SURF_GS)
     167             :         {
     168           0 :             if (svr->validity.surfSpeed)
     169             :             {
     170           0 :                 svr->surface.speed = toGS(&buffer[PBASE + ofs]);
     171             :             }
     172             :             else
     173             :             {
     174           0 :                 svr->surface.speed = 0;
     175             :             }
     176             : 
     177           0 :             ofs += 1;
     178             :         }
     179             : 
     180           0 :         if (fields[1] & SV_PARAM_SURF_HEAD)
     181             :         {
     182           0 :             if (svr->validity.surfHeading)
     183             :             {
     184           0 :                 svr->surface.heading = toHeading(&buffer[PBASE + ofs]);
     185             :             }
     186             :             else
     187             :             {
     188           0 :                 svr->surface.heading = 0;
     189             :             }
     190             : 
     191           0 :             ofs += 1;
     192             :         }
     193             :     }
     194             : 
     195        4155 :     if (fields[1] & SV_PARAM_NIC)
     196             :     {
     197           0 :         svr->nic = buffer[PBASE + ofs];
     198             : 
     199           0 :         ofs += 1;
     200             :     }
     201             : 
     202        4155 :     if (fields[1] & SV_PARAM_ESTLAT)
     203             :     {
     204           0 :         if (svr->evalidity.estPosition)
     205             :         {
     206           0 :             svr->airborne.estLat = toLatLon(&buffer[PBASE + ofs]);
     207             :         }
     208             :         else
     209             :         {
     210           0 :             svr->airborne.estLat = 0;
     211             :         }
     212             : 
     213           0 :         ofs += 3;
     214             :     }
     215             : 
     216        4155 :     if (fields[2] & SV_PARAM_ESTLON)
     217             :     {
     218           0 :         if (svr->evalidity.estPosition)
     219             :         {
     220           0 :             svr->airborne.estLon = toLatLon(&buffer[PBASE + ofs]);
     221             :         }
     222             :         else
     223             :         {
     224           0 :             svr->airborne.estLon = 0;
     225             :         }
     226             : 
     227           0 :         ofs += 3;
     228             :     }
     229             : 
     230        4155 :     if (fields[2] & SV_PARAM_SURV)
     231             :     {
     232           0 :         svr->survStatus = buffer[PBASE + ofs];
     233           0 :         ofs += 1;
     234             :     }
     235             : 
     236        4155 :     if (fields[2] & SV_PARAM_REPORT)
     237             :     {
     238           0 :         svr->mode = buffer[PBASE + ofs];
     239             :     }
     240             : 
     241        4155 :     return true;
     242             : }