FX/A570: new subprotocol for VTOL QIDI-570

Multiprotocol/FX_nrf24l01.ino

@@ -45,9 +45,12 @@ Multiprotocol is distributed in the hope that it will be useful,
 #define FX_BM26_BIND_CHANNEL		40      
 #define FX_BM26_PACKET_PERIOD		14066
 
+#define FX_A570_PACKET_PERIOD		10270
+
 //#define FORCE_FX620_ID
 //#define FORCE_FX9630_ID
 //#define FORCE_QIDI_ID
+//#define FORCE_FX_A570_ID
 
 enum 
 {
@@ -64,7 +67,7 @@ static void __attribute__((unused)) FX_send_packet()
 	{
 		XN297_Hopping(hopping_frequency_no++);
 		if(sub_protocol >= FX9630)
-		{ // FX9630 & FX_Q560 & FX_QF012
+		{ // FX9630 & FX_Q560 & FX_QF012 & FX_A570
 			if (hopping_frequency_no >= FX9630_NUM_CHANNELS)
 			{
 				hopping_frequency_no = 0;
@@ -73,7 +76,7 @@ static void __attribute__((unused)) FX_send_packet()
 					trim_ch++;
 					trim_ch &= 3;
 				}
-				else // FX_Q560 & FX_QF012
+				else // FX_Q560 & FX_QF012 & FX_A570
 					trim_ch = 0;
 			}
 		}
@@ -87,7 +90,36 @@ static void __attribute__((unused)) FX_send_packet()
 
 	//Channels
 	uint8_t val;
-	if (sub_protocol >= FX9630)
+	if (sub_protocol == FX816 || sub_protocol == FX620)
+	{
+		uint8_t offset=sub_protocol == FX816 ? FX816_CH_OFFSET:FX620_CH_OFFSET;
+		val=convert_channel_8b(AILERON);
+		if(val>127+FX_SWITCH)
+			packet[offset] = sub_protocol == FX816 ? 1:0xFF;
+		else if(val<127-FX_SWITCH)
+			packet[offset] = sub_protocol == FX816 ? 2:0x00;
+		else
+			packet[offset] = sub_protocol == FX816 ? 0:0x7F;
+		packet[offset+1] = convert_channel_16b_limit(THROTTLE,0,100);	//FX816:0x00..0x63, FX620:0x00..0x5E but that should work
+	}
+	else if (sub_protocol == FX_A570)
+	{
+		packet[0] = convert_channel_16b_limit(THROTTLE,0x1C,0xE4);
+		packet[1] = convert_channel_16b_limit(AILERON,0xE4,0x1C);
+		packet[2] = convert_channel_16b_limit(ELEVATOR,0xE4,0x1C);
+		packet[3] = convert_channel_16b_limit(RUDDER,0x1C,0xE4);
+				// Trims - Ruddder L:01 R:02, Aileron L:03 R:04, Elevator Fwd:05 Back:06
+		packet[4]  =  (Channel_data[CH10] < CHANNEL_MIN_COMMAND ? 0x01 : GET_FLAG(CH10_SW, 0x02))  // Rudder trim
+					| (Channel_data[CH11] < CHANNEL_MIN_COMMAND ? 0x03 : GET_FLAG(CH11_SW, 0x04))  // Aileron trim
+					| (Channel_data[CH12] < CHANNEL_MIN_COMMAND ? 0x06 : GET_FLAG(CH12_SW, 0x05)); // Elevator trim
+		packet[5]  =  GET_FLAG(CH5_SW, 0x40)  // motors stop
+				// A570 flight modes = 0>vertical flight(3 axis mode):00, 1>flat flight(6G):02, 2>vertical flight(2 axis mode):04
+					| (Channel_data[CH6] < CHANNEL_MIN_COMMAND ? 0x00 : (Channel_data[CH6] > CHANNEL_MAX_COMMAND ? 0x04 : 0x02))
+					| GET_FLAG(CH7_SW, 0x01)  // 0:low rate, 1:high rate
+					| GET_FLAG(CH8_SW, 0x08)  // LED color change
+					| GET_FLAG(CH9_SW, 0x10); // LED off
+	}
+	else
 	{ // FX9630 & FX_Q560 & FX_QF012 & FX_BM26
 		packet[0] = convert_channel_8b(THROTTLE);
 		packet[1] = convert_channel_8b(AILERON);
@@ -124,18 +156,6 @@ static void __attribute__((unused)) FX_send_packet()
         			  | GET_FLAG( CH9_SW, 0x01);	// Gyro Calibration
     	}  
 	}
-	else // FX816 & FX620
-	{
-		uint8_t offset=sub_protocol == FX816 ? FX816_CH_OFFSET:FX620_CH_OFFSET;
-		val=convert_channel_8b(AILERON);
-		if(val>127+FX_SWITCH)
-			packet[offset] = sub_protocol == FX816 ? 1:0xFF;
-		else if(val<127-FX_SWITCH)
-			packet[offset] = sub_protocol == FX816 ? 2:0x00;
-		else
-			packet[offset] = sub_protocol == FX816 ? 0:0x7F;
-		packet[offset+1] = convert_channel_16b_limit(THROTTLE,0,100);	//FX816:0x00..0x63, FX620:0x00..0x5E but that should work
-	}
 
 	//Bind and specifics
 	if(sub_protocol == FX816)
@@ -162,7 +182,7 @@ static void __attribute__((unused)) FX_send_packet()
 			packet[5] = 0xAB;	// Is it based on ID??
 		}
 	}
-	else // FX9630 & FX_Q560 & FX_QF012 & FX_BM26
+	else // FX9630 & FX_Q560 & FX_QF012 & FX_BM26 & FX_A570 
 	{
 		if(IS_BIND_IN_PROGRESS)
 		{
@@ -202,6 +222,7 @@ static void __attribute__((unused)) FX_send_packet()
 static void __attribute__((unused)) FX_RF_init()
 {
 	XN297_Configure(XN297_CRCEN, XN297_SCRAMBLED, XN297_1M);
+   	packet_length = FX9630_PAYLOAD_SIZE;	// default
 	if(sub_protocol == FX816)
 	{
 		XN297_SetTXAddr((uint8_t *)"\xcc\xcc\xcc\xcc\xcc", 5);
@@ -221,14 +242,18 @@ static void __attribute__((unused)) FX_RF_init()
 		XN297_SetTXAddr((uint8_t *)"\x12\x34\x10\x10", 4); 
     	XN297_RFChannel(FX_BM26_BIND_CHANNEL); 
     	packet_period = FX_BM26_PACKET_PERIOD;
-    	packet_length = FX9630_PAYLOAD_SIZE;
+	}
+	else if(sub_protocol == FX_A570)
+	{
+		XN297_SetTXAddr((uint8_t *)"\x56\x78\x92\x13", 4);
+		XN297_RFChannel(FX9630_BIND_CHANNEL);
+		packet_period = FX_A570_PACKET_PERIOD;
 	}
 	else // FX9630 & FX_Q560 & FX_QF012
 	{
 		XN297_SetTXAddr((uint8_t *)"\x56\x78\x90\x12", 4);
 		XN297_RFChannel(FX9630_BIND_CHANNEL);
 		packet_period = sub_protocol == FX_QF012 ? FX_QF012_PACKET_PERIOD : FX9630_PACKET_PERIOD;
-		packet_length = FX9630_PAYLOAD_SIZE;
 	}
 }
 
@@ -255,10 +280,11 @@ static void __attribute__((unused)) FX_initialize_txid()
 		for(uint8_t i=1;i<FX_NUM_CHANNELS;i++)
 			hopping_frequency[i] = i*10 + hopping_frequency[0];
 	}
-	else // FX9630 & FX_Q560 & FX_QF012 & FX_BM26
+	else // FX9630 & FX_Q560 & FX_QF012 & FX_BM26 & FX_A570
 	{
 		//??? Need to find out how the first RF channel is calculated ???
 		hopping_frequency[0] = sub_protocol == FX_BM26? 0x0C : 0x13;
+		if (sub_protocol == FX_A570) hopping_frequency[0] = 0x0E;
 		//Other 2 RF channels are sent during the bind phase so they can be whatever
 		hopping_frequency[1] = RX_num & 0x0F + 0x1A;
 		hopping_frequency[2] = rx_tx_addr[3] & 0x0F + 0x38;
@@ -275,6 +301,10 @@ static void __attribute__((unused)) FX_initialize_txid()
 			//memcpy(hopping_frequency,"\x0D\x20\x3A", FX9630_NUM_CHANNELS);
 		#endif
 		// BM26 rx_tx_addr: "\x17\x03\x00\x00", hopping_frequency: 0x0C,0x30,0x43(12,48,67)
+		#ifdef FORCE_FX_A570_ID
+			memcpy(rx_tx_addr,(uint8_t*)"\xA9\x56\xFC\x1C", 4);
+			memcpy(hopping_frequency,"\x0E\x1F\x39", FX9630_NUM_CHANNELS);		//Original dump=>14=0x0E,31=0x1F,57=0x39
+		#endif
 	} 
 }
 
@@ -311,7 +341,7 @@ uint16_t FX_callback()
 					}
 				FX_send_packet();
 	#ifdef FX_HUB_TELEMETRY
-				if(sub_protocol < FX9630)
+				if (sub_protocol == FX816 || sub_protocol == FX620 || sub_protocol == FX_A570)
 					break;
 				if(rx)
 				{

Multiprotocol/Multi.txt

@@ -55,7 +55,7 @@
 55,Frsky_RX,Multi,CloneTX,EraseTX,CPPM
 56,AFHDS2A_RX,Multi,CPPM
 57,HoTT,Sync,No_Sync
-58,FX,FX816,FX620,9630,Q560,QF012,BM26,FX818
+58,FX,FX816,FX620,9630,Q560,QF012,BM26,A570
 59,Bayang_RX,Multi,CPPM
 60,Pelikan,Pro,Lite,SCX24
 61,EazyRC

Multiprotocol/Multi_Protos.ino

@@ -188,7 +188,7 @@ const char STR_SUBTYPE_JJRC345[] =    "\x08""JJRC345\0""SkyTmblr";
 const char STR_SUBTYPE_MOULDKG[] =    "\x05""A4444""D4444""A664\0";
 const char STR_SUBTYPE_KF606[] =      "\x06""KF606\0""MIG320""ZCZ50\0";
 const char STR_SUBTYPE_E129[] =       "\x04""E129""C186";
-const char STR_SUBTYPE_FX[] =         "\x05""FX816""FX620""9630\0""Q560\0""QF012""BM26\0""FX818";
+const char STR_SUBTYPE_FX[] =         "\x05""FX816""FX620""9630\0""Q560\0""QF012""BM26\0""A570\0";
 const char STR_SUBTYPE_SGF22[] =      "\x04""F22\0""F22S""J20\0""CX10";
 const char STR_SUBTYPE_JIABAILE[] =   "\x04""Std\0""Gyro";
 #define NO_SUBTYPE		nullptr

Multiprotocol/Multiprotocol.h

@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		3
 #define VERSION_REVISION	4
-#define VERSION_PATCH_LEVEL	55
+#define VERSION_PATCH_LEVEL	56
 
 #define MODE_SERIAL 0
 
@@ -494,7 +494,7 @@ enum FX
 	FX_Q560			= 3,
 	FX_QF012		= 4,
 	FX_BM26			= 5,
-	FX818			= 6,
+	FX_A570			= 6,
 };
 enum SGF22
 {

Multiprotocol/_Config.h

@@ -700,7 +700,7 @@ const PPM_Parameters PPM_prot[14*NBR_BANKS]=	{
 		FX_Q560
 		FX_QF012
 		FX_BM26
-		FX818
+		FX_A570
 	PROTO_FY326
 		FY326
 		FY319