mavlink_checksum.h 3.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
/** @file
 *	@brief MAVLink comm protocol checksum routines.
 *	@see http://qgroundcontrol.org/mavlink/
 *	Edited on Monday, August 8 2011
 */

#ifdef __cplusplus
extern "C" {
#endif

#ifndef _CHECKSUM_H_
#define _CHECKSUM_H_

#include "inttypes.h"


/**
 *
 *  CALCULATE THE CHECKSUM
 *
 */

#define X25_INIT_CRC 0xffff
#define X25_VALIDATE_CRC 0xf0b8

/**
 * @brief Accumulate the X.25 CRC by adding one char at a time.
 *
 * The checksum function adds the hash of one char at a time to the
 * 16 bit checksum (uint16_t).
 *
 * @param data new char to hash
 * @param crcAccum the already accumulated checksum
 **/
static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
{
        /*Accumulate one byte of data into the CRC*/
        uint8_t tmp;

        tmp=data ^ (uint8_t)(*crcAccum &0xff);
        tmp^= (tmp<<4);
        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
//        *crcAccum += data; // super simple to test
}

/**
 * @brief Initiliaze the buffer for the X.25 CRC
 *
 * @param crcAccum the 16 bit X.25 CRC
 */
static inline void crc_init(uint16_t* crcAccum)
{
        *crcAccum = X25_INIT_CRC;
}

/**
 * @brief Initiliaze the buffer for the X.25 CRC to a specified value
 *
 * @param crcAccum the 16 bit X.25 CRC
 */
static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue)
{
        *crcAccum = crcValue;
}



/**
 * @brief Calculates the X.25 checksum on a byte buffer
 *
 * @param  pBuffer buffer containing the byte array to hash
 * @param  length  length of the byte array
 * @return the checksum over the buffer bytes
 **/
static inline uint16_t crc_calculate(uint8_t* pBuffer, int length)
{

        // For a "message" of length bytes contained in the unsigned char array
        // pointed to by pBuffer, calculate the CRC
        // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed

        uint16_t crcTmp;
        //uint16_t tmp;
        uint8_t* pTmp;
		int i;

        pTmp=pBuffer;
        

        /* init crcTmp */
        crc_init(&crcTmp);

        for (i = 0; i < length; i++){
                crc_accumulate(*pTmp++, &crcTmp);
        }

        /* This is currently not needed, as only the checksum over payload should be computed
        tmp = crcTmp;
        crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp);
        crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp);
        *checkConst = tmp;
        */
        return(crcTmp);
}


/**
 * @brief Calculates the X.25 checksum on a msg buffer
 *
 * @param  pMSG buffer containing the msg to hash
 * @param  length  number of bytes to hash
 * @return the checksum over the buffer bytes
 **/
static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length)
{

        // For a "message" of length bytes contained in the unsigned char array
        // pointed to by pBuffer, calculate the CRC
        // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed

        uint16_t crcTmp;
        //uint16_t tmp;
        uint8_t* pTmp;
		int i;

        pTmp=&pMSG->len;
       
        /* init crcTmp */
        crc_init(&crcTmp);

        for (i = 0; i < 5; i++){
                crc_accumulate(*pTmp++, &crcTmp);
        }

        pTmp=&pMSG->payload[0];
        for (; i < length; i++){
                crc_accumulate(*pTmp++, &crcTmp);
        }

        /* This is currently not needed, as only the checksum over payload should be computed
        tmp = crcTmp;
        crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp);
        crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp);
        *checkConst = tmp;
        */
        return(crcTmp);
}




#endif /* _CHECKSUM_H_ */

#ifdef __cplusplus
}
#endif