STM32L010でMAVLinkのCRCで遊ぶ

x　動作確認済み2022/5/27 6:17

目的
MAVLinkは、今時流行りのドローンの高度や位置のログを送る
プロトコルらしい
持ってないけど
MAVLinkのパケット構造は、簡単だけど
CRCの計算がいまだかって理解できないがソースがあるので適当にぱくって
都合よくマイコンに実装する
なんか、送信、受信でCRC_EXTRAと称する
固定化された暗号キーみたいなが必要になるらしい
ラジコンのマニュアルに00とか124とか書いてある物が必要らしいが
適当に124とする。
読める、読めるぞ(ラピタふう)

dfを除いたヘッダー部分が43558になれば当たりらしい
初期値値は、X25_INIT_CRC 0xffff

32 00 00 ca 01 01 7a e6 00

0x0e,0x04,0x19... が　63626になっていれば、正しいらしい

[EF]13395
[D7]35224

たぶんEFが間違いでD7が正しい?

名前は、まだない

//MAVLink_CRC_010_1


#include "mbed.h"


RawSerial pc(PA_2, PA_3); //010


char c_hex[] = {'0', '1', '2', '3', '4', '5', '6', '7',
                '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };

void p_hex(unsigned char x){

  //ヘキサに値を設定
  // packet move
  char out_buff[8];
  out_buff[0] = '[';
  out_buff[1] = c_hex[(x >> 4) & 0x0f];
  out_buff[2] = c_hex[x & 0x0f];
  out_buff[3] = ']';
  out_buff[4] = 0x00;

  //送信処理
  //send 8 byte
  pc.printf(out_buff);

    
}//printf_hex


void crc_accumulate(unsigned char data, unsigned short *crcAccum)
{
    
    
    p_hex(data);
    
    
    unsigned char tmp;

    tmp = data ^ (unsigned char)(*crcAccum &0xff);
    tmp ^= (tmp<<4);
    *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);

}//crc_accumulate


unsigned short crc_calculate(unsigned char *pBuffer, int length)
{
    unsigned short crcTmp = 0xffff; //X25_INIT_CRC

    for(int ii=0;ii<length;ii++){
        crc_accumulate(pBuffer[ii] , &crcTmp);
    }
    return crcTmp;
}//crc_calculate


void crc_accumulate_buffer(unsigned short *crcAccum, char *pBuffer, int length)
{
  unsigned char *p = (unsigned char*)pBuffer;

  for(int ii=0;ii<length;ii++){
                crc_accumulate(p[ii], crcAccum);
        }
}//crc_accumulate_buffer


//メイン関数
int main()
{

  pc.printf("<ST>\r\n");

    //無限ループ
    while(1) {


    unsigned char buff1[] = { 0x32,0x00,0x00,0xca,0x01,0x01,0x7a,0xe6,0x00 };
    
             char buff2[] = {                     0x0e,0x04,0x19,0x0f,0x0f,0x0f,
0x0f,0x0f,0x0f,0x0f,0x0f,0x0c,0x00,0x00,0x0f,0xff,0xff,0xff,0xff,0xff,0x0f,0xff,
0xff,0xff,0xff,0xff,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0x00,0x00,0x00,0x00,0x0f,0x0f };
    
    
    pc.printf("START\r\n");
    
    
    unsigned short ret = crc_calculate(buff1, 9);
    
    pc.printf("\r\n");
    
    pc.printf("%d",ret);pc.printf("\r\n");

    
    unsigned short checksum = 43558;
    
    crc_accumulate_buffer(&checksum, buff2, 50);
    
    pc.printf("\r\n");
    
    pc.printf("%d",checksum);pc.printf("\r\n");
    
    /*
    unsigned short chk=0;
    for(int ii=0;ii<256;ii++){
        
        chk = 63626;
        crc_accumulate((unsigned char)ii,&chk);
        printf("%d\r\n",chk);
    
    }//for
    */
    
    
    unsigned short chk=0;
    chk = 63626;
    crc_accumulate((unsigned char)0xEF,&chk);
    pc.printf("%d",chk);pc.printf("\r\n");
 
    
    chk = 63626;
    crc_accumulate((unsigned char)0xD7,&chk);
    pc.printf("%d",chk);pc.printf("\r\n");
 

  //2秒の待ち
  wait_ms(2000);    //debug

} //while

}//main