Kim Hyeongmin / Mbed 2 deprecated mbed_spatial

sensor parsing program

Dependencies:   mbed

Fork of mbed_gps3 by Kim Hyeongmin

an_packet_protocol.c@0:0389ce4f9789, 2018-05-12 (annotated)

Committer:
FDLKHM
Date:
Sat May 12 07:28:33 2018 +0000
Revision:
0:0389ce4f9789
Child:
1:fbc9c604df21
This is my code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
FDLKHM 0:0389ce4f9789 1 /****************************************************************/
FDLKHM 0:0389ce4f9789 2 /* */
FDLKHM 0:0389ce4f9789 3 /* Advanced Navigation Packet Protocol Library */
FDLKHM 0:0389ce4f9789 4 /* C Language Dynamic Spatial SDK, Version 4.0 */
FDLKHM 0:0389ce4f9789 5 /* Copyright 2014, Xavier Orr, Advanced Navigation Pty Ltd */
FDLKHM 0:0389ce4f9789 6 /* */
FDLKHM 0:0389ce4f9789 7 /****************************************************************/
FDLKHM 0:0389ce4f9789 8 /*
FDLKHM 0:0389ce4f9789 9 * Copyright (C) 2014 Advanced Navigation Pty Ltd
FDLKHM 0:0389ce4f9789 10 *
FDLKHM 0:0389ce4f9789 11 * Permission is hereby granted, free of charge, to any person obtaining
FDLKHM 0:0389ce4f9789 12 * a copy of this software and associated documentation files (the "Software"),
FDLKHM 0:0389ce4f9789 13 * to deal in the Software without restriction, including without limitation
FDLKHM 0:0389ce4f9789 14 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
FDLKHM 0:0389ce4f9789 15 * and/or sell copies of the Software, and to permit persons to whom the
FDLKHM 0:0389ce4f9789 16 * Software is furnished to do so, subject to the following conditions:
FDLKHM 0:0389ce4f9789 17 *
FDLKHM 0:0389ce4f9789 18 * The above copyright notice and this permission notice shall be included
FDLKHM 0:0389ce4f9789 19 * in all copies or substantial portions of the Software.
FDLKHM 0:0389ce4f9789 20 *
FDLKHM 0:0389ce4f9789 21 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
FDLKHM 0:0389ce4f9789 22 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FDLKHM 0:0389ce4f9789 23 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
FDLKHM 0:0389ce4f9789 24 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
FDLKHM 0:0389ce4f9789 25 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FDLKHM 0:0389ce4f9789 26 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
FDLKHM 0:0389ce4f9789 27 * DEALINGS IN THE SOFTWARE.
FDLKHM 0:0389ce4f9789 28 */
FDLKHM 0:0389ce4f9789 29
FDLKHM 0:0389ce4f9789 30 #include <stdlib.h>
FDLKHM 0:0389ce4f9789 31 #include <stdint.h>
FDLKHM 0:0389ce4f9789 32 #include <string.h>
FDLKHM 0:0389ce4f9789 33 #include "an_packet_protocol.h"
FDLKHM 0:0389ce4f9789 34
FDLKHM 0:0389ce4f9789 35 /*
FDLKHM 0:0389ce4f9789 36 * Function to calculate the CRC16 of data
FDLKHM 0:0389ce4f9789 37 * CRC16-CCITT
FDLKHM 0:0389ce4f9789 38 * Initial value = 0xFFFF
FDLKHM 0:0389ce4f9789 39 * Polynomial = x^16 + x^12 + x^5 + x^0
FDLKHM 0:0389ce4f9789 40 */
FDLKHM 0:0389ce4f9789 41 uint16_t calculate_crc16(const void *data, uint16_t length)
FDLKHM 0:0389ce4f9789 42 {
FDLKHM 0:0389ce4f9789 43 uint8_t *bytes = (uint8_t *)data;
FDLKHM 0:0389ce4f9789 44 uint16_t crc = 0xFFFF, i;
FDLKHM 0:0389ce4f9789 45 for (i = 0; i < length; i++)
FDLKHM 0:0389ce4f9789 46 {
FDLKHM 0:0389ce4f9789 47 crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]);
FDLKHM 0:0389ce4f9789 48 }
FDLKHM 0:0389ce4f9789 49 return crc;
FDLKHM 0:0389ce4f9789 50 }
FDLKHM 0:0389ce4f9789 51
FDLKHM 0:0389ce4f9789 52 /*
FDLKHM 0:0389ce4f9789 53 * Function to calculate a 4 byte LRC
FDLKHM 0:0389ce4f9789 54 */
FDLKHM 0:0389ce4f9789 55 uint8_t calculate_header_lrc(uint8_t *data)
FDLKHM 0:0389ce4f9789 56 {
FDLKHM 0:0389ce4f9789 57 return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1;
FDLKHM 0:0389ce4f9789 58 }
FDLKHM 0:0389ce4f9789 59
FDLKHM 0:0389ce4f9789 60 /*
FDLKHM 0:0389ce4f9789 61 * Function to dynamically allocate an an_packet
FDLKHM 0:0389ce4f9789 62 */
FDLKHM 0:0389ce4f9789 63 an_packet_t *an_packet_allocate(uint8_t length, uint8_t id)
FDLKHM 0:0389ce4f9789 64 {
FDLKHM 0:0389ce4f9789 65 an_packet_t* an_packet = (an_packet_t*)malloc(sizeof(an_packet_t)+length * sizeof(uint8_t));
FDLKHM 0:0389ce4f9789 66 if (an_packet != NULL)
FDLKHM 0:0389ce4f9789 67 {
FDLKHM 0:0389ce4f9789 68 an_packet->id = id;
FDLKHM 0:0389ce4f9789 69 an_packet->length = length;
FDLKHM 0:0389ce4f9789 70 }
FDLKHM 0:0389ce4f9789 71 return an_packet;
FDLKHM 0:0389ce4f9789 72 }
FDLKHM 0:0389ce4f9789 73
FDLKHM 0:0389ce4f9789 74 /*
FDLKHM 0:0389ce4f9789 75 * Function to free an an_packet
FDLKHM 0:0389ce4f9789 76 */
FDLKHM 0:0389ce4f9789 77 void an_packet_free(an_packet_t **an_packet)
FDLKHM 0:0389ce4f9789 78 {
FDLKHM 0:0389ce4f9789 79 free(*an_packet);
FDLKHM 0:0389ce4f9789 80 *an_packet = NULL;
FDLKHM 0:0389ce4f9789 81 }
FDLKHM 0:0389ce4f9789 82
FDLKHM 0:0389ce4f9789 83 /*
FDLKHM 0:0389ce4f9789 84 * Initialise the decoder
FDLKHM 0:0389ce4f9789 85 */
FDLKHM 0:0389ce4f9789 86 void an_decoder_initialise(an_decoder_t *an_decoder)
FDLKHM 0:0389ce4f9789 87 {
FDLKHM 0:0389ce4f9789 88 an_decoder->buffer_length = 0;
FDLKHM 0:0389ce4f9789 89 an_decoder->crc_errors = 0;
FDLKHM 0:0389ce4f9789 90 }
FDLKHM 0:0389ce4f9789 91
FDLKHM 0:0389ce4f9789 92 /*
FDLKHM 0:0389ce4f9789 93 * Function to decode an_packets from raw data
FDLKHM 0:0389ce4f9789 94 * Returns a pointer to the packet decoded or NULL if no packet was decoded
FDLKHM 0:0389ce4f9789 95 */
FDLKHM 0:0389ce4f9789 96 an_packet_t *an_packet_decode(an_decoder_t *an_decoder)
FDLKHM 0:0389ce4f9789 97 {
FDLKHM 0:0389ce4f9789 98 uint16_t decode_iterator = 0;
FDLKHM 0:0389ce4f9789 99 an_packet_t *an_packet = NULL;
FDLKHM 0:0389ce4f9789 100 uint8_t header_lrc, id, length;
FDLKHM 0:0389ce4f9789 101 uint16_t crc;
FDLKHM 0:0389ce4f9789 102
FDLKHM 0:0389ce4f9789 103 while (decode_iterator + AN_PACKET_HEADER_SIZE <= an_decoder->buffer_length)
FDLKHM 0:0389ce4f9789 104 {
FDLKHM 0:0389ce4f9789 105 header_lrc = an_decoder->buffer[decode_iterator++];
FDLKHM 0:0389ce4f9789 106 if (header_lrc == calculate_header_lrc(&an_decoder->buffer[decode_iterator]))
FDLKHM 0:0389ce4f9789 107 {
FDLKHM 0:0389ce4f9789 108 id = an_decoder->buffer[decode_iterator++];
FDLKHM 0:0389ce4f9789 109 length = an_decoder->buffer[decode_iterator++];
FDLKHM 0:0389ce4f9789 110 crc = an_decoder->buffer[decode_iterator++];
FDLKHM 0:0389ce4f9789 111 crc |= an_decoder->buffer[decode_iterator++] << 8;
FDLKHM 0:0389ce4f9789 112
FDLKHM 0:0389ce4f9789 113 if (decode_iterator + length > an_decoder->buffer_length)
FDLKHM 0:0389ce4f9789 114 {
FDLKHM 0:0389ce4f9789 115 decode_iterator -= AN_PACKET_HEADER_SIZE;
FDLKHM 0:0389ce4f9789 116 break;
FDLKHM 0:0389ce4f9789 117 }
FDLKHM 0:0389ce4f9789 118
FDLKHM 0:0389ce4f9789 119 if (crc == calculate_crc16(&an_decoder->buffer[decode_iterator], length))
FDLKHM 0:0389ce4f9789 120 {
FDLKHM 0:0389ce4f9789 121 an_packet = an_packet_allocate(length, id);
FDLKHM 0:0389ce4f9789 122 if (an_packet != NULL)
FDLKHM 0:0389ce4f9789 123 {
FDLKHM 0:0389ce4f9789 124 memcpy(an_packet->header, &an_decoder->buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE * sizeof(uint8_t));
FDLKHM 0:0389ce4f9789 125 memcpy(an_packet->data, &an_decoder->buffer[decode_iterator], length * sizeof(uint8_t));
FDLKHM 0:0389ce4f9789 126 }
FDLKHM 0:0389ce4f9789 127 decode_iterator += length;
FDLKHM 0:0389ce4f9789 128 break;
FDLKHM 0:0389ce4f9789 129 }
FDLKHM 0:0389ce4f9789 130 else
FDLKHM 0:0389ce4f9789 131 {
FDLKHM 0:0389ce4f9789 132 decode_iterator -= (AN_PACKET_HEADER_SIZE - 1);
FDLKHM 0:0389ce4f9789 133 an_decoder->crc_errors++;
FDLKHM 0:0389ce4f9789 134 }
FDLKHM 0:0389ce4f9789 135 }
FDLKHM 0:0389ce4f9789 136 }
FDLKHM 0:0389ce4f9789 137 if (decode_iterator < an_decoder->buffer_length)
FDLKHM 0:0389ce4f9789 138 {
FDLKHM 0:0389ce4f9789 139 if (decode_iterator > 0)
FDLKHM 0:0389ce4f9789 140 {
FDLKHM 0:0389ce4f9789 141 memmove(&an_decoder->buffer[0], &an_decoder->buffer[decode_iterator], (an_decoder->buffer_length - decode_iterator) * sizeof(uint8_t));
FDLKHM 0:0389ce4f9789 142 an_decoder->buffer_length -= decode_iterator;
FDLKHM 0:0389ce4f9789 143 }
FDLKHM 0:0389ce4f9789 144 }
FDLKHM 0:0389ce4f9789 145 else an_decoder->buffer_length = 0;
FDLKHM 0:0389ce4f9789 146
FDLKHM 0:0389ce4f9789 147 return an_packet;
FDLKHM 0:0389ce4f9789 148 }
FDLKHM 0:0389ce4f9789 149
FDLKHM 0:0389ce4f9789 150 /*
FDLKHM 0:0389ce4f9789 151 * Function to encode an an_packet
FDLKHM 0:0389ce4f9789 152 */
FDLKHM 0:0389ce4f9789 153 void an_packet_encode(an_packet_t *an_packet)
FDLKHM 0:0389ce4f9789 154 {
FDLKHM 0:0389ce4f9789 155 uint16_t crc;
FDLKHM 0:0389ce4f9789 156 an_packet->header[1] = an_packet->id;
FDLKHM 0:0389ce4f9789 157 an_packet->header[2] = an_packet->length;
FDLKHM 0:0389ce4f9789 158 crc = calculate_crc16(an_packet->data, an_packet->length);
FDLKHM 0:0389ce4f9789 159 memcpy(&an_packet->header[3], &crc, sizeof(uint16_t));
FDLKHM 0:0389ce4f9789 160 an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]);
FDLKHM 0:0389ce4f9789 161 }
FDLKHM 0:0389ce4f9789 162