From 73bb9768ea060f6adfa15fe884d4b25f4a6fe095 Mon Sep 17 00:00:00 2001
From: Richard Li <richard.li@ces.hk>
Date: Tue, 7 Apr 2020 15:37:27 +0800
Subject: [PATCH] Added Devo telemetry using Frsky Hub, not a complete
 implementation, mainly for battery voltage feedback.

---
 Multiprotocol/Devo_cyrf6936.ino | 88 ++++++++++++++++++++++++++++++++-
 1 file changed, 87 insertions(+), 1 deletion(-)

diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino
index de0a384..47a2951 100644
--- a/Multiprotocol/Devo_cyrf6936.ino
+++ b/Multiprotocol/Devo_cyrf6936.ino
@@ -23,6 +23,8 @@
 #define DEVO_PKTS_PER_CHANNEL	4
 #define DEVO_BIND_COUNT			0x1388
 
+#define TELEMETRY_ENABLE 0x30
+
 #define DEVO_NUM_WAIT_LOOPS (100 / 5) //each loop is ~5us.  Do not wait more than 100us
 
 enum {
@@ -162,6 +164,21 @@ static void __attribute__((unused)) DEVO_build_data_pkt()
 	DEVO_add_pkt_suffix();
 }
 
+static void __attribute__((unused)) DEVO_parse_telemetry_packet()
+{
+	DEVO_scramble_pkt(); //This will unscramble the packet
+	
+	if (((packet[0] & 0xF0) != TELEMETRY_ENABLE) ||
+		((((uint32_t)packet[15] << 16) | ((uint32_t)packet[14] << 8) | packet[13]) != (MProtocol_id & 0x00ffffff)))
+	{
+		return;
+	}
+	if (packet[0] == TELEMETRY_ENABLE) {
+		v_lipo1 = packet[1] << 1;
+		v_lipo2 = packet[3] << 1;
+	}
+}
+
 static void __attribute__((unused)) DEVO_cyrf_set_bound_sop_code()
 {
 	/* crc == 0 isn't allowed, so use 1 if the math results in 0 */
@@ -270,6 +287,74 @@ static void __attribute__((unused)) DEVO_BuildPacket()
 uint16_t devo_callback()
 {
 	static uint8_t txState=0;
+
+#if defined DEVO_HUB_TELEMETRY  
+	int delay;
+	
+	if (txState == 0) {
+		#ifdef MULTI_SYNC
+			telemetry_set_input_sync(2400);
+		#endif
+	DEVO_BuildPacket();
+	CYRF_WriteDataPacket(packet);
+	txState = 1;
+	return 900;
+	}
+	if (txState == 1) {
+		int i = 0;
+		uint8_t reg;
+		while (! ((reg = CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS)) & 0x02)) {
+			if (++i >= DEVO_NUM_WAIT_LOOPS)
+				break;
+		}
+		if (((reg & 0x22) == 0x20) || (CYRF_ReadRegister(CYRF_02_TX_CTRL) & 0x80)) {
+			CYRF_Reset();
+			DEVO_cyrf_init();
+			DEVO_cyrf_set_bound_sop_code();
+			CYRF_ConfigRFChannel(*hopping_frequency_ptr);
+			//printf("Rst CYRF\n");
+			delay = 1500;
+			txState = 15;
+		} else {
+			if (phase == DEVO_BOUND) {
+				/* exit binding state */
+				phase = DEVO_BOUND_3;
+				DEVO_cyrf_set_bound_sop_code();
+			}
+			if((packet_count != 0) && (bind_counter == 0)) {
+				CYRF_SetTxRxMode(RX_EN); //Receive mode
+				CYRF_WriteRegister(CYRF_05_RX_CTRL, 0x87); //0x80??? //Prepare to receive
+				txState = 2;
+				return 1300;
+			}
+		}
+		if(packet_count == 0) {
+			CYRF_SetPower(0x08);		//Keep tx power updated
+			hopping_frequency_ptr = hopping_frequency_ptr == &hopping_frequency[2] ? hopping_frequency : hopping_frequency_ptr + 1;
+			CYRF_ConfigRFChannel(*hopping_frequency_ptr);
+		}
+		delay = 1500;
+	}
+	if(txState == 2) {
+		uint8_t rx_state = CYRF_ReadRegister(CYRF_07_RX_IRQ_STATUS);
+		if((rx_state & 0x03) == 0x02) {  // RXC=1, RXE=0 then 2nd check is required (debouncing)
+			rx_state |= CYRF_ReadRegister(CYRF_07_RX_IRQ_STATUS);   
+		}
+		if((rx_state & 0x07) == 0x02) { // good data (complete with no errors)
+			CYRF_WriteRegister(CYRF_07_RX_IRQ_STATUS, 0x80);	// need to set RXOW before data read
+			CYRF_ReadDataPacketLen(packet, CYRF_ReadRegister(CYRF_09_RX_COUNT));
+			DEVO_parse_telemetry_packet();
+			RX_RSSI = CYRF_ReadRegister(CYRF_13_RSSI) & 0x1F;
+			RX_RSSI = (RX_RSSI << 1) + RX_RSSI;
+			telemetry_link = 1;     
+		}
+		CYRF_SetTxRxMode(TX_EN); //Write mode
+		delay = 200;
+	}
+	txState = 0;   
+	return delay;
+	
+#else
 	if (txState == 0)
 	{
 		#ifdef MULTI_SYNC
@@ -298,6 +383,7 @@ uint16_t devo_callback()
 		CYRF_ConfigRFChannel(*hopping_frequency_ptr);
 	}
 	return 1200;
+#endif
 }
 
 uint16_t DevoInit()
@@ -320,7 +406,7 @@ uint16_t DevoInit()
 			num_ch=8;
 			break;
 	}
-	DEVO_cyrf_init();
+        DEVO_cyrf_init();
 	CYRF_GetMfgData(cyrfmfg_id);
 	CYRF_SetTxRxMode(TX_EN);
 	CYRF_ConfigCRCSeed(0x0000);