static void OnDataReceived(Packet_t *pkt) { if(pkt->type != 0xA5) { return; } int offset = 0; uint8_t *p = pkt->buf; while(offset < pkt->payload_len) { switch(p[offset]) { case kItemID: id = p[1]; offset += 2; break; case kItemAccRaw: case kItemAccCalibrated: case kItemAccFiltered: case kItemAccLinear: memcpy(acc, p + offset + 1, sizeof(acc)); offset += 7; break; case kItemGyoRaw: case kItemGyoCalibrated: case kItemGyoFiltered: memcpy(gyo, p + offset + 1, sizeof(gyo)); offset += 7; break; case kItemMagRaw: case kItemMagCalibrated: case kItemMagFiltered: memcpy(mag, p + offset + 1, sizeof(mag)); offset += 7; break; case kItemRotationEular: eular[0] = ((float)(int16_t)(p[offset+1] + (p[offset+2]<<8)))/100; eular[1] = ((float)(int16_t)(p[offset+3] + (p[offset+4]<<8)))/100; eular[2] = ((float)(int16_t)(p[offset+5] + (p[offset+6]<<8)))/10; offset += 7; break; case kItemRotationEular2: memcpy(eular, p + offset + 1, sizeof(eular)); offset += 13; break; case kItemRotationQuat: memcpy(quat, p + offset + 1, sizeof(quat)); offset += 17; break; case kItemPressure: offset += 5; break; case kItemTemperature: offset += 5; break; default: printf("data decode wrong\r\n"); return; break; } } }
时间: 2024-02-10 08:35:16 浏览: 179
这段代码实现了函数OnDataReceived的具体内容,处理接收到的数据包pkt。
代码中先对数据包的类型进行判断,如果不是0xA5,就直接返回,不做处理。
接着,定义了一个整型变量offset和一个指向数据包缓存的指针p,用于在数据包中遍历各个数据项。
在while循环中,使用switch语句对不同的数据项进行处理:
1. 对于ID号(kItemID)数据项,将其值赋给全局变量id,并将offset增加2。
2. 对于加速度(acc)相关的数据项(kItemAccRaw、kItemAccCalibrated、kItemAccFiltered、kItemAccLinear),将其值复制到全局变量acc中,并将offset增加7。
3. 对于陀螺仪(gyo)相关的数据项(kItemGyoRaw、kItemGyoCalibrated、kItemGyoFiltered),将其值复制到全局变量gyo中,并将offset增加7。
4. 对于磁力计(mag)相关的数据项(kItemMagRaw、kItemMagCalibrated、kItemMagFiltered),将其值复制到全局变量mag中,并将offset增加7。
5. 对于欧拉角(eular)相关的数据项(kItemRotationEular、kItemRotationEular2),将其值转换为浮点数并存储到全局变量eular中,并将offset增加7或13。
6. 对于四元数(quat)相关的数据项(kItemRotationQuat),将其值复制到全局变量quat中,并将offset增加17。
7. 对于压力(kItemPressure)和温度(kItemTemperature)相关的数据项,将offset增加5。
8. 如果遇到未知的数据项,打印错误信息并直接返回。
函数执行完毕后,全局变量中存储的数据就被更新为最新的值。
阅读全文