Line data    Source code

       1             : /**
       2             :  * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
       3             :  *
       4             :  * @file sgEncodeGPS.c
       5             :  * @author Jacob.Garrison
       6             :  *
       7             :  * @date Mar 1, 2021
       8             :  *
       9             :  * This file receives a populated GPS message struct and
      10             :  * converts it into a GPS 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_GPS SG_MSG_LEN_GPS - 5 /// the payload length.
      20             : #define _UNUSED(x) ((void)(x))
      21             : 
      22             : #define PBASE 4            /// the payload offset.
      23             : #define OFFSET_LONGITUDE 0 /// the longitude offset in the payload.
      24             : #define OFFSET_LATITUDE 11 /// the latitude offset in the payload.
      25             : #define OFFSET_SPEED 21    /// the ground speed offset in the payload.
      26             : #define OFFSET_TRACK 27    /// the ground track offset in the payload.
      27             : #define OFFSET_STATUS 35   /// the hemisphere/data status offset in the payload.
      28             : #define OFFSET_TIME 36     /// the time of fix offset in the payload.
      29             : #define OFFSET_HEIGHT 46   /// the GNSS height offset in the payload.
      30             : #define OFFSET_HPL 50      /// the horizontal protection limit offset in the payload.
      31             : #define OFFSET_HFOM 54     /// the horizontal figure of merit offset in the payload.
      32             : #define OFFSET_VFOM 58     /// the vertical figure of merit offset in the payload.
      33             : #define OFFSET_NACV 62     /// the navigation accuracy for velocity offset in the payload.
      34             : 
      35             : #define LEN_LNG 11  /// bytes in the longitude field
      36             : #define LEN_LAT 10  /// bytes in the latitude field
      37             : #define LEN_SPD 6   /// bytes in the speed over ground field
      38             : #define LEN_TRK 8   /// bytes in the ground track field
      39             : #define LEN_TIME 10 /// bytes in the time of fix field
      40             : 
      41             : /*
      42             :  * Documented in the header file.
      43             :  */
      44           9 : bool sgEncodeGPS(uint8_t *buffer, sg_gps_t *gps, uint8_t msgId)
      45             : {
      46             :     // populate header
      47           9 :     buffer[0] = SG_MSG_START_BYTE;
      48           9 :     buffer[1] = SG_MSG_TYPE_HOST_GPS;
      49           9 :     buffer[2] = msgId;
      50           9 :     buffer[3] = SG_PAYLOAD_LEN_GPS;
      51             : 
      52             :     // populate longitude
      53           9 :     charArray2Buf(&buffer[PBASE + OFFSET_LONGITUDE], gps->longitude, LEN_LNG);
      54             : 
      55             :     // populate latitude
      56           9 :     charArray2Buf(&buffer[PBASE + OFFSET_LATITUDE], gps->latitude, LEN_LAT);
      57             : 
      58             :     // populate ground speed
      59           9 :     charArray2Buf(&buffer[PBASE + OFFSET_SPEED], gps->grdSpeed, LEN_SPD);
      60             : 
      61             :     // populate ground track
      62           9 :     charArray2Buf(&buffer[PBASE + OFFSET_TRACK], gps->grdTrack, LEN_TRK);
      63             : 
      64             :     // populate hemisphere/data status
      65           9 :     buffer[PBASE + OFFSET_STATUS] = !gps->gpsValid << 7 |
      66           9 :                                     gps->fdeFail << 6 |
      67           9 :                                     gps->lngEast << 1 |
      68           9 :                                     gps->latNorth;
      69             : 
      70             :     // populate time of fix
      71           9 :     charArray2Buf(&buffer[PBASE + OFFSET_TIME], gps->timeOfFix, LEN_TIME);
      72             : 
      73             :     // populate gnss height
      74           9 :     float2Buf(&buffer[PBASE + OFFSET_HEIGHT], gps->height);
      75             : 
      76             :     // populate HPL
      77           9 :     float2Buf(&buffer[PBASE + OFFSET_HPL], gps->hpl);
      78             : 
      79             :     // populate HFOM
      80           9 :     float2Buf(&buffer[PBASE + OFFSET_HFOM], gps->hfom);
      81             : 
      82             :     // populate VFOM
      83           9 :     float2Buf(&buffer[PBASE + OFFSET_VFOM], gps->vfom);
      84             : 
      85             :     // populate NACv
      86           9 :     buffer[PBASE + OFFSET_NACV] = gps->nacv << 4;
      87             : 
      88             :     // populate checksum
      89           9 :     appendChecksum(buffer, SG_MSG_LEN_GPS);
      90             : 
      91           9 :     return true;
      92             : }