Line data    Source code

       1             : /**
       2             :  * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
       3             :  *
       4             :  * @file   sgEncodeOperating.c
       5             :  * @author Jacob.Garrison
       6             :  *
       7             :  * @date   Feb 15, 2021
       8             :  *
       9             :  * This file receives a populated operating message struct and
      10             :  * converts it into a operating message buffer.
      11             :  */
      12             : 
      13             : #include <stdbool.h>
      14             : #include <stdlib.h>
      15             : 
      16             : #include "sg.h"
      17             : #include "sgUtil.h"
      18             : 
      19             : #define SG_PAYLOAD_LEN_OPMSG SG_MSG_LEN_OPMSG - 5 /// the payload length.
      20             : 
      21             : #define PBASE 4            /// the payload offset.
      22             : #define OFFSET_SQUAWK 0    /// the squawk code offset in the payload.
      23             : #define OFFSET_CONFIG 2    /// the mode/config offset in the payload.
      24             : #define OFFSET_EMRG_ID 3   /// the emergency flag offset in the payload.
      25             : #define OFFSET_ALT 4       /// the altitude offset in the payload.
      26             : #define OFFSET_RATE 6      /// the climb rate offset in the payload.
      27             : #define OFFSET_HEADING 8   /// the heading offset in the payload.
      28             : #define OFFSET_AIRSPEED 10 /// the airspeed offset in the payload.
      29             : 
      30             : /*
      31             :  * Documented in the header file.
      32             :  */
      33          11 : bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId)
      34             : {
      35             :     // populate header
      36          11 :     buffer[0] = SG_MSG_START_BYTE;
      37          11 :     buffer[1] = SG_MSG_TYPE_HOST_OPMSG;
      38          11 :     buffer[2] = msgId;
      39          11 :     buffer[3] = SG_PAYLOAD_LEN_OPMSG;
      40             : 
      41             :     // populate Squawk code
      42          11 :     uint162Buf(&buffer[PBASE + OFFSET_SQUAWK], op->squawk);
      43             : 
      44             :     // populate Mode/Config
      45          11 :     buffer[PBASE + OFFSET_CONFIG] = op->milEmergency << 5 |
      46          11 :                                     op->enableXBit << 4 |
      47          11 :                                     op->enableSqt << 3 |
      48          11 :                                     op->savePowerUp << 2 |
      49          11 :                                     op->opMode;
      50             : 
      51             :     // populate Emergency/Ident
      52          11 :     buffer[PBASE + OFFSET_EMRG_ID] = op->identOn << 3 |
      53          11 :                                      op->emergcType;
      54             : 
      55             :     // populate Altitude
      56          11 :     uint16_t altCode = 0;
      57          11 :     if (op->altUseIntrnl)
      58             :     {
      59          11 :         altCode = 0x8000;
      60             :     }
      61           0 :     else if (op->altHostAvlbl)
      62             :     {
      63             :         // 100 foot encoding conversion
      64           0 :         altCode = (op->altitude + 1200) / 100;
      65             : 
      66           0 :         if (op->altRes25)
      67             :         {
      68           0 :             altCode *= 4;
      69             :         }
      70             : 
      71             :         // 'Host altitude available' flag
      72           0 :         altCode += 0x4000;
      73             :     }
      74          11 :     uint162Buf(&buffer[PBASE + OFFSET_ALT], altCode);
      75             : 
      76             :     // populate Altitude Rate
      77          11 :     int16_t rate = op->climbRate / 64;
      78          11 :     if (!op->climbValid)
      79             :     {
      80           0 :         rate = 0x8000;
      81             :     }
      82          11 :     uint162Buf(&buffer[PBASE + OFFSET_RATE], rate);
      83             : 
      84             :     // populate Heading
      85             :     //    conversion: heading * ( pow(2, 15) / 360 )
      86          11 :     uint16_t heading = op->heading * 32768 / 360;
      87          11 :     if (op->headingValid)
      88             :     {
      89           1 :         heading += 0x8000;
      90             :     }
      91          11 :     uint162Buf(&buffer[PBASE + OFFSET_HEADING], heading);
      92             : 
      93             :     // populate Airspeed
      94          11 :     uint16_t airspeed = op->airspd;
      95          11 :     if (op->airspdValid)
      96             :     {
      97           1 :         airspeed += 0x8000;
      98             :     }
      99          11 :     uint162Buf(&buffer[PBASE + OFFSET_AIRSPEED], airspeed);
     100             : 
     101             :     // populate checksum
     102          11 :     appendChecksum(buffer, SG_MSG_LEN_OPMSG);
     103             : 
     104          11 :     return true;
     105             : }