an_packet_protocol.c
Go to the documentation of this file.
00001 /****************************************************************/
00002 /*                                                              */
00003 /*         Advanced Navigation Packet Protocol Library          */
00004 /*         C Language Dynamic Orientus SDK, Version 1.1         */
00005 /*   Copyright 2013, Xavier Orr, Advanced Navigation Pty Ltd    */
00006 /*                                                              */
00007 /****************************************************************/
00008 /*
00009  * Copyright (C) 2013 Advanced Navigation Pty Ltd
00010  *
00011  * Permission is hereby granted, free of charge, to any person obtaining
00012  * a copy of this software and associated documentation files (the "Software"),
00013  * to deal in the Software without restriction, including without limitation
00014  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00015  * and/or sell copies of the Software, and to permit persons to whom the
00016  * Software is furnished to do so, subject to the following conditions:
00017  *
00018  * The above copyright notice and this permission notice shall be included
00019  * in all copies or substantial portions of the Software.
00020  *
00021  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00022  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00023  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
00024  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00025  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00026  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
00027  * DEALINGS IN THE SOFTWARE.
00028  */
00029 
00030 #include <stdlib.h>
00031 #include <stdint.h>
00032 #include <string.h>
00033 #include "an_packet_protocol.h"
00034 
00035 /*
00036  * Function to calculate the CRC16 of data
00037  * CRC16-CCITT
00038  * Initial value = 0xFFFF
00039  * Polynomial = x^16 + x^12 + x^5 + x^0
00040  */
00041 uint16_t calculate_crc16(const void *data, uint16_t length)
00042 {
00043         uint8_t *bytes = (uint8_t *) data;
00044         uint16_t crc = 0xFFFF, i;
00045         for (i = 0; i < length; i++)
00046         {
00047                 crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]);
00048         }
00049         return crc;
00050 }
00051 
00052 /*
00053  * Function to calculate a 4 byte LRC
00054  */
00055 uint8_t calculate_header_lrc(uint8_t *data)
00056 {
00057         return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1;
00058 }
00059 
00060 /*
00061  * Function to dynamically allocate an an_packet
00062  */
00063 an_packet_t *an_packet_allocate(uint8_t length, uint8_t id)
00064 {
00065         an_packet_t *an_packet = malloc(sizeof(an_packet_t) + length * sizeof(uint8_t));
00066         if (an_packet != NULL)
00067         {
00068                 an_packet->id = id;
00069                 an_packet->length = length;
00070         }
00071         return an_packet;
00072 }
00073 
00074 /*
00075  * Function to free an an_packet
00076  */
00077 void an_packet_free(an_packet_t **an_packet)
00078 {
00079         free(*an_packet);
00080         *an_packet = NULL;
00081 }
00082 
00083 /*
00084  * Initialise the decoder
00085  */
00086 void an_decoder_initialise(an_decoder_t *an_decoder)
00087 {
00088         an_decoder->buffer_length = 0;
00089         an_decoder->crc_errors = 0;
00090 }
00091 
00092 /*
00093  * Function to decode an_packets from raw data
00094  * Returns a pointer to the packet decoded or NULL if no packet was decoded
00095  */
00096 an_packet_t *an_packet_decode(an_decoder_t *an_decoder)
00097 {
00098         uint16_t decode_iterator = 0;
00099         an_packet_t *an_packet = NULL;
00100         uint8_t header_lrc, id, length;
00101         uint16_t crc;
00102 
00103         while (decode_iterator + AN_PACKET_HEADER_SIZE <= an_decoder->buffer_length)
00104         {
00105                 header_lrc = an_decoder->buffer[decode_iterator++];
00106                 if (header_lrc == calculate_header_lrc(&an_decoder->buffer[decode_iterator]))
00107                 {
00108                         id = an_decoder->buffer[decode_iterator++];
00109                         length = an_decoder->buffer[decode_iterator++];
00110                         crc = an_decoder->buffer[decode_iterator++];
00111                         crc |= an_decoder->buffer[decode_iterator++] << 8;
00112 
00113                         if (decode_iterator + length > an_decoder->buffer_length)
00114                         {
00115                                 decode_iterator -= AN_PACKET_HEADER_SIZE;
00116                                 break;
00117                         }
00118 
00119                         if (crc == calculate_crc16(&an_decoder->buffer[decode_iterator], length))
00120                         {
00121                                 an_packet = an_packet_allocate(length, id);
00122                                 if (an_packet != NULL)
00123                                 {
00124                                         memcpy(an_packet->header, &an_decoder->buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE * sizeof(uint8_t));
00125                                         memcpy(an_packet->data, &an_decoder->buffer[decode_iterator], length * sizeof(uint8_t));
00126                                 }
00127                                 decode_iterator += length;
00128                                 break;
00129                         }
00130                         else
00131                         {
00132                                 decode_iterator -= (AN_PACKET_HEADER_SIZE - 1);
00133                                 an_decoder->crc_errors++;
00134                         }
00135                 }
00136         }
00137         if (decode_iterator < an_decoder->buffer_length)
00138         {
00139                 if (decode_iterator > 0)
00140                 {
00141                         memmove(&an_decoder->buffer[0], &an_decoder->buffer[decode_iterator], (an_decoder->buffer_length - decode_iterator) * sizeof(uint8_t));
00142                         an_decoder->buffer_length -= decode_iterator;
00143                 }
00144         }
00145         else an_decoder->buffer_length = 0;
00146 
00147         return an_packet;
00148 }
00149 
00150 /*
00151  * Function to encode an an_packet
00152  */
00153 void an_packet_encode(an_packet_t *an_packet)
00154 {
00155         uint16_t crc;
00156         an_packet->header[1] = an_packet->id;
00157         an_packet->header[2] = an_packet->length;
00158         crc = calculate_crc16(an_packet->data, an_packet->length);
00159         memcpy(&an_packet->header[3], &crc, sizeof(uint16_t));
00160         an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]);
00161 }
00162 


orientus_sdk_c
Author(s): Advanced Navigation, Nick Otero
autogenerated on Wed Aug 26 2015 15:12:17