/* This code reads the serial output from a Yuneec SR-24 receiver (like what is used in the Yuneec Typhoon H) and converts it to a SBUS output signal that can be read by most flight controllers. It also has a serial output for the CGO3+ camera.
* It was written to be used on an ESP8266 and tested on a Wemos D1 R2 ESP8266 board. The ESP8266 was chosen for its small size and high clock speed. Some modification and testing would
* be required for other microcontrollers.
*
*
* This code relies on the st24.h and st24.cpp files which need to be placed in a subfolder of your Arduino libraries folder. The st24.h and st24.cpp files were taken from the git hub for the
* PX4 firmware. Link:
* https://github.com/PX4/Firmware/tree/master/src/lib/rc
* I also modified the st24.cpp to change a couple of defines (below) to scale the ouput to servo values
* #define ST24_TARGET_MIN 1000.0f
* #define ST24_TARGET_MAX 2000.0f
*
* The Checksum.h is from here:
* https://github.com/mavlink/c_library_v1
*
* Is uses the ESP32SERVO library downloaded from the Arduino Library manager.
*
* The bindmode function works and should only be called if the BINPIN is shorted to ground on powerup
*
*
*
* The quality of this code is suspect. I have no way of knowing if it is safe or reliable. Extra care should be taken to avoid injury or damage to property. USE AT YOUR OWN RISK. Be safe!
*
*/
#include <EEPROM.h>
#include <Ticker.h>
#include <HardwareSerial.h>
#include <SoftwareSerial.h>
#include <WiFi.h>
#include <ESP32Servo.h>
#include <string.h> // it is needed for subString function
#include "checksum.h"
#include <st24.h>
#define BINDPIN 27
#define CPU_MHZ 80
#define NUM_INPUT_CHAN 12 //The number of channels to read from the receiver
#define NUM_SBUS_OUT_CHAN 16 //The number of channels to output in
#define FAILSAFE_COUNT 100
#define BAUD_RATE 115200 //For Yuneec Serial
#define AIL 1 //Channel order from the receiver
#define ELE 2
#define THR 0
#define RUD 3
#define AUX1 6 //Camera tilt slider
#define AUX2 7 //Pan knob
#define TILTMODESW 8 // tilt mode switch
#define AUX3 9 //Pan mode switch
#define AUX4 10 // Landing Gear switch
#define AUXBUTTON 11 // Aux button
#define MAX_STRING_LEN 80
#define YUN_RX_PIN 14 //Set pin number For Yuneec Serial. yellow
#define YUN_TX_PIN 33 //Set pin number For Yuneec Serial. grey
#define CGO3_TX_PIN 12 //orange
#define CGO3_RX_PIN 32 // green
//#define SBUS_RX_PIN 16 // NC
//#define SBUS_TX_PIN 17 // orange
#define chan1pin 16
#define chan2pin 17
//#define CHAN_SCALING 0.5
//#define SBUS_REFRESH 20000
#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
char serialbuf[80]; //This gives the incoming serial some room
// This is the array of values taht need to be sent to the Yuneec SR24 receiver to initiate bind mode.
const byte BINDARR[55] = {0x55, 0x55, 0x08, 0x04, 0x00, 0x00, 0x42, 0x49, 0x4E, 0x44, 0xB0, 0x55, 0x55, 0x08, 0x04, 0x00, 0x00, 0x42, 0x49, 0x4E, 0x44, 0xB0, 0x55, 0x55,
0x08, 0x04, 0x00, 0x00, 0x42, 0x49, 0x4E, 0x44, 0xB0, 0x55, 0x55, 0x08, 0x04, 0x00, 0x00, 0x42, 0x49, 0x4E, 0x44, 0xB0, 0x55, 0x55, 0x08, 0x04
, 0x00, 0x00, 0x42, 0x49, 0x4E, 0x44, 0xB0
};
int sbusOutOrder[]={AIL,ELE,THR,RUD,AUX4,AUXBUTTON}; // Order to output the channels in.
int cgo3OutOrder[]={AUX1,AUX2};
unsigned int sbusOutDefault[]={1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,0,0,0};
#define SBUS_OUT_ORDER_SIZE sizeof(sbusOutOrder)/sizeof(sbusOutOrder[0])
uint16_t SBUS_Channel_Data[16];
bool SBUS_Failsafe_Active;
bool SBUS_Lost_Frame;
#define CGO3_SCALE_FACTOR 1.8 //
#define CGO3_SCALE_OFFSET 200
uint8_t packcount;
volatile unsigned int outsincelastpack;
volatile bool SBUSready;
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;
#define MAX_CHAN_COUNT sizeof(tempchan)/sizeof(tempchan[0])
HardwareSerial YuneecSerial(1);
//SoftwareSerial CGO3Serial(CGO3_RX_PIN,CGO3_TX_PIN, false);
Servo chan1;
Servo chan2;
void bindmode() {
for (int i = 0; i < 55; i = i + 1) {
YuneecSerial.write(BINDARR[i]);
}
while (1) {
delay(100);
}
}
//void updateCGO3msg(uint8_t* msgptr, uint16_t slider1, uint16_t knob1,uint16_t switch1,uint16_t switch2) {
// uint8_t c;
// uint16_t tempchecksum;
// uint16_t tempword;
//
// msgptr[PACKET_SEQ_LOC]=packet_seq;
// if (msgptr[1]==0x1A) {
// tempword = (uint16_t)(slider1*CGO3_SCALE_FACTOR + .5f) + CGO3_SCALE_OFFSET;
// msgptr[SL1_LOC_BYTE1] = lowByte(tempword);
// msgptr[SL1_LOC_BYTE1+1] = highByte(tempword);
// tempword = (uint16_t)(knob1*CGO3_SCALE_FACTOR + .5f) + CGO3_SCALE_OFFSET;
// msgptr[K1_LOC_BYTE1]=lowByte(tempword);
// msgptr[K1_LOC_BYTE1+1]=highByte(tempword);
// tempword = (uint16_t)(switch1*CGO3_SCALE_FACTOR + .5f) + CGO3_SCALE_OFFSET;
// msgptr[S1_LOC_BYTE1]=lowByte(tempword);
// msgptr[S1_LOC_BYTE1+1]=highByte(tempword);
// tempword = (uint16_t)(switch2*CGO3_SCALE_FACTOR + .5f) + CGO3_SCALE_OFFSET;
// msgptr[S2_LOC_BYTE1]=lowByte(tempword);
// msgptr[S2_LOC_BYTE1+1]=highByte(tempword);
// }
// 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;
// }
//}
void setup() {
// WIFI is turned off to reduce the chances of interference with the 2.4GHz Yuneec receiver.
WiFi.mode( WIFI_MODE_NULL );
Serial.begin(BAUD_RATE);
YuneecSerial.begin(BAUD_RATE,SERIAL_8N1,YUN_RX_PIN,YUN_TX_PIN );
//CGO3Serial.begin(BAUD_RATE);
//Serial.println("Yuneec Serial Started.");
pinMode(BINDPIN, INPUT_PULLUP);
delay(100);
if (digitalRead(BINDPIN) == LOW) {
bindmode();
}
chan1.setPeriodHertz(50);
chan2.setPeriodHertz(50);
chan1.attach(chan1pin,1000 ,2000);
chan2.attach(chan2pin,1000,2000);
packcount=0;
outsincelastpack=0;
//initial SBUS values
for (int i = 0; i < NUM_SBUS_OUT_CHAN; i = i + 1) {
SBUS_Channel_Data[i]=sbusOutDefault[i];
}
delay(500);
// while (CGO3Serial.available()<1) {
// for (int i=0;i<5;i=i+1) {
// updateCGO3msg(initpacket, 0, 0,0, 0 );
// CGO3Serial.write(initpacket,13);
// }
// }
}
void loop() {
byte tempbyte;
//uint16_t tempword;
uint16_t tempchan[NUM_INPUT_CHAN];
uint8_t current;
uint8_t rssi;
uint16_t tempchannelcount;
if (YuneecSerial.available() > 0) {
tempbyte=YuneecSerial.read();
if(!st24_decode(tempbyte,&rssi,&packcount,&tempchannelcount,tempchan,NUM_INPUT_CHAN)) {
for (int i=0;i<SBUS_OUT_ORDER_SIZE;i=i+1) {
SBUS_Channel_Data[i]=tempchan[sbusOutOrder[i]];
}
chan1.write(SBUS_Channel_Data[0]);
chan2.write(SBUS_Channel_Data[1]);
Serial.print(SBUS_Channel_Data[0]);
Serial.print(" ,");
Serial.println(SBUS_Channel_Data[1]);
outsincelastpack=packcount/10;
//updateCGO3msg(outputmsg, tempchan[AUX1], tempchan[AUX2], tempchan[TILTMODESW], tempchan[AUX3]);
//CGO3Serial.write(outputmsg,outputmsg[1]+10);
}
}
// Packcount is the number of packets that the receiver has output since it received a new packet from the TX. It is a byte so it is < 255. I divide that by 5 and if that value is >FAILSAFE_COUNT it failsafes. The outsincelastpacket is also incremented
// each time the PPM isr runs, so if there is no serial input at all it will failsafe after FAILSAFE_COUNT PPM outputs.
if (outsincelastpack>FAILSAFE_COUNT) {
for (int i = 0; i < NUM_SBUS_OUT_CHAN; i = i + 1) {
SBUS_Channel_Data[i]=sbusOutDefault[i];
SBUS_Failsafe_Active = 1;
chan1.write(SBUS_Channel_Data[0]);
chan2.write(SBUS_Channel_Data[1]);
}
}
}