flight/libraries/ubx_utils.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       ubx_utils.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      UBX Protocol utilities.
   7  *             --
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26 #include <ubx_utils.h>
  27 bool ubx_getLastSentence(uint8_t *data, uint16_t bufferCount, uint8_t * *lastSentence, uint16_t *length)
  28 {
  29     const uint8_t packet_overhead = UBX_HEADER_LEN + 2;
  30     uint8_t *current = data + bufferCount - packet_overhead;
  31
  32     while (current >= data) {
  33         // look for a ubx a sentence
  34         if (current[0] == UBX_SYN1 && current[1] == UBX_SYN2) {
  35             // check whether it fits the current buffer (whole sentence is into buffer)
  36             uint16_t len = current[4] + (current[5] << 8);
  37             if (len + packet_overhead + current <= data + bufferCount) {
  38                 *lastSentence = current;
  39                 *length = len + packet_overhead;
  40                 return true;
  41             }
  42         }
  43         current--;
  44     }
  45     // no complete sentence found
  46     return false;
  47 }
  48
  49 void ubx_buildPacket(UBXPacket_t *pkt, uint8_t packetClass, uint8_t packetId, uint16_t len)
  50 {
  51     pkt->packet.header.syn1 = UBX_SYN1;
  52     pkt->packet.header.syn2 = UBX_SYN2;
  53
  54     // don't make any assumption on alignments...
  55     ((uint8_t *)&pkt->packet.header.len)[0] = len & 0xFF;
  56     ((uint8_t *)&pkt->packet.header.len)[1] = (len >> 8) & 0xFF;
  57
  58     pkt->packet.header.class = packetClass;
  59     pkt->packet.header.id    = packetId;
  60     ubx_appendChecksum(pkt);
  61 }
  62
  63 void ubx_appendChecksum(UBXPacket_t *pkt)
  64 {
  65     uint8_t chkA = 0;
  66     uint8_t chkB = 0;
  67     uint16_t len = ((uint8_t *)&pkt->packet.header.len)[0] | ((uint8_t *)&pkt->packet.header.len)[1] << 8;
  68
  69     // From class field to the end of payload
  70     for (uint8_t i = 2; i < len + UBX_HEADER_LEN; i++) {
  71         chkA += pkt->binarystream[i];
  72         chkB += chkA;
  73     }
  74     ;
  75     pkt->packet.payload[len]     = chkA;
  76     pkt->packet.payload[len + 1] = chkB;
  77 }