The team mode has worth to be investigated too. Better is now than later, due to use the same experimental base.
Good point, yes I will do. I switch off/on the ST16 very often in order to download telemetry to compare with what I have sent.Hold in mind, the SR24 has been informed of what will be connected in the power-up stage of the ST16. If you change the model you should restart the ST.
ZigBee sniffer will be the best of your friends. It has online export to a terminal. A suitable one is YAT. After that you'll have the up and down streams to analyze, The good point will be to prepare some parser of this stream. On the Pi of course. This will be the starting point to my old dream to have a secondary telemetry display on the ST16n order to download telemetry to compare with what I have sent.
That's awesome and very nice to see, congrats!ZigBee sniffer will not help. ZigBee is only the transport protocol. What comes out depends on SR24 firmware and what drone and controller firmware defines. The data format is pretty clear now. I can see the channel data on the Pi and I can send valid telemetry, faking a drone.
View attachment 26602
br HE
And here I am - even more late!Late is better than never...![]()
Here is how I do the binding in the Thunderbird firmware:Try to disconnect it from the Pi, not to power down only. Probably you feed it via RS232 via some internal circuit in the SR24.
Do you put the receiving side in binding mode? I don't know the sequence for this, because especially the drone side is out of my interest, but I guess some happens when the copter blinks orange. You can sniff what is in the air at this moment with a ZigBee sniffer.
Have you managed to find out what's the difference between the receiving SR24 on the drone, and the transmitting one on the ST16?
Hi,At least the speed is different. I never tried to listen from the drone side, because my ideas were pointed in two directions:
1. To add a secondary telemetry panel to the ST16 like in the ST24 FPV mode and to inject the drone position in the Google Earth pro. For example, for me is very important to know the position of the ST16 and the quality of the received GPS signal.;
2. To sniff the link between the camera and the drone for further development of the ID and telemetry transmitter when this will be requested from FAA, EASA, Mars aliens and so on.
Both points were successfully engineered but only this was done. I'm not a coder so I usually asking someone to give some help in the implementation of my ideas. Helmut did some good tasks for me and I very much appreciate his help.
Really, in the camera<>drone communication I found a strange device, numbered as 5, which is still a mystery for me, but this is not so important. All the rest is more or less clear.
One more idea was to change the SR24 in the drone with a 4G modem and autonomous movement with fake GPS data in the follow-me mode. Probably this is illegal in some countries, but "just for educational purposes" will be OK.
Some examples:
Sync Ln ???? Seq PAN Dest Src Rad Time Latitude Longitude Altitude Vx Vy Vz Sat Vbat Cb Roll Pitch Yaw M6 IMU B/C f/m vt Err GPS fsk CRC8
5555 30 8841 84 9616 46F1 8AC5 08 A603 6045181A AAFB230E 01000000 FCFF 0100 0900 0B 6C 00 A4FF F5FF 6F5D FF 61 55 10 05 00 2D E5 5C
5555 30 8841 C4 9616 46F1 8AC5 08 A803 6045181A AAFB230E 02000000 FCFF 0100 0900 0B 6C 00 A4FF F5FF 6F5D FF 61 55 10 05 00 2D E9 90
5555 30 8841 84 9616 46F1 8AC5 08 AA03 6045181A AAFB230E 02000000 FCFF 0100 0900 0B 6C 00 A4FF F5FF 6F5D FF 61 55 10 05 00 2D E5 48
5555 30 8841 84 9616 46F1 8AC5 08 AC03 6045181A AAFB230E 02000000 FCFF 0100 0900 0B 6C 00 A4FF F5FF 6F5D FF 61 55 10 05 00 2D E9 47
5555 30 8841 C4 9616 46F1 8AC5 08 AE03 6045181A AAFB230E 02000000 FDFF 0000 0B00 0B 6C 00 A4FF F5FF 6F5D FF 61 55 10 05 00 2D E4 4E
5555 30 8841 84 9616 46F1 8AC5 08 B003 5645181A AFFB230E 02000000 FDFF 0000 0B00 0B 6C 00 A4FF F5FF 6F5D FF 61 55 10 05 00 2D E5 8B
5555 30 8841 84 9616 46F1 8AC5 08 B203 5645181A AFFB230E 03000000 FDFF 0000 0B00 0B 6C 00 A4FF F5FF 6E5D FF 61 55 10 05 00 2D E9 E0
Sync Ln ???? Seq PAN Dest Src Rad Thr Ail Ele Rud FLM RTH CAv CAh CAT CAP LDG AUX Latitude Longitude ???? Acc ?? Alt Speed Angle Sat ???????? CRC8
5555 32 0000 C4 88F2 5C3E B82A 31 800 800 800 800 800 800 29D 802 888 2AB 0 4 9736181A DF1E240E 0000 6C 42 1A00 0000 7A00 09 00000000 A2
5555 32 0000 C4 88F2 5C3E B82A 31 800 800 800 800 800 800 29D 802 888 2AB 0 4 9636181A E71E240E 0000 6C 42 1A00 0000 7A00 09 00000000 7C
5555 32 0000 C4 88F2 5C3E B82A 31 800 800 800 800 800 800 29D 802 888 2AB 0 4 9636181A EF1E240E 0000 6C 42 1A00 0000 7A00 09 00000000 DE
5555 32 0000 C4 88F2 5C3E B82A 31 800 800 800 800 800 800 29D 802 888 2AB 0 4 9636181A F71E240E 0000 6C 42 1A00 0000 7A00 09 00000000 3F
5555 32 0000 C4 88F2 5C3E B82A 31 800 800 800 800 800 800 29D 802 888 2AB 0 4 9536181A 001F240E 0000 6C 42 1A00 0000 7A00 09 00000000 2D
5555 32 0000 C4 88F2 5C3E B82A 31 800 800 800 800 800 800 29D 802 888 2AB 0 4 9536181A 071F240E 0000 6C 42 1A00 0000 7A00 09 00000000 01
Use fixed font and tabs...
//Alignment/padding
#pragma pack(push, 1)
typedef struct {
uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
uint8_t type; ///< from enum ST24_PACKET_TYPE
uint16_t t; ///< packet counter or clock
int32_t lat; ///< latitude (degrees) +/- 90 deg
int32_t lon; ///< longitude (degrees) +/- 180 deg
int32_t alt; ///< 0.01m resolution, altitude (meters)
int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down
uint8_t nsat; ///< Sum of GPS status and number of satellites.
//GPS status is bit 8 of the byte. Number of satellites is defined using the first 5 bits.
//0b10011111 = GPS Ready, 31 satellites.
//0b00011111 = GPS Acguiring, 31 satellites.
uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
uint8_t current; ///< 0.5A resolution
int16_t roll, pitch, yaw; ///< 0.01 degree resolution
uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail
uint8_t gpsStatus; ///< gps and obs status
/* Example: 0x61
* 0x[X]Y
* 0001 yyyy = gps disabled, obs rdy
* 0010 yyyy = gps acquiring, obs disabled
* 0100 yyyy = gps disabled, obs disabled
* 1000 yyyy = gps disabled, obs disabled
*
* 0xX[Y]
* xxxx 0001 = none
* xxxx 0010 = none
* xxxx 0100 = none
* xxxx 1000 = none
*/
uint8_t obsStatus; ///< obs_avoidance | unknown
/* Example: 0x55
* 0x[X]Y
* 0001 yyyy = obs avoidance fail
* 0010 yyyy = obs avoidance fail
* 0100 yyyy = obs avoidance disabled
* 1000 yyyy = obs avoidance fail
*
* 0xX[Y]
* xxxx 0001 = none
* xxxx 0010 = none
* xxxx 0100 = none
* xxxx 1000 = none
*/
uint8_t flightmode;
uint8_t optionbytes; ///< drone model | flight mode
/* Example: 0x0510
*
* 0x00XY (Bytes 0-1)
*
* 0x[X]Y
* 0001 yyyy = READY
* 0010 yyyy = IPS
* 0100 yyyy = "N/A"
* 1000 yyyy = THR
*
* 0x1[Y]
* 0001 1000 = WATCH
* 0001 0100 = RATE
* 0001 0010 = MAG CALI
* 0001 0001 = NO RC
*
*0xXY00 (Bytes 2-3)
*
* 0x[X]Y
* 0001 yyyy = nothing
* 0010 yyyy = nothing
* 0100 yyyy = nothing
* 1000 yyyy = nothing
*
* 0xX[Y]
* //xxxx 0001 = BATT RED, drone model: hex
* //xxxx 0010 = BATT FULL, drone model: quad
* //xxxx 0100 = BATT RED, drone model: hex
* //xxxx 1000 = BATT RED, drone model: hex
* 0x03xx = drone model: quad, batt red
* 0x05xx = drone model: hex, batt ok <---- Typhoon H (captured)
* 0x06xx = drone model: hex, batt red
* 0x07xx = drone model: hex, batt ok <---- ?
*/
/* Provided by h-elsner
f_mode (decimal) Meaning Display
0 Stability mode (Blue solid) THR
1 Blue flashing - GPS off THR
2 Blue - GPS lost THR
3 Angle mode (Purple solid) Angle
4 Purple flashing - GPS off Angle
5 Angle mode (Purple solid)-GPS lost Angle
6 Smart mode Smart
7 Smart mode - GPS lost Angle
8 Motor starting Start
9 Temperature calibration Temp
10 Pressure calibration Pre Cali
11 Accelerator calibration Acc Cali
12 Emergency/Killed EMER
13 RTH coming Home
14 RTH landing Land
15 Binding Bind
16 Initializing, Ready to start Ready
17 Waiting for RC No RC
18 Magnetometer calibration Mag Cali
19 Unknown mode
20 Agility mode (Rate) Rate
21 Smart mode - Follow me Follow
22 Smart mode - Follow me - GPS lost THR
23 Smart mode - Camera tracking Watch
24 Camera tracking - GPS lost THR
26 Task Curve Cable Cam CCC
27 Task Journey JOUR
28 Task Point of Interest POI
29 Task Orbit ORBIT
32 Indoor Positioning System IPS
33 Waypoints WAYPOINT
*/
uint16_t alarmbytes; ///< unknown | alarms
/* Example: 0x5820
*
* 0x00XY (Bytes 0-1)
*
* 0x[X]Y
* 0001 yyyy = imu temperature warning
* 0010 yyyy = compass warning (captured, uncalibrated compass attached)
* 0100 yyyy = ?
* 1000 yyyy = no fly zone warning
*
* 0xX[Y]
* xxxx 0001 = battery voltage low
* xxxx 0010 = battery voltage critically low
* xxxx 0100 = imu temperature warning (again?)
* xxxx 1000 = ?
*
* 0xXY00 (Bytes 2-3)
*
* 0001 yyyy = ?
* 0010 yyyy = ?
* 0100 yyyy = ?
* 1000 yyyy = ?
*
* xxxx 0001 = ?
* xxxx 0010 = ?
* xxxx 0100 = ?
* xxxx 1000 = ?
*/
} telemPayload;
typedef struct {
uint8_t header1; ///< 0x55 for a valid packet
uint8_t header2; ///< 0x55 for a valid packet
telemPayload payload;
uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
} telemData;
#pragma pack(pop)
/* Send telemetry packet */
int RCInput::send_packet(int uart_fd)
{
RCInput::telemPayload telemetryPayload;
RCInput::telemData telemetryData;
{
RCInput::uorb_update_topics();
if (counter >= 65535) counter = 0;
counter ++;
const battery_status_s &bat = subscription_data->battery_status_sub.get();
const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get();
const vehicle_global_position_s &gpos = subscription_data->vehicle_global_position_sub.get();
const vehicle_status_flags_s &vsflags = subscription_data->vehicle_status_flags_sub.get();
const vehicle_status_s &vs = subscription_data->vehicle_status_sub.get();
const vehicle_attitude_s &att = subscription_data->vehicle_attitude_sub.get();
//const vehicle_air_data_s &air_data = subscription_data->vehicle_air_data_sub.get();
//Disarmed
if (vsflags.condition_global_position_valid == 1 && vsflags.condition_home_position_valid == 1 && vsflags.condition_local_position_valid == 1 && vsflags.condition_local_velocity_valid == 1)
{
gps_status = 0b10000000; //GPS status READY
telemetryPayload.flightmode = 0x10;
}
else
{
gps_status = 0b00000000; //GPS status ACQUIRING
telemetryPayload.flightmode = 0x08;
}
//Armed
if (vs.arming_state == 2)
{
if(vs.nav_state == NAVIGATION_STATE_STAB) telemetryPayload.flightmode = 0x01;
if(vs.nav_state == NAVIGATION_STATE_ALTCTL) telemetryPayload.flightmode = 0x01;
if(vs.nav_state == NAVIGATION_STATE_POSCTL) telemetryPayload.flightmode = 0x03;
if(vs.nav_state == NAVIGATION_STATE_AUTO_MISSION || vs.nav_state == NAVIGATION_STATE_AUTO_LOITER) telemetryPayload.flightmode = 0x21;
if(vs.nav_state == NAVIGATION_STATE_AUTO_RTL) telemetryPayload.flightmode = 0x0d;
if(vs.nav_state == NAVIGATION_STATE_ACRO) telemetryPayload.flightmode = 0x14;
if(vs.nav_state == NAVIGATION_STATE_RATTITUDE) telemetryPayload.flightmode = 0x14;
if(vs.failsafe == true || vs.nav_state == NAVIGATION_STATE_AUTO_RCRECOVER || vs.nav_state == NAVIGATION_STATE_AUTO_RTGS || vs.nav_state == NAVIGATION_STATE_AUTO_LANDENGFAIL || vs.nav_state == NAVIGATION_STATE_AUTO_LANDGPSFAIL) telemetryPayload.flightmode = 0x0c;
}
//Emergency
if (vs.arming_state == 3) telemetryPayload.flightmode = 0x0c;
//Get pitch/roll/yaw values from the vehicle_attitude topic's quaternion
Quatf q(att.q);
Eulerf euler(q);
telemetryPayload.length = 0x26;
telemetryPayload.type = 0x02;
telemetryPayload.t = RCInput::counter;
telemetryPayload.lat = gps.lat;
telemetryPayload.lon = gps.lon;
//telemetryPayload.alt = roundf(frac(air_data.baro_alt_meter) * 100.0f);
telemetryPayload.alt = gps.alt * 0.1f;
telemetryPayload.vx = (int16_t)gpos.vel_n * 100.0f;
telemetryPayload.vy = (int16_t)gpos.vel_e * 100.0f;
telemetryPayload.vz = (int16_t)gpos.vel_d * 100.0f;
telemetryPayload.nsat = gps_status + (uint8_t)(gps.satellites_used);
telemetryPayload.voltage = (uint8_t)roundf((bat.voltage_v-5) * 10.0f);// 0x63; //hex
telemetryPayload.current = 0;
telemetryPayload.roll = (int16_t)euler.phi() * 100.0f;
telemetryPayload.pitch = (int16_t)euler.theta() * 100.0f;
telemetryPayload.yaw = (int16_t)euler.psi() * 100.0f;
telemetryPayload.motorStatus = 0xFF; //OK
telemetryPayload.gpsStatus = 0x61;
telemetryPayload.obsStatus = 0x55;
telemetryPayload.optionbytes = 0x05;
telemetryPayload.alarmbytes = 0x0000;
telemetryData.header1 = 0x55; //Header 1
telemetryData.header2 = 0x55; //Header 2
telemetryData.payload = telemetryPayload;
telemetryData.crc8 = st24_common_crc8((uint8_t*)&telemetryPayload, sizeof(telemetryPayload));
int packet_len = sizeof(telemetryData);
::write(uart_fd, (uint8_t*)&telemetryData, packet_len);
}
return 1;
}
You're invited. Ask there. When something will be made for the public, we'll create an appropriate topic.Could you tell me a bit more about that camera<- ->drone communication?
Wrong. The init phase is a pure RS232 translated request and response. To see the request the best way is to sniff it without the camera mounted. You will see something like this one.At first the CGO3+ expects a certain PWM sequence,
That sounds very strange for me, as the PWM has always been used to control the tilt on the gimbal. Where I could find more information regarding this? How fresh CGO3+'s are we talking about? I think I have one of the oldest ones (with poorly adjusted narrow-fov lens) and the later model with wide and sharp lens. Both of them work with PWM. If that's the case, I need to do some adjustments for the Thunderbird firmware...More than, the last CGO3+ FW in the fresh cameras from China is not PWM tolerant at all.
PWM pin was TX or RX, don't remember exactly. Just attach some analyzer as a Saleae and you will close this question in no time. I used a two-channel UART<>USB because for me the init itself is not important at all. My goal was to listen to both lines and to catch the redundancy too. This is almost done, enough for proper beacon with telemetry and ST16 position.PWM pin that definitely requires a specifically timed init sequence.
We use essential cookies to make this site work, and optional cookies to enhance your experience.