Hello Fellow Yuneec Pilot!
Join our free Yuneec community and remove this annoying banner!
Sign up

Reverse engineering CGO3+ UART

Joined
Sep 28, 2020
Messages
1,378
Reaction score
539
Age
56
Location
Kozloduy, Bulgaria
* UART support for gimbal control (tilt, rotate). This will need a lot of research/high effort.

MAGIC = {0x05, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x7F, 0x00, 0x01};

#define UART_BAUD_RATE 115200

Send the MAGIC to the copter's camera UART every second. Log what the copter sends to you. Without a camera attached. Paste here what you have received. If this is done I'll paste some text too. ;)
 
To don't waste time about the CRC>

#include <stdint.h>
#include <stddef.h>

uint16_t crc16_mcrf4xx(uint16_t crc, uint8_t *data, size_t len)
{
uint8_t t;
uint8_t L;
if (!data || len < 0)
return crc;

while (len--)
{
crc ^= *data++;
L = crc ^ (crc << 4);
t = (L << 3) | (L >> 5);
L ^= (t & 0x07);
t = (t & 0xF8) ^ (((t << 1) | (t >> 7)) & 0x0F) ^ (uint8_t)(crc >> 8);
crc = (L << 8) | t;
}
return crc;
}
 
and a bit more...
uint16_t crc16_mcrf4xx(uint16_t crc, uint8_t *data, size_t len);

struct Mavlink_V1 //https://mavlink.io/en/guide/serialization.html#v1_packet_format
{
uint8_t magic;
// ---
uint8_t len;
uint8_t seq;
uint8_t sysid;
uint8_t compid;
uint8_t msgid;
uint8_t msgTypeH;
uint8_t msgTypeL;
// uint8_t payload[255];
uint16_t checksum; // -
uint8_t crcLow; // |
uint8_t crcHigh; // -
};

uint16_t initCRC = 0xFFFF, calcCRC = 0x0000;
uint8_t magic[] = {0xFE};
uint8_t zero[] = {0x00};
uint8_t crc[] = {0x00, 0x00};

int count;

int receivePacket(uint8_t *read_buffer)
{
struct Mavlink_V1 mavlink;

int
position = 0x00,
receivedBytes = 0x00,
count = 0x00;

do
{
if (uart_gets(&mavlink.magic, 1) == 0)
{
return count;
}
}
while (mavlink.magic != 0xFE);

while (uart_gets(&mavlink.len, 1) == 0);
read_buffer[position++] = mavlink.len;

while (uart_gets(&mavlink.seq, 1) == 0);
read_buffer[position++] = mavlink.seq;

while (uart_gets(&mavlink.sysid, 1) == 0);
read_buffer[position++] = mavlink.sysid;

while (uart_gets(&mavlink.compid, 1) == 0);
read_buffer[position++] = mavlink.compid;

while (uart_gets(&mavlink.msgid, 1) == 0);
read_buffer[position++] = mavlink.msgid;

while (uart_gets(&mavlink.msgTypeH, 1) == 0);
read_buffer[position++] = mavlink.msgTypeH;

while (uart_gets(&mavlink.msgTypeL, 1) == 0);
read_buffer[position++] = mavlink.msgTypeL;

do
{
receivedBytes += uart_gets(&read_buffer[position + receivedBytes], mavlink.len - receivedBytes);
}
while (mavlink.len != receivedBytes);

while (uart_gets(&mavlink.crcLow, 1) == 0);

while (uart_gets(&mavlink.crcHigh, 1) == 0);

count = position + receivedBytes;


calcCRC = (uint16_t) crc16_mcrf4xx(initCRC, read_buffer, count + 1);
 
MAGIC = {0x05, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x7F, 0x00, 0x01};

#define UART_BAUD_RATE 115200

Send the MAGIC to the copter's camera UART every second. Log what the copter sends to you. Without a camera attached. Paste here what you have received. If this is done I'll paste some text too. ;)
Forgot to remember you should not forget to pack the telegram properly.

int sendPacket(uint8_t* send_buffer, int send_buffer_size)
{
...
{
calcCRC = (uint16_t) crc16_mcrf4xx(initCRC, send_buffer, send_buffer_size);
calcCRC = (uint16_t) crc16_mcrf4xx(calcCRC, zero, sizeof(zero));
crc[0] = calcCRC & 0xFF;
crc[1] = (calcCRC >> 8) & 0xFF;

uart_puts_raw(magic, sizeof(magic));
uart_puts_raw(send_buffer, send_buffer_size);
uart_puts_raw(crc, sizeof(crc));
}
...
 
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;
*/
 
  • Like
Reactions: Chris Koch
MAGIC = {0x05, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x7F, 0x00, 0x01};
This is Heartbeat Message from camera, sent each second. Unfortunately the Yuneec MAVlink differs from MAVlink V1 description. I did not found any documetation of that. My interpretation:
0xFE Magic for MAVlink V1
0x05 Length payload (n=0 n/a; n=1 6; n>1 5+n)
0x00 Sequence number
0x02 System ID (2 is camera)
0x00 Component ID (Looks like always zero)
0x00 Target ID (0 Broadcast)
0x00 SubTarget ID (Looks like always zero)
0x00 Message ID (0 - Heartbeat)
0x00 ?
0x00 ?
0x7D ? Status flags?
0x00 ?
0x01 ? could probably be MAV Version 1
0x3D CRC low
0x40 CRC high

The code above I don't understand. Currently I'm recording serial communication. I will see what's coming out.
The oly message that meets nearly 1:1 the MAVlink desription is MsgID 20 'PARAM_REQUEST_READ'. The parameter is always "Q2SYS_SW_VER".
Let's see if I find camera, drone attitude NED and commands for that.
 
Look at all of my posts thoroughly. There is all the necessary information. Get it as is, don't try to find similarities with the traditional MavLink.
The heartbeat message is not informative. It is necessary only to emulate a camera connected. In another case, the drone will not send any message.
 
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.
I'm not fully agree with that what your source code implies. For some data field I can confirm, for some I'm uncertain for some I disagree.
What is the source of that findings?

The only MAV message that I could verify is msg_ID=8. I call it "RC data". Only the last byte is not verified but it coincidences with GPS accuracy.

Here is what I have:
 

Attachments

My experiments are only the source. The results covered my needs. You're free to do your own.
 
As you can see this is C but not C++. Experiments are based on Pi4 for messages analysis and on Atmel 328 for mavlink to PWM translator.
Due to the specific configuration of the Pi, there is no way to reproduce it without a lot of explanation from my side. The other projects need quartz replacement, so the situation is the same.

I have posted all the necessary information to prepare everything you want.
 
As you can see this is C but not C++. Experiments are based on Pi4 for messages analysis and on Atmel 328 for mavlink to PWM translator.
Due to the specific configuration of the Pi, there is no way to reproduce it without a lot of explanation from my side. The other projects need quartz replacement, so the situation is the same.

I have posted all the necessary information to prepare everything you want.
Where did you post the source?
 
Do you think I remember the whole code İ have posted? What you see is what you have. I have the same ;) Nothing in the structure and fields is missing. What I didn't understand properly or didn't investigate, as I decided it was useless for me, is marked as TBD (ToBeDefined). Use all as is. This was done by me two or so years before (check my posts about CGO3+ video streaming), and the date is almost the same.
 
Do you think I remember the whole code İ have posted? What you see is what you have. I have the same ;) Nothing in the structure and fields is missing. What I didn't understand properly or didn't investigate, as I decided it was useless for me, is marked as TBD (ToBeDefined). Use all as is. This was done by me two or so years before (check my posts about CGO3+ video streaming), and the date is almost the same.
It looks like the parser_show function is incomplete. The last "else if" condition appears incomplete. It's hard to tell since everything after it is commented out.
 
As I mentioned in the listing almost all is commented out because it was used for different purposes in different projects. The last one was MavLinkY to PWM to use GB203 with H480.

Read all thoroughly and go deep into the logic of the code. This is not intended for use AS-IS.

After the last else was a code to make three blinks of the LED if there is a loss of synchronization or an unknown message. Is this important at all to understand the rest of the code?

The coding itself is not so simple because you should organize self-synchronization and so on. All is dependent on the target you have.
 
Now I found out that the Message Type 0xFF and Target_ID=3 contains the same data that are recorded as Sensor_xxxxx.txt in ST16 flight logs.

The message itself contains an nested message format with again Magic=0xBC, payload length, sequence number, Sys_ID=1, Component_ID=1, Msg_ID, payload and CRC16.

This means the flight controller sends those data to camera and camera via WiFi to ST16. ST16 put it in the flight log and possibly doing "something" with it (or else not...).
 

New Posts

Members online

Forum statistics

Threads
21,169
Messages
244,178
Members
27,910
Latest member
Conson Droppa