Today is your day! Here you are! Almost all is commented on because I have used the same parser for different purposes. This one is probably from the CGO3+ (UART) to GB203 (PWM) converter.
// Yuneec Typhoon H480 Mavlink ID's:
const char
to_all = 0x00,
drone = 0x01,
gimbal = 0x02,
camera = 0x03,
remote = 0x04,
_tbd_1 = 0x05,
_tbd_2 = 0x06;
// int i = 0;
// bool _switch = false;
void parser_init(void)
{
}
void parser_show(uint8_t *read_buffer) // int count,
{
uint8_t *ptr = read_buffer;
uint8_t *len = ptr + 0; //&read_buffer[0];
// uint8_t *seq = ptr++; //&read_buffer[1];
uint8_t *sysid = ptr + 2; //&read_buffer[2];
// uint8_t *compid = ptr++; //&read_buffer[3];
uint8_t *msgid = ptr + 4; //&read_buffer[4];
// uint8_t *msgTypeH = ptr++; //&read_buffer[5];
// uint8_t *msgTypeL = ptr++; //&read_buffer[6];
if (*len == 0x1A && *sysid == drone && *msgid == gimbal)
{
// int16_t copterYaw = ((read_buffer[8] << 8) & 0xFF00) | read_buffer[7];
// int16_t copterPitch = ((read_buffer[10] << 8) & 0xFF00) | read_buffer[9];
// int16_t copterRoll = ((read_buffer[12] << 8) & 0xFF00) | read_buffer[11];
// int16_t cameraPan = ((read_buffer[22] << 8) & 0xFF00) | read_buffer[21];
int16_t cameraTilt = ((read_buffer[24] << 8) & 0xFF00) | read_buffer[23];
tt = cameraTilt;
// int16_t cameraPanMode = ((read_buffer[28] << 8) & 0xFF00) | read_buffer[27];
// int16_t cameraTiltMode = ((read_buffer[30] << 8) & 0xFF00) | read_buffer[29];
// int16_t copterVz = ((read_buffer[14] << 8) & 0xFF00) | read_buffer[13];
// int16_t tbd_2 = ((read_buffer[16] << 8) & 0xFF00) | read_buffer[15];
// int16_t copterVy = ((read_buffer[18] << 8) & 0xFF00) | read_buffer[17];
// int16_t copterVx = ((read_buffer[20] << 8) & 0xFF00) | read_buffer[19];
// int16_t tbd_5 = ((read_buffer[32] << 8) & 0xFF00) | read_buffer[31];
// snprintf(str, BUFSIZE, "Yaw: %3.1f Pitch: %2.1f Roll: %2.1f ", (180.0/16000 * copterYaw - 180), (-90.0 / 7650 * copterPitch), (-90.0 / 7750 * copterRoll));
// print_string_horizontal(1, 11, str, sizeof(str), _White, _Black);
// snprintf(str, BUFSIZE, "Pan: %2.0f%% Tilt: %2.0f Pan mode: %4d Tilt mode: %4d ", (100.0/2048 * (cameraPan - 2048)), (90.0/4096 * cameraTilt), cameraPanMode, cameraTiltMode);
// print_string_horizontal(1, 13, str, sizeof(str), _White, _Black);
// snprintf(str, BUFSIZE, "Vx: %4d Vy: %4d Vz: %4d ", copterVx, copterVy, copterVz);
// print_string_horizontal(1, 15, str, sizeof(str), _White, _Black);
}
else if (*len == 0x22 && *sysid == drone && *msgid == camera)
{
// int32_t copterLongitude = (((read_buffer[10] << 24) & 0xFF000000) | ((read_buffer[9] << 16) & 0xFFFF0000) | ((read_buffer[8] << 8) & 0xFFFFFF00) | read_buffer[7]);
// printf("%08X, %f\n", copterLongitude, 0.0001f * copterLongitude);
// int32_t copterLatitude = (((read_buffer[14] << 24) & 0xFF000000) | ((read_buffer[13] << 16) & 0xFFFF0000) | ((read_buffer[12] << 8) & 0xFFFFFF00) | read_buffer[11]);
// printf("%08X, %f\n", copterLatitude, 0.0001f * copterLatitude);
// int32_t copterAltitude = (((read_buffer[18] << 24) & 0xFF000000) | ((read_buffer[17] << 16) & 0xFFFF0000) | ((read_buffer[16] << 8) & 0xFFFFFF00) | read_buffer[15]);
// snprintf(str, BUFSIZE, "Long: %f\n Lat: %f\n Alt: %.2f\n ",0.0001f * copterLongitude, 0.0001f * copterLatitude, 0.01f * copterAltitude);
// print_string_horizontal(1, 17, str, sizeof(str), _White, _Black);
// printf("%08X, %.2f\n", copterAltitude, 0.01f * copterAltitude);
// int32_t remoteLongitude = (((read_buffer[22] << 24) & 0xFF000000) | ((read_buffer[21] << 16) & 0xFFFF0000) | ((read_buffer[20] << 8) & 0xFFFFFF00) | read_buffer[19]);
// printf("%08X, %f\n", remoteLongitude, 0.0001f * remoteLongitude);
/*
uint16_t copterTBD1 = ((read_buffer[20] << 8) & 0xFF00) | read_buffer[19];
uint16_t copterTBD2 = ((read_buffer[22] << 8) & 0xFF00) | read_buffer[21];
uint16_t copterTBD3 = ((read_buffer[24] << 8) & 0xFF00) | read_buffer[23];
uint16_t copterTBD4 = ((read_buffer[26] << 8) & 0xFF00) | read_buffer[25];
uint16_t copterTBD5 = ((read_buffer[28] << 8) & 0xFF00) | read_buffer[27];
uint16_t copterTBD6 = ((read_buffer[30] << 8) & 0xFF00) | read_buffer[29];
snprintf(str, BUFSIZE,"%04X, %04X, %04X, %04X, %04X, %04X\n", copterTBD1, copterTBD2, copterTBD3, copterTBD4, copterTBD5, copterTBD6);
print_string_horizontal(1, 19, str, sizeof(str), _White, _Black);
*/
// int32_t remoteLatitude = (((read_buffer[26] << 24) & 0xFF000000) | ((read_buffer[25] << 16) & 0xFFFF0000) | ((read_buffer[24] << 8) & 0xFFFFFF00) | read_buffer[23]);
// printf("%08X, %f\n", remoteLatitude, 0.0001f * remoteLatitude);
// int32_t copterTBD = (((read_buffer[30] << 24) & 0xFF000000) | ((read_buffer[29] << 16) & 0xFFFF0000) | ((read_buffer[28] << 8) & 0xFFFFFF00) | read_buffer[27]);
// printf("%08X, %.2f\n", copterTBD, 0.01f * copterTBD);
/* i = 31;
while (i < count)
{
printf("%02X", read_buffer[i++]);
}
printf("\n\n");
i = 0;
*/
