123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241 |
- #include "drv_IAP.h"
- #include "usbd_cdc_if.h"
- //#include "CRC.h"
- #include "CRC16.h"
- #include "usb_device.h"
- #include "usbd_cdc.h"
- static void short_to_buf(unsigned char *buf, short *tshort){
- int i;
- unsigned char *p1 = (unsigned char *)tshort;
- unsigned char *p2 = buf;
- for(i = 0;i < 2;i++){
- *p2 = *p1;
- p1++;
- p2++;
- }
- }
- static void buf_to_short(short *tshort, unsigned char *buf){
- int i;
- unsigned char *p1 = (unsigned char *)tshort;
- unsigned char *p2 = buf;
- for(i = 0;i < 2;i++){
- *p1 = *p2;
- p1++;
- p2++;
- }
- }
- static void buf_to_int(int *tint, unsigned char *buf){
- int i;
- unsigned char *p1 = (unsigned char *)tint;
- unsigned char *p2 = buf;
- for(i = 0;i < 4;i++){
- *p1 = *p2;
- p1++;
- p2++;
- }
- }
- unsigned char send_what = 0;
- uint8_t ack_id = 0;
- short id_cont = 0;
- bool IAP_STATUS_FLAG = true;
- uint32_t usb_rx_index = 0;
- uint8_t usb_rx_buf[IAP_PACK_SIZE + 2] = {0};
- uint8_t usb_tx_buf[IAP_PACK_SIZE] = {0};
- uint32_t usb_tx_index = 0;
- bool recv_aid_flag = false,
- recv_init_flag = false,
- recv_erasure_flag = false,
- recv_finish_flag = false,
- recv_onepack_flag = false;
- IAP_STATUS recv_iap_flag = DEFAULT;
- int iap_bin_Size = 0;
- short iap_bin_num = 0, pre_iap_bin_num = 0;
- unsigned char one_pack_size = 0;
- bool pack_send_flag = true;
- mavlink_system_t mavlink_system = {
- .stx = 254,
- .headbytes = 6,
- .checkbytes = 2,
- .sysid = 0,
- .compid = 0,
- .tx_seq = 0,
- .rx_seq = 0
- };
- mavlink_msg_t mavlink_msg = {
- .stx = 254,
- .sysid = 0,
- .compid = 5
- };
- bool receive_mavlink_msg = false;
- //除最后一包,前面固件包固定大小 0xFA
- const uint8_t pack_size = 0xFA;
- extern uint32_t rec_err , crc_err;
- //发送MAVLINK_ACK
- void send_mavlink_ack(void){
- int index = 0;
- uint16_t checksum = 0;
- usb_tx_buf[index++] = mavlink_system.stx;
- usb_tx_buf[index++] = mavlink_system.stx;
- usb_tx_buf[index++] = mavlink_system.tx_seq;
- usb_tx_buf[index++] = mavlink_system.sysid;
- usb_tx_buf[index++] = mavlink_system.compid;
- usb_tx_buf[index++] = MAVLINK_ACK_ID;
- usb_tx_buf[index++] = ack_id;
- short_to_buf(&usb_tx_buf[index], &id_cont);
- index += 2;
- // 计算有效载荷长度
- usb_tx_buf[1] = index - 6;
-
- checksum = Get_Crc16(usb_tx_buf,(uint16_t) index);
- usb_tx_buf[index++] = checksum & 0xff;
- usb_tx_buf[index++] = (checksum >> 8) & 0xff;
- CDC_Transmit_FS(usb_tx_buf, index);
- }
- uint16_t offset;
- //上报MAVLINK_BOOTLOADER_STATUS
- void send_mavlink_bootloader_status(uint8_t bootloader_status){
- uint16_t index = 0;
- offset = 0;
- uint16_t checksum = 0;
- memset(usb_tx_buf,0,sizeof(usb_tx_buf));
- usb_tx_buf[offset++] = mavlink_system.stx;
-
- usb_tx_buf[offset++] = mavlink_system.stx;
- usb_tx_buf[offset++] = mavlink_system.tx_seq;
- usb_tx_buf[offset++] = mavlink_system.sysid;
- usb_tx_buf[offset++] = mavlink_system.compid;
- usb_tx_buf[offset++] = MAVLINK_BOOTLOADER_STATUS;
-
- usb_tx_buf[offset++] = bootloader_status;
- usb_tx_buf[1] = offset - 6;
- checksum = Get_Crc16(usb_tx_buf,(uint16_t) offset);
- usb_tx_buf[offset++] = checksum & 0xff;
- usb_tx_buf[offset++] = (checksum >> 8) & 0xff;
-
-
- CDC_Transmit_FS(usb_tx_buf, offset);
- }
- uint8_t first_rxdata = 0, second_rxdata = 0, third_rxdata = 0, fourth_rxdata = 0, fifth_rxdata = 0, sixth_rxdata = 0;
- void recv_iap_station_data_hookfunction(unsigned char rec_data){
- sixth_rxdata = fifth_rxdata;
- fifth_rxdata = fourth_rxdata;
- fourth_rxdata = third_rxdata;
- third_rxdata = second_rxdata;
- second_rxdata = first_rxdata;
- first_rxdata = rec_data;
- //如果接收到协议数据包
- if (receive_mavlink_msg == true){
- usb_rx_buf[usb_rx_index++] = rec_data;
- //如果数据包超出协议规定的最大长度
- if(mavlink_msg.payload > IAP_PACK_SIZE - mavlink_system.headbytes
- - mavlink_system.checkbytes){
- receive_mavlink_msg = false;
- usb_rx_index = 0;
- }
- //如果到达数据包尾
- if(usb_rx_index >= mavlink_msg.payload + mavlink_system.headbytes +
- mavlink_system.checkbytes){
- receive_mavlink_msg = false;
- usb_rx_index = 0;
-
- if(Get_Crc16(usb_rx_buf, mavlink_msg.payload + mavlink_system.headbytes
- + mavlink_system.checkbytes) == 0x0000){
- switch(mavlink_msg.msgid){
- case 200: // 固件信息初始化包
- buf_to_int(&iap_bin_Size, &usb_rx_buf[6]);
- recv_iap_flag = INIT; // 进入初始化状态
- recv_init_flag = true;
- break;
- case 201: // 固件数据传输包
- if(recv_iap_flag == TRANSMIT){ //如果是传输状态
- if(recv_onepack_flag != true){
- //USBD_Stop(&hUsbDeviceFS); // 停止USB接收
- //USBD_CDC_Stop(&hUsbDeviceFS); // 停止USB接收
- pack_send_flag = false; // 停止发送
- recv_onepack_flag = true; // 设置包接收标志
- buf_to_short(&iap_bin_num, &usb_rx_buf[6]);
- one_pack_size = mavlink_msg.payload - 2;
- //保护usb_rx_buf数组不被更新
- }
- }
- else if(recv_iap_flag == INIT){ //如果是初始化状态
- buf_to_short(&iap_bin_num, &usb_rx_buf[6]);
- if(iap_bin_num == 1){
- recv_erasure_flag = true; //进行擦除
- }
- }
- break;
- case 202:
- //考虑到固件包大小刚好能被250字节整除和不被乘除的情况
- if(pre_iap_bin_num >= iap_bin_Size / pack_size){
- if(recv_iap_flag == TRANSMIT){
- recv_iap_flag = FINISH;
- }
- recv_finish_flag = true;
- }
- break;
- default:
- break;
- }
- }
- else{
- crc_err++;
- }
- }
- //如果接收到协议头
- }else if(sixth_rxdata == mavlink_msg.stx &&
- third_rxdata == mavlink_msg.sysid &&
- second_rxdata == mavlink_msg.compid){
- receive_mavlink_msg = true;
- usb_rx_index = 0;
-
- mavlink_msg.payload = fifth_rxdata;
- mavlink_msg.seq = fourth_rxdata;
- mavlink_msg.msgid = first_rxdata;
- //填充rx_buf头
- usb_rx_buf[0] = sixth_rxdata;
- usb_rx_buf[1] = fifth_rxdata;
- usb_rx_buf[2] = fourth_rxdata;
- usb_rx_buf[3] = third_rxdata;
- usb_rx_buf[4] = second_rxdata;
- usb_rx_buf[5] = first_rxdata;
- usb_rx_index = 6;
- }
- //如果接收到急救指令
- else if((fourth_rxdata == '$') && (third_rxdata == 'A') && (second_rxdata == 'I')
- && (first_rxdata == 'D')){
- recv_aid_flag = true;
- }
- }
|