CGO3+ serial link

h-elsner · Dec 16, 2019

@dylanfm:
I'm intrested in that MAVlink-like message format that you found. Up to now I saw two different format in the sensor files, one from Typhoon H has packet start ID &BC and the messages from Typhoon H Plus start with $FD. The last one van be decodes according this: mavlink/c_library_v2

The first one (Typhoon H senor files) are a secret to me. I see a record format with payload length but I cannot decode the payload nor the message ID.

Is one of those two, what you see on the serial connection between CGO3+ and mainboard?

br HE

dylanfm · Dec 16, 2019

h-elsner said:
@dylanfm:
I'm intrested in that MAVlink-like message format that you found. Up to now I saw two different format in the sensor files, one from Typhoon H has packet start ID &BC and the messages from Typhoon H Plus start with $FD. The last one van be decodes according this: mavlink/c_library_v2

The first one (Typhoon H senor files) are a secret to me. I see a record format with payload length but I cannot decode the payload nor the message ID.
View attachment 19387

Is one of those two, what you see on the serial connection between CGO3+ and mainboard?

br HE

That screen capture is the Mavlink2 packets right? What is that program is that? Can it do mavlink 1?

I have a whole bunch of captured packets under various conditions here:
TyphoonH_CGO3_Serial_capture - Google Drive
Everything that has "RX" in the title is from the TH to the CGO3+, everything that has TX in the title is from the CGO3+ to the TH. Some of them have some framing errors because I decoded with autobaud turned on. I need to recapture those.

Here are the first 7 packets sent from the TH to the CGO3 after power up
From Drone to camera
Time:
8.3982205 FE030001000000010200018F68
8.39934725 FE03010100000001020001A844
8.400473958 FE03020100000001020001C130
8.401600708 FE050301000000000000000001AAE8
8.402900792 FE1A040100020001000000000000000000000C000300000853060008AB028808F4014629
8.406020958 FE22050100030002000000000000000000000000000000000000B3FFFFFFDE22006300FF615110050000C956
8.4098345 FE1A060100020001000000000000000000000C000300000853060008AB028808F40180A9

Here are the first 9 packets from the CGO3 to the TH

From camera to drone:
Time
3.474456792 FE0500020000000000007D00013D40
4.475220208 FE0501020000000000007D000168C5
5.476483875 FE0502020000000000007D00018642
6.477295958 FE0503020000000000007D0001D3C7
7.4783405 FE0504020000000000007D00014B45
8.479420458 FE06050200010003FA078538B838B3BC (EDIT: Corrected).
8.480811667 FE0506020000000000007D0001F047
9.48057875 FE0507020000000000007D0001A5C2
9.481883 FE06080200010003F9078638B8386037

They all start with 0xFE which matches the Mavlink V1 protocol.

I posted a question to stackoverflow with everything I was doing to try to decode the CRC.

Decoding an unknown CRC or checksum?

I've been trying decode the CRC or checksum algorithm that is being used for the serial communication between a drone and its camera for about a week without a lot of luck and I was wondering if an...

stackoverflow.com

I spent some time this morning trying to decode the messages with a mavlink library on an arduino, but haven't got it to work (yet?). I'm really not much of a programmer so I don't know if will be able to figure this out or not.

Mrgs1 · Dec 16, 2019

h-elsner said:
@dylanfm:
I'm intrested in that MAVlink-like message format that you found. Up to now I saw two different format in the sensor files, one from Typhoon H has packet start ID &BC and the messages from Typhoon H Plus start with $FD. The last one van be decodes according this: mavlink/c_library_v2

The first one (Typhoon H senor files) are a secret to me. I see a record format with payload length but I cannot decode the payload nor the message ID.
View attachment 19387

Is one of those two, what you see on the serial connection between CGO3+ and mainboard?

br HE

So is it possible to transfer all functions to a Pixhawk setup? So in theory an ST16 could control a H Plus?

h-elsner · Dec 17, 2019

Mrgs1 said:
So is it possible to transfer all functions to a Pixhawk setup? So in theory an ST16 could control a H Plus?

I don't know. I have looked in the FlightLog files and can only say what I have seen in the Sensor files. How the communication on ZigBee link and/or the communication (back-up control link) on 5GHz WiFi is done I don't know. Sensor files have different fomat for H and H Plus and the serial link to the cam is a third format that @dylanfm found.
The controller firmware is the key. Too much effort I would say....

dylanfm said:
That screen capture is the Mavlink2 packets right? What is that program is that? Can it do mavlink 1?

No, not really. This is the format of the Sensor_xxxxx.bin files that belongs to the FligtLogs. I looks a little bit like MAVlink messages but I couldn't find a relation to MAVlink2. I don't know what it really means.
But the Sensor files from H Plus have MAVlink V2 format. A summary is here in chapter 2.3.2:

http://h-elsner.mooo.com/pdf/Q500log2kml_en.pdf

The program is still Q500log2kml: h-elsner/Q500log2kml.

br HE

h-elsner · Dec 18, 2019

@dylanfm: Here was a discussion about RS232 CRC checks:

Serial RS232 Communication Checksums?

I am communicating with a servo via RS232 serial. The built-in functions that came with my servo are too slow (25 ms for a simple 54 byte message on a 57,600 baud port), so I am trying to write my ...

stackoverflow.com

Maybe the CRC finder script can help...

br HE

dylanfm · Dec 19, 2019

h-elsner said:
@dylanfm: Here was a discussion about RS232 CRC checks:

Serial RS232 Communication Checksums?

I am communicating with a servo via RS232 serial. The built-in functions that came with my servo are too slow (25 ms for a simple 54 byte message on a 57,600 baud port), so I am trying to write my ...

stackoverflow.com

Maybe the CRC finder script can help...

br HE

I will look into that script if I can't figure out how to check it with the mavlink library. Since I don't know which byte(s) are the message id, I can't test it with the standard CRC_extra lookup table, but there should only be 255 different values to loop through.

dylanfm · Dec 19, 2019

dylanfm said:
I will look into that script if I can't figure out how to check it with the mavlink library. Since I don't know which byte(s) are the message id, I can't test it with the standard CRC_extra lookup table, but there should only be 255 different values to loop through.

I will play with it more when I get home from work, but I believe using parts of the mavlink 1.0 library I have successfully calculated the CRC on at least 2 packets (one of the initialization packets and one of the 0x1A packets). For both of those the CRC_Extra value was set to 0. This should make it easier for me. I still won't know what the different bytes in the header mean, but for right now I just want to be able to get it to initialize and then get the pan and tilt to respond when I send it customized 0x1A packets and I think that is completely doable. Later on I might try to figure out some of the other messages that get sent to the CGO3+

h-elsner · Dec 20, 2019

Let's proceed here.

For me it's not clear where is the message ID

Code:

8.402900792 FE 1A 04 01 00020001 00000000000000000000 0C000300000853060008 AB028808F401 4629

0: Record ID
1: Payload length (n = 26 in example above)
2: Sequenz number
3: Source ID  (0..None, 1..Autopilot, 2..Gimbal)
4: ?
5: ?
6: ?
7: ?
8: Payload, n bytes
...
8+n:   CRC1
8+n+1: CRC2

Found a CRC check procedure in ST16_flightmode:

Java:

    public static synchronized int crc16(byte[] message, int offset, int count) {
        int crc;
        synchronized (MavLinkPackage.class) {
            crc = $FFFF;
            for (int i = offset; i < count; i++) {
                crc = (crc >> 8) ^ TABLE[((message[i] & 255) ^ crc) & 255];
            }
        }
        return crc;
    }

   public static boolean checkPackageCRC(byte[] yData, int length) {
        boolean z = true;
        if ((yData[0] & 255) != JpegConst.COM) {                                                 //254
            return false;
        }
        int count = ((yData[length - 1] & 255) << 8) | (yData[length - 2] & 255);
        yData[10] = (byte) 0;
        yData[11] = (byte) 0;
        if (count != crc16(yData, 1, length - 1)) {
            z = false;
        }
        return z;
    }

br HE

h-elsner · Dec 20, 2019

And some more from ST16 firmware:

Java:

    public static final byte YMAVLINK_MSG_ID_BIND_ACK = (byte) 7;
    public static final byte YMAVLINK_MSG_ID_BIND_REQUEST = (byte) 6;
    public static final byte YMAVLINK_MSG_ID_CHANNEL_DATA = (byte) 6;
    public static final byte YMAVLINK_MSG_PARAM_ACK_FAIL = (byte) 1;
    public static final byte YMAVLINK_MSG_PARAM_ACK_SUCCESS = (byte) 2;
    public static final byte YMAVLINK_MSG_PARAM_BIND_ENNABLE_REQUEST = (byte) 1;
    public static final byte YMAVLINK_MSG_PARAM_BIND_REQUEST_MASTER = (byte) 2;
    public static final byte YMAVLINK_MSG_PARAM_BIND_REQUEST_SLAVE = (byte) 3;
    public static final byte YMAVLINK_MSG_PARAM_NONE = (byte) 0;
    public static final byte YMAVLINK_MSG_TYPE_BINDED_CHECK_ACK = (byte) 2;
    public static final byte YMAVLINK_MSG_TYPE_BINDED_CHECK_REQUEST = (byte) 2;
    public static final byte YMAVLINK_MSG_TYPE_BIND_ENNABLE_ACK = (byte) 1;
    public static final byte YMAVLINK_MSG_TYPE_BIND_ENNABLE_REQUEST = (byte) 1;
    public static final byte YMAVLINK_MSG_TYPE_NONE = (byte) 0;

    public static final byte YMAVLINK_TARGET_ID_AUTOPILOT = (byte) 1;
    public static final byte YMAVLINK_TARGET_ID_GIMBAL = (byte) 2;
    public static final byte YMAVLINK_TARGET_ID_NULL = (byte) 0;

Target ID Camera = 3? Maybe. In CGO3Plusrequest.java it says: protected static final byte targetid = (byte) 3;

Header length is 8. This matches with your data. So I think we can guess:

0: Record ID = $FE
1: Payload length (n = 26 in example above)
2: Sequenz number
3: Source ID? (0..None, 1..Autopilot, 2..Gimbal)
4: Always Filled with Zero?
5: Target ID
6: Taget Sub ID
7: Message ID
8: Payload, n bytes
...
8+n: CRC1
8+n+1: CRC2

br HE

h-elsner · Dec 20, 2019

Only this message dosn't match:
8.479420458 FE 06 05 02 00010003 FA078538B8 38B3 ?

The payload length is wrong of a byte is missing.

dylanfm · Dec 20, 2019

dylanfm said:
I will play with it more when I get home from work, but I believe using parts of the mavlink 1.0 library I have successfully calculated the CRC on at least 2 packets (one of the initialization packets and one of the 0x1A packets). For both of those the CRC_Extra value was set to 0. This should make it easier for me. I still won't know what the different bytes in the header mean, but for right now I just want to be able to get it to initialize and then get the pan and tilt to respond when I send it customized 0x1A packets and I think that is completely doable. Later on I might try to figure out some of the other messages that get sent to the CGO3+

This code replicates the first 3 initialization packets and I believe any of the 0x1A packets (similar code correctly generated the correct CRC for several 0x1A sized packet). It uses the checksum.h file the mavlink library that I have. I don't know if that matches the current mavlink checksum.h file but I suspect it does.

Code:

#include "checksum.h"

#define PACKET_SEQ_LOC 2
#define K1_LOC_BYTE1 21
#define SL1_LOC_BYTE1 23
#define S1_LOC_BYTE1 25
#define S2_LOC_BYTE1 27

uint8_t initpacket[]={0xFE,0x03,0x00,0x01,0x00,0x00,0x00,0x01,0x02,0x00,0x01,0x00,0x00};
uint8_t outputmsg[]={0xFE,0x1A,0x00,0x01,0x00,0x02,0x00,0x01,0x9D,0x0A,0xE6,0xFF,0xFD,0xFF,0x97,0x25,0x1F,0x00,0x01,0x00,0x00,0x00,0x02,0x08,0x56,0x0D,0x00,0x08,0xAB,0x02,0x88,0x08,0xF4,0x01,0x00,0x00};
uint8_t packet_seq = 0;
uint8_t k1_command[2];
uint8_t sl1_command[2];
uint8_t s1_command[2];
uint8_t s2_command[2];

void setup() {
  // put your setup code here, to run once:
  uint8_t k;
  Serial.begin(115200);
  for (int i=0;i<3;i=i+1) {
    updatemsg(initpacket);
    for (k=0;k<13;k=k+1) {
      Serial.print(initpacket[k],HEX);
      Serial.print(",");
    }
    Serial.println(".");
  }

}

void loop() {
  uint8_t k;
  updatemsg(outputmsg);
  for (k=0;k<36;k=k+1) {
      Serial.print(outputmsg[k],HEX);
      Serial.print(",");
    }
    Serial.println(".");
  delay(1000);
}

void updatemsg(uint8_t* msgptr) {
  uint8_t c;
  uint16_t tempchecksum;
  if (packet_seq==255) {
    packet_seq=0;
  }
  msgptr[PACKET_SEQ_LOC]=packet_seq;
  if (msgptr[1]==0x1A) {
    msgptr[K1_LOC_BYTE1]=k1_command[0];
    msgptr[K1_LOC_BYTE1+1]=k1_command[1];
    msgptr[SL1_LOC_BYTE1]=sl1_command[0];
    msgptr[K1_LOC_BYTE1+1]=sl1_command[1];
    msgptr[S1_LOC_BYTE1]=s1_command[0];
    msgptr[S1_LOC_BYTE1+1]=s1_command[1];
    msgptr[S2_LOC_BYTE1]=s2_command[0];
    msgptr[S2_LOC_BYTE1+1]=s2_command[1];
  }
  crc_init(&tempchecksum);
  //length from packet + 10 header bytes - CRC
  for (uint8_t j=1;j<msgptr[1]+8;j=j+1) {
    crc_accumulate(msgptr[j],&tempchecksum);
  }
  //add the CRC_EXTRA Byte which seems to be 0
  crc_accumulate(0,&tempchecksum);
  msgptr[msgptr[1]+8]=tempchecksum & 0x00FF;
  msgptr[msgptr[1]+9]=((tempchecksum >> 8)&0x00FF);
  packet_seq=packet_seq+1;
}

h-elsner · Dec 20, 2019

Sounds good. I'm still curious what the bytes after the sequence number are. There is no logic in.
I guess message type, parameter IDs and so on are in payload.

dylanfm · Dec 20, 2019

h-elsner said:
Only this message dosn't match:
8.479420458 FE 06 05 02 00010003 FA078538B8 38B3 ?

The payload length is wrong of a byte is missing.

That is weird. I'm guess it is just a bad packet? I can try to do the CRC on it when I get home. If there was a bit error that changed the 2nd byte from 00001001 to 00001010 that would explain the issue. That is from the camera to the TH and I haven't looked a those much at all.

dylanfm · Dec 20, 2019

h-elsner said:
Sounds good. I'm still curious what the bytes after the sequence number are. There is no logic in.
I guess message type, parameter IDs and so on are in payload.

I'm guessing that you are probably right that byte 3 is the source ID and byte 5 is the destination ID. I don't know about the sub id or not. Looked packets sent from the TH to the CGO3+ from 0 to 60 seconds after powering to see how consistent the bytes were. Out of 4322 packets there were 23 different size payloads with values between 2 and 171. There were four different values for byte 3 (0,1,6,36). There two values for byte 4 (0 and 227). Four different values for byte 5 (0,1,2,3). Two different values of byte 6 (0,8). Five different values for byte 7 (0,1,2,3,255).

EDIT: I'm failing to get the format of this post right. I will edit again later.

Some of those are more common than others. For instance there one unique packet which was the only packet that had a 2 byte payload or byte3=0 or byte 6=8.

Code:

[TABLE]
[TR]
[TD]Time [s][/TD]
[TD]Value[/TD]
[TD]Parity Error[/TD]
[TD]Framing Error[/TD]
[TD]Beginning of packet[/TD]
[TD]Packet number[/TD]
[TD]Packet Size[/TD]
[TD]Packet Seq[/TD]
[TD]Source ID[/TD]
[TD]Source sub ID?[/TD]
[TD]Dest ID[/TD]
[TD]Dest sub id?[/TD]
[TD]Msg ID[/TD]
[/TR]
[TR]
[TD]50.58205642[/TD]
[TD]0xFE[/TD]
[TD][/TD]

[TD][/TD]

[TD]1[/TD]
[TD]3298[/TD]
[TD]2[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]8[/TD]
[TD]0[/TD]
[/TR]
[/TABLE]

Likewise this packet was unique as the only one with a payload of 171bytes, byte 3=36, byte 4=227, byte 5=1, or byte 7=3

Code:

[TABLE]
[TR]
[TD]Time [s][/TD]
[TD]Value[/TD]
[TD]Parity Error[/TD]
[TD]Framing Error[/TD]
[TD]Beginning of packet[/TD]
[TD]Packet number[/TD]
[TD]Packet Size[/TD]
[TD]Packet Seq[/TD]
[TD]Source ID[/TD]
[TD]Source sub ID?[/TD]
[TD]Dest ID[/TD]
[TD]Dest sub id?[/TD]
[TD]Msg ID[/TD]
[/TR]
[TR]
[TD]53.99512312[/TD]
[TD]0xFE[/TD]
[TD][/TD]

[TD][/TD]

[TD]1[/TD]
[TD]3666[/TD]
[TD]171[/TD]
[TD]254[/TD]
[TD]36[/TD]
[TD]227[/TD]
[TD]1[/TD]
[TD]0[/TD]
[TD]3[/TD]
[/TR]
[/TABLE]

So if one or both of those packets are bad data, then it reduces this different possible values for those bytes.

After that I think It makes since to figure out which headers look the same.
All the 0x1A packet headers looks the same:

Code:

[TABLE]
[TR]
[TD]Time [s][/TD]
[TD]Value[/TD]
[TD]Parity Error[/TD]
[TD]Framing Error[/TD]
[TD]Beginning of packet[/TD]
[TD]Packet number[/TD]
[TD]Packet Size[/TD]
[TD]Packet Seq[/TD]
[TD]Source ID[/TD]
[TD]Source sub ID?[/TD]
[TD]Dest ID[/TD]
[TD]Dest sub id?[/TD]
[TD]Msg ID[/TD]
[/TR]
[TR]
[TD]8.402900792[/TD]
[TD]0xFE[/TD]
[TD][/TD]

[TD][/TD]

[TD]1[/TD]
[TD]5[/TD]
[TD]26[/TD]
[TD]4[/TD]
[TD]1[/TD]
[TD]0[/TD]
[TD]2[/TD]
[TD]0[/TD]
[TD]1[/TD]
[/TR]
[/TABLE]

All the 0x19 packets seemed to be sent in pairs, one with byte 3=1, followed by one with byte 3=6 (these are also the only packets that have byte 3=6).

Code:

[TABLE]
[TR]
[TD]Time [s][/TD]
[TD]Value[/TD]
[TD]Parity Error[/TD]
[TD]Framing Error[/TD]
[TD]Beginning of packet[/TD]
[TD]Packet number[/TD]
[TD]Packet Size[/TD]
[TD]Packet Seq[/TD]
[TD]Source ID[/TD]
[TD]Source sub ID?[/TD]
[TD]Dest ID[/TD]
[TD]Dest sub id?[/TD]
[TD]Msg ID[/TD]
[/TR]
[TR]
[TD]17.61345642[/TD]
[TD]0xFE[/TD]
[TD][/TD]

[TD][/TD]

[TD]1[/TD]
[TD]902[/TD]
[TD]25[/TD]
[TD]133[/TD]
[TD]1[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]2[/TD]
[/TR]
[TR]
[TD]17.61648992[/TD]
[TD]0xFE[/TD]
[TD][/TD]

[TD][/TD]

[TD]1[/TD]
[TD]903[/TD]
[TD]25[/TD]
[TD]134[/TD]
[TD]6[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]0[/TD]
[TD]2[/TD]
[/TR]
[/TABLE]

There are a lot more packets to go through.

I will keep looking at it....

Dylan

dylanfm · Dec 21, 2019

h-elsner said:
Only this message dosn't match:
8.479420458 FE 06 05 02 00010003 FA078538B8 38B3 ?

The payload length is wrong of a byte is missing.

This was a copy and paste error. It should be :
8.479420458 FE 06 05 02 00010003 FA078538B838 B3BC

h-elsner · Dec 22, 2019

This may also a good source for understanding MAVlink communications used by Yuneec:

GitHub - 1107979819/MavlinkLib: MavlinLib

MavlinLib. Contribute to 1107979819/MavlinkLib development by creating an account on GitHub.

github.com

br HE

dylanfm · Dec 22, 2019

Here is the arduino code that I programmed onto a ESP32 to test the control the CGO3+. It mimics the tilt slider, pan knob, tilt mode switch, and pan mode switch. The simplified commands are read from arduino serial monitor and then sent to the CGO3. the commands are u,d,c for up, down, and center. l,r,m for left right and middle. a and v for tilt mode. f and g for pan mode. The pan seems to work after sending the g command. If I try to do l or r commands in f mode the camera pans to weird location, which might be the right response I don't know. I'm not sure if the a or v mode really changes anything for the tilt. I could be wrong which bytes control that mode or I might just need to pay more attention to see if they have an effect.

Code:

#include <SoftwareSerial.h>
#include <Arduino.h>

#include "checksum.h"
//#include <SoftwareSerial.h>
//#include <Arduino.h>
#include <dummy.h>

#define PACKET_SEQ_LOC 2
#define K1_LOC_BYTE1 22
#define SL1_LOC_BYTE1 24
#define S1_LOC_BYTE1 26
#define S2_LOC_BYTE1 28

#define CGO3_RX_PIN 32
#define CGO3_TX_PIN 33

uint8_t initpacket[]={0xFE,0x03,0x00,0x01,0x00,0x00,0x00,0x01,0x02,0x00,0x01,0x00,0x00};
uint8_t outputmsg[]={0xFE,0x1A,0x00,0x01,0x00,0x02,0x00,0x01,0x9D,0x0A,0xE6,0xFF,0xFD,0xFF,0x97,0x25,0x1F,0x00,0x01,0x00,0x00,0x00,0x02,0x08,0x56,0x0D,0x00,0x08,0xAB,0x02,0x88,0x08,0xF4,0x01,0x00,0x00};
uint8_t packet_seq = 0;
uint8_t k1_command[2];
uint8_t sl1_command[2];
uint8_t s1_command[2];
uint8_t s2_command[2];

SoftwareSerial CGO3Serial(CGO3_RX_PIN,CGO3_TX_PIN, false);

void setup() {
  // put your setup code here, to run once:
  uint8_t k;
  Serial.begin(115200);
  CGO3Serial.begin(115200);
  delay(100);
  while (CGO3Serial.available()<1) {
    for (int i=0;i<5;i=i+1) {
      updatemsg(initpacket);
      Serial.println(".");
      CGO3Serial.write(initpacket,13);   
    }
  }
  s1_command[0]=0x00;
  s1_command[1]=0x08;
  s2_command[0]=0x00;
  s2_command[1]=0x08;
  sl1_command[0]=0x00;
  sl1_command[1]=0x08;
  k1_command[0]=0x00;
  k1_command[1]=0x08;
}

void loop() {
  uint8_t k;
  char t;
  if (Serial.available() > 0) {
    t=Serial.read();
    switch (t) {
      case 'c':
        sl1_command[0]=0x02;
        sl1_command[1]=0x08;
        break;
      case 'u':
        sl1_command[0]=0xAB;
        sl1_command[1]=0x02;
        break;
      case 'd':
        sl1_command[0]=0x56;
        sl1_command[1]=0x0d;
        break; 
       case 'm':
        k1_command[0]=0x02;
        k1_command[1]=0x08;
        break;
      case 'r':
        k1_command[0]=0xAB;
        k1_command[1]=0x02;
        break;
      case 'l':
        k1_command[0]=0x56;
        k1_command[1]=0x0d;
        break;
      case 'a':
        s1_command[0]=0xAB;
        s1_command[1]=0x02;
        break;
      case 'v':
        s1_command[0]=0x54;
        s1_command[1]=0x0D;
        break;
      case 'f':
        s2_command[0]=0xAB;
        s2_command[1]=0x02;
        break;
      case 'g':
        s2_command[0]=0x54;
        s2_command[1]=0x0D;
        break;
    }
  }
  updatemsg(outputmsg);
  CGO3Serial.write(outputmsg,outputmsg[1]+10);

}

void updatemsg(uint8_t* msgptr) {
  uint8_t c;
  uint16_t tempchecksum;

  msgptr[PACKET_SEQ_LOC]=packet_seq;
  if (msgptr[1]==0x1A) {
    msgptr[K1_LOC_BYTE1]=k1_command[0];
    msgptr[K1_LOC_BYTE1+1]=k1_command[1];
    msgptr[SL1_LOC_BYTE1]=sl1_command[0];
    msgptr[SL1_LOC_BYTE1+1]=sl1_command[1];
    msgptr[S1_LOC_BYTE1]=s1_command[0];
    msgptr[S1_LOC_BYTE1+1]=s1_command[1];
    msgptr[S2_LOC_BYTE1]=s2_command[0];
    msgptr[S2_LOC_BYTE1+1]=s2_command[1];
  }
  crc_init(&tempchecksum);
  //length from packet + 10 header bytes - CRC
  for (uint8_t j=1;j<msgptr[1]+8;j=j+1) {
    crc_accumulate(msgptr[j],&tempchecksum);
  }
  //add the CRC_EXTRA Byte which seems to be 0
  crc_accumulate(0,&tempchecksum);
  msgptr[msgptr[1]+8]=tempchecksum & 0x00FF;
  msgptr[msgptr[1]+9]=((tempchecksum >> 8)&0x00FF);
  if (packet_seq==255) {
    packet_seq=0;
  } else {
    packet_seq=packet_seq+1;
  }
}

Mrgs1 · Dec 23, 2019

h-elsner said:
I don't know. I have looked in the FlightLog files and can only say what I have seen in the Sensor files. How the communication on ZigBee link and/or the communication (back-up control link) on 5GHz WiFi is done I don't know. Sensor files have different fomat for H and H Plus and the serial link to the cam is a third format that @dylanfm found.
The controller firmware is the key. Too much effort I would say....

No, not really. This is the format of the Sensor_xxxxx.bin files that belongs to the FligtLogs. I looks a little bit like MAVlink messages but I couldn't find a relation to MAVlink2. I don't know what it really means.
But the Sensor files from H Plus have MAVlink V2 format. A summary is here in chapter 2.3.2:

http://h-elsner.mooo.com/pdf/Q500log2kml_en.pdf

The program is still Q500log2kml: h-elsner/Q500log2kml.

br HE

Sorry I'll rephrase. Could a C23 camera be controlled by an ST16+?

zrgravity · Jun 11, 2020

dylanfm said:

Here is the arduino code that I programmed onto a ESP32 to test the control the CGO3+. It mimics the tilt slider, pan knob, tilt mode switch, and pan mode switch. The simplified commands are read from arduino serial monitor and then sent to the CGO3. the commands are u,d,c for up, down, and center. l,r,m for left right and middle. a and v for tilt mode. f and g for pan mode. The pan seems to work after sending the g command. If I try to do l or r commands in f mode the camera pans to weird location, which might be the right response I don't know. I'm not sure if the a or v mode really changes anything for the tilt. I could be wrong which bytes control that mode or I might just need to pay more attention to see if they have an effect.

Code:

#include <SoftwareSerial.h>
#include <Arduino.h>

#include "checksum.h"
//#include <SoftwareSerial.h>
//#include <Arduino.h>
#include <dummy.h>

#define PACKET_SEQ_LOC 2
#define K1_LOC_BYTE1 22
#define SL1_LOC_BYTE1 24
#define S1_LOC_BYTE1 26
#define S2_LOC_BYTE1 28

#define CGO3_RX_PIN 32
#define CGO3_TX_PIN 33

uint8_t initpacket[]={0xFE,0x03,0x00,0x01,0x00,0x00,0x00,0x01,0x02,0x00,0x01,0x00,0x00};
uint8_t outputmsg[]={0xFE,0x1A,0x00,0x01,0x00,0x02,0x00,0x01,0x9D,0x0A,0xE6,0xFF,0xFD,0xFF,0x97,0x25,0x1F,0x00,0x01,0x00,0x00,0x00,0x02,0x08,0x56,0x0D,0x00,0x08,0xAB,0x02,0x88,0x08,0xF4,0x01,0x00,0x00};
uint8_t packet_seq = 0;
uint8_t k1_command[2];
uint8_t sl1_command[2];
uint8_t s1_command[2];
uint8_t s2_command[2];

SoftwareSerial CGO3Serial(CGO3_RX_PIN,CGO3_TX_PIN, false);

void setup() {
  // put your setup code here, to run once:
  uint8_t k;
  Serial.begin(115200);
  CGO3Serial.begin(115200);
  delay(100);
  while (CGO3Serial.available()<1) {
    for (int i=0;i<5;i=i+1) {
      updatemsg(initpacket);
      Serial.println(".");
      CGO3Serial.write(initpacket,13);  
    }
  }
  s1_command[0]=0x00;
  s1_command[1]=0x08;
  s2_command[0]=0x00;
  s2_command[1]=0x08;
  sl1_command[0]=0x00;
  sl1_command[1]=0x08;
  k1_command[0]=0x00;
  k1_command[1]=0x08;
}

void loop() {
  uint8_t k;
  char t;
  if (Serial.available() > 0) {
    t=Serial.read();
    switch (t) {
      case 'c':
        sl1_command[0]=0x02;
        sl1_command[1]=0x08;
        break;
      case 'u':
        sl1_command[0]=0xAB;
        sl1_command[1]=0x02;
        break;
      case 'd':
        sl1_command[0]=0x56;
        sl1_command[1]=0x0d;
        break;
       case 'm':
        k1_command[0]=0x02;
        k1_command[1]=0x08;
        break;
      case 'r':
        k1_command[0]=0xAB;
        k1_command[1]=0x02;
        break;
      case 'l':
        k1_command[0]=0x56;
        k1_command[1]=0x0d;
        break;
      case 'a':
        s1_command[0]=0xAB;
        s1_command[1]=0x02;
        break;
      case 'v':
        s1_command[0]=0x54;
        s1_command[1]=0x0D;
        break;
      case 'f':
        s2_command[0]=0xAB;
        s2_command[1]=0x02;
        break;
      case 'g':
        s2_command[0]=0x54;
        s2_command[1]=0x0D;
        break;
    }
  }
  updatemsg(outputmsg);
  CGO3Serial.write(outputmsg,outputmsg[1]+10);

}

void updatemsg(uint8_t* msgptr) {
  uint8_t c;
  uint16_t tempchecksum;

  msgptr[PACKET_SEQ_LOC]=packet_seq;
  if (msgptr[1]==0x1A) {
    msgptr[K1_LOC_BYTE1]=k1_command[0];
    msgptr[K1_LOC_BYTE1+1]=k1_command[1];
    msgptr[SL1_LOC_BYTE1]=sl1_command[0];
    msgptr[SL1_LOC_BYTE1+1]=sl1_command[1];
    msgptr[S1_LOC_BYTE1]=s1_command[0];
    msgptr[S1_LOC_BYTE1+1]=s1_command[1];
    msgptr[S2_LOC_BYTE1]=s2_command[0];
    msgptr[S2_LOC_BYTE1+1]=s2_command[1];
  }
  crc_init(&tempchecksum);
  //length from packet + 10 header bytes - CRC
  for (uint8_t j=1;j<msgptr[1]+8;j=j+1) {
    crc_accumulate(msgptr[j],&tempchecksum);
  }
  //add the CRC_EXTRA Byte which seems to be 0
  crc_accumulate(0,&tempchecksum);
  msgptr[msgptr[1]+8]=tempchecksum & 0x00FF;
  msgptr[msgptr[1]+9]=((tempchecksum >> 8)&0x00FF);
  if (packet_seq==255) {
    packet_seq=0;
  } else {
    packet_seq=packet_seq+1;
  }
}

Do you have a source for the checksum.h file? I'm working on porting this to python, so any other documentation you have on the protocol would be helpful!

zrgravity · Jun 11, 2020

zrgravity said:
Do you have a source for the checksum.h file? I'm working on porting this to python, so any other documentation you have on the protocol would be helpful!

Current progress (not working...) is at zrgravity/cgo3-python

Probably my CRC is incorrect, or the initialization isn't happening fast enough. If you have some signal captures for the init sequence and some good commands with correct CRC, that could be really helpful.

At some point I plan on looking into if the camera can be commanded to start/stop recording over the serial interface with the gimbal.