diff --git a/sw/airborne/modules/datalink/mavlink_decoder.c b/sw/airborne/modules/datalink/mavlink_decoder.c index a75db2e9ae2..c0f21a1a58a 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.c +++ b/sw/airborne/modules/datalink/mavlink_decoder.c @@ -29,7 +29,7 @@ struct mavlink_transport mavlink_tp; #ifndef MAVLINK_NO_CRC_EXTRA -uint8_t crc_extra[256]={50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}; +uint8_t mavlink_crc_extra[256]={50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}; #endif #if MAVLINK_DECODER_DEBUG diff --git a/sw/airborne/modules/datalink/mavlink_decoder.h b/sw/airborne/modules/datalink/mavlink_decoder.h index a625ac0be2c..8f2622cafc6 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.h +++ b/sw/airborne/modules/datalink/mavlink_decoder.h @@ -65,7 +65,7 @@ struct mavlink_message { #ifndef MAVLINK_NO_CRC_EXTRA // CRC Extra (!!! staticaly calculated !!!) -extern uint8_t crc_extra[256]; +extern uint8_t mavlink_crc_extra[256]; #endif /** @@ -77,7 +77,7 @@ extern uint8_t crc_extra[256]; * @param data new char to hash * @param crcAccum the already accumulated checksum */ -static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +static inline void mavlink_crc_accumulate(uint8_t data, uint16_t *crcAccum) { /*Accumulate one byte of data into the CRC*/ uint8_t tmp; @@ -92,7 +92,7 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) * * @param crcAccum the 16 bit X.25 CRC */ -static inline void crc_init(uint16_t* crcAccum) +static inline void mavlink_crc_init(uint16_t* crcAccum) { *crcAccum = X25_INIT_CRC; } @@ -104,12 +104,12 @@ static inline void crc_init(uint16_t* crcAccum) * @param length length of the byte array * @return the checksum over the buffer bytes **/ -static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) +static inline uint16_t mavlink_crc_calculate(const uint8_t* pBuffer, uint16_t length) { uint16_t crcTmp; - crc_init(&crcTmp); + mavlink_crc_init(&crcTmp); while (length--) { - crc_accumulate(*pBuffer++, &crcTmp); + mavlink_crc_accumulate(*pBuffer++, &crcTmp); } return crcTmp; } @@ -175,7 +175,7 @@ static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { case MAVLINK_PARSE_STATE_IDLE: if (c == STXMAV) { t->status = MAVLINK_PARSE_STATE_GOT_STX; - crc_init(&(t->checksum)); + mavlink_crc_init(&(t->checksum)); } break; case MAVLINK_PARSE_STATE_GOT_STX: @@ -184,13 +184,13 @@ static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { goto error; } t->trans.payload_len = c + MAVLINK_PAYLOAD_OFFSET; /* Not Counting STX, CRC1 and CRC2, adding LENGTH, SEQ, SYSID, COMPID, MSGID */ - crc_accumulate(c, &(t->checksum)); + mavlink_crc_accumulate(c, &(t->checksum)); t->status = MAVLINK_PARSE_STATE_GOT_LENGTH; t->payload_idx = 0; break; case MAVLINK_PARSE_STATE_GOT_LENGTH: t->trans.payload[t->payload_idx] = c; - crc_accumulate(c, &(t->checksum)); + mavlink_crc_accumulate(c, &(t->checksum)); t->payload_idx++; if (t->payload_idx == t->trans.payload_len) t->status = MAVLINK_PARSE_STATE_GOT_PAYLOAD; @@ -201,7 +201,7 @@ static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { #endif #ifndef MAVLINK_NO_CRC_EXTRA // add extra CRC - crc_accumulate(crc_extra[(t->trans.payload[MAVLINK_MSG_ID_IDX])], &(t->checksum)); + mavlink_crc_accumulate(mavlink_crc_extra[(t->trans.payload[MAVLINK_MSG_ID_IDX])], &(t->checksum)); #endif if (c != (t->checksum & 0xFF)) goto error;