Kim Hyeongmin
/
mbed_spatial
sensor parsing program
Fork of mbed_gps3 by
an_packet_protocol.c
- Committer:
- FDLKHM
- Date:
- 2018-05-16
- Revision:
- 1:fbc9c604df21
- Parent:
- 0:0389ce4f9789
File content as of revision 1:fbc9c604df21:
/****************************************************************/ /* */ /* Advanced Navigation Packet Protocol Library */ /* C Language Dynamic Spatial SDK, Version 4.0 */ /* Copyright 2014, Xavier Orr, Advanced Navigation Pty Ltd */ /* */ /****************************************************************/ /* * Copyright (C) 2014 Advanced Navigation Pty Ltd * * Permission is hereby granted, free of charge, to any person obtaining * a copy of this software and associated documentation files (the "Software"), * to deal in the Software without restriction, including without limitation * the rights to use, copy, modify, merge, publish, distribute, sublicense, * and/or sell copies of the Software, and to permit persons to whom the * Software is furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included * in all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER * DEALINGS IN THE SOFTWARE. */ #include <stdlib.h> #include <stdint.h> #include <string.h> #include "an_packet_protocol.h" /* * Function to calculate the CRC16 of data * CRC16-CCITT * Initial value = 0xFFFF * Polynomial = x^16 + x^12 + x^5 + x^0 */ uint16_t calculate_crc16(const void *data, uint16_t length) { uint8_t *bytes = (uint8_t *)data; uint16_t crc = 0xFFFF, i; for (i = 0; i < length; i++) { crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]); } return crc; } /* * Function to calculate a 4 byte LRC */ uint8_t calculate_header_lrc(uint8_t *data) { return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1; } /* * Function to dynamically allocate an an_packet */ an_packet_t *an_packet_allocate(uint8_t length, uint8_t id) { an_packet_t* an_packet = (an_packet_t*)malloc(sizeof(an_packet_t)+length * sizeof(uint8_t)); if (an_packet != NULL) { an_packet->id = id; an_packet->length = length; } return an_packet; } /* * Function to free an an_packet */ void an_packet_free(an_packet_t **an_packet) { free(*an_packet); *an_packet = NULL; } /* * Initialise the decoder */ void an_decoder_initialise(an_decoder_t *an_decoder) { an_decoder->buffer_length = 0; an_decoder->crc_errors = 0; } /* * Function to decode an_packets from raw data * Returns a pointer to the packet decoded or NULL if no packet was decoded */ an_packet_t *an_packet_decode(an_decoder_t *an_decoder) { uint16_t decode_iterator = 0; an_packet_t *an_packet = NULL; uint8_t header_lrc, id, length; uint16_t crc; while (decode_iterator + AN_PACKET_HEADER_SIZE <= an_decoder->buffer_length) { header_lrc = an_decoder->buffer[decode_iterator++]; if (header_lrc == calculate_header_lrc(&an_decoder->buffer[decode_iterator])) { id = an_decoder->buffer[decode_iterator++]; length = an_decoder->buffer[decode_iterator++]; crc = an_decoder->buffer[decode_iterator++]; crc |= an_decoder->buffer[decode_iterator++] << 8; if (decode_iterator + length > an_decoder->buffer_length) { decode_iterator -= AN_PACKET_HEADER_SIZE; break; } if (crc == calculate_crc16(&an_decoder->buffer[decode_iterator], length)) { an_packet = an_packet_allocate(length, id); if (an_packet != NULL) { memcpy(an_packet->header, &an_decoder->buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE * sizeof(uint8_t)); memcpy(an_packet->data, &an_decoder->buffer[decode_iterator], length * sizeof(uint8_t)); } decode_iterator += length; break; } else { decode_iterator -= (AN_PACKET_HEADER_SIZE - 1); an_decoder->crc_errors++; } } } if (decode_iterator < an_decoder->buffer_length) { if (decode_iterator > 0) { memmove(&an_decoder->buffer[0], &an_decoder->buffer[decode_iterator], (an_decoder->buffer_length - decode_iterator) * sizeof(uint8_t)); an_decoder->buffer_length -= decode_iterator; } } else an_decoder->buffer_length = 0; return an_packet; } /* * Function to encode an an_packet */ void an_packet_encode(an_packet_t *an_packet) { uint16_t crc; an_packet->header[1] = an_packet->id; an_packet->header[2] = an_packet->length; crc = calculate_crc16(an_packet->data, an_packet->length); memcpy(&an_packet->header[3], &crc, sizeof(uint16_t)); an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]); }