From 059c6baa4efa50ee5324d21edd705e4a16f16251 Mon Sep 17 00:00:00 2001
From: pascallanger <planger@mathworks.com>
Date: Fri, 26 Aug 2016 16:18:50 +0200
Subject: [PATCH] FrSkyX/SFHSS rfcal & second channel ASSAN

---
 Multiprotocol/ASSAN_nrf24l01.ino | 14 +++++++---
 Multiprotocol/FrSkyX_cc2500.ino  | 44 ++++++++++++--------------------
 Multiprotocol/Multiprotocol.ino  |  6 ++---
 Multiprotocol/SFHSS_cc2500.ino   |  4 +--
 4 files changed, 32 insertions(+), 36 deletions(-)

diff --git a/Multiprotocol/ASSAN_nrf24l01.ino b/Multiprotocol/ASSAN_nrf24l01.ino
index 3356e2f..3add45a 100644
--- a/Multiprotocol/ASSAN_nrf24l01.ino
+++ b/Multiprotocol/ASSAN_nrf24l01.ino
@@ -140,7 +140,7 @@ static void __attribute__((unused)) ASSAN_initialize_txid()
 	packet[21]=0xFA;
 	packet[20]=0x53; */
 	// Using packet[20..23] to store the ID1 and packet[24..27] to store the ID2
-	uint8_t freq=0;
+	uint8_t freq=0,freq2;
 	for(uint8_t i=0;i<4;i++)
 	{
 		uint8_t temp=rx_tx_addr[0];
@@ -153,9 +153,15 @@ static void __attribute__((unused)) ASSAN_initialize_txid()
 	freq=((freq%25)+2)<<1;
 	if(freq&0x02)	freq|=0x01;
 	hopping_frequency[0]=freq;
-	// Alternate frequency
-	hopping_frequency[1]=freq*2-6;
-	hopping_frequency[1]+=analogRead(A6)%12;		// Add some random to the second channel
+	// Alternate frequency has some random
+	do
+	{
+		randomSeed((uint32_t)analogRead(A6) << 10 | analogRead(A7));
+		freq2=random(0xfefefefe)%9;
+		freq2+=freq*2-5;
+	}
+	while( (freq2>118) || (freq2<freq+1) || (freq2==2*freq) );
+	hopping_frequency[1]=freq2;			// Add some random to the second channel
 }
 
 uint16_t initASSAN()
diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino
index 452f23f..ba5dfd5 100644
--- a/Multiprotocol/FrSkyX_cc2500.ino
+++ b/Multiprotocol/FrSkyX_cc2500.ino
@@ -21,10 +21,8 @@
 #include "iface_cc2500.h"
 
 uint8_t chanskip;
-uint8_t channr;
 uint8_t counter_rst;
 uint8_t ctr;
-uint8_t FS_flag=0;
 uint8_t seq_last_sent;
 uint8_t seq_last_rcvd;
 
@@ -49,16 +47,12 @@ static uint8_t __attribute__((unused)) hop(uint8_t byte)
 static void __attribute__((unused)) set_start(uint8_t ch )
 {
 	CC2500_Strobe(CC2500_SIDLE);
-	CC2500_WriteReg(CC2500_23_FSCAL3, calData[ch][0]);
-	CC2500_WriteReg(CC2500_24_FSCAL2, calData[ch][1]);
-	CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch][2]);
-	CC2500_WriteReg(CC2500_0A_CHANNR, ch==47? 0:pgm_read_word(&hop_data[ch]));
+	CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch]);
+	CC2500_WriteReg(CC2500_0A_CHANNR, ch==47? 0:hop(ch));
 }		
 
 static void __attribute__((unused)) frskyX_init()
 {
-	CC2500_Reset();
-	
 	for(uint8_t i=0;i<36;i++)
 	{
 		uint8_t reg=pgm_read_byte_near(&cc2500_conf[i][0]);
@@ -96,20 +90,16 @@ static void __attribute__((unused)) frskyX_init()
 	for(uint8_t c=0;c < 47;c++)
 	{//calibrate hop channels
 		CC2500_Strobe(CC2500_SIDLE);    
-		CC2500_WriteReg(CC2500_0A_CHANNR,pgm_read_word(&hop_data[c]));
+		CC2500_WriteReg(CC2500_0A_CHANNR,hop(c));
 		CC2500_Strobe(CC2500_SCAL);
 		delayMicroseconds(900);//
-		calData[c][0] = CC2500_ReadReg(CC2500_23_FSCAL3);
-		calData[c][1] = CC2500_ReadReg(CC2500_24_FSCAL2);	
-		calData[c][2] = CC2500_ReadReg(CC2500_25_FSCAL1);
+		calData[c] = CC2500_ReadReg(CC2500_25_FSCAL1);
 	}
 	CC2500_Strobe(CC2500_SIDLE);    	
 	CC2500_WriteReg(CC2500_0A_CHANNR,0x00);
 	CC2500_Strobe(CC2500_SCAL);
 	delayMicroseconds(900);
-	calData[47][0] = CC2500_ReadReg(CC2500_23_FSCAL3);
-	calData[47][1] = CC2500_ReadReg(CC2500_24_FSCAL2);	
-	calData[47][2] = CC2500_ReadReg(CC2500_25_FSCAL1);
+	calData[47] = CC2500_ReadReg(CC2500_25_FSCAL1);
 	//#######END INIT########		
 }
 
@@ -158,11 +148,11 @@ static void __attribute__((unused)) frskyX_build_bind_packet()
 	packet[4] = rx_tx_addr[2];
 	int idx = ((state -FRSKY_BIND) % 10) * 5;
 	packet[5] = idx;	
-	packet[6] = pgm_read_word(&hop_data[idx++]);
-	packet[7] = pgm_read_word(&hop_data[idx++]);
-	packet[8] = pgm_read_word(&hop_data[idx++]);
-	packet[9] = pgm_read_word(&hop_data[idx++]);
-	packet[10] = pgm_read_word(&hop_data[idx++]);
+	packet[6] = hop(idx++);
+	packet[7] = hop(idx++);
+	packet[8] = hop(idx++);
+	packet[9] = hop(idx++);
+	packet[10] = hop(idx++);
 	packet[11] = 0x02;
 	packet[12] = RX_num;
 	//
@@ -188,13 +178,13 @@ static void __attribute__((unused)) frskyX_data_frame()
 	packet[2] = rx_tx_addr[2];
 	packet[3] = 0x02;
 	//  
-	packet[4] = (ctr<<6)+channr; 
+	packet[4] = (ctr<<6)+hopping_frequency_no; 
 	packet[5] = counter_rst;
 	packet[6] = RX_num;
-	//FLAGS 00 - standard packet
+	//packet[7] = FLAGS 00 - standard packet
 	//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
 	//20 - range check packet
-	packet[7] = FS_flag;
+	packet[7] = 0;
 	packet[8] = 0;		
 	//
 	if ( lpass & 1 )
@@ -252,7 +242,7 @@ uint16_t ReadFrSkyX()
 			return 9000;
 		case FRSKY_BIND_DONE:
 			initialize_data(0);
-			channr=0;
+			hopping_frequency_no=0;
 			BIND_DONE;
 			state++;			
 			break;		
@@ -264,10 +254,10 @@ uint16_t ReadFrSkyX()
 			}
 			LED_ON;
 			CC2500_SetTxRxMode(TX_EN);
-			set_start(channr);
+			set_start(hopping_frequency_no);
 			CC2500_SetPower();		
 			CC2500_Strobe(CC2500_SFRX);
-			channr = (channr+chanskip)%47;
+			hopping_frequency_no = (hopping_frequency_no+chanskip)%47;
 			CC2500_Strobe(CC2500_SIDLE);		
 			CC2500_WriteData(packet, packet[0]+1);
 			//
@@ -319,7 +309,7 @@ uint16_t initFrSkyX()
 		chanskip=random(0xfefefefe)%47;
 	}
 	while((chanskip-ctr)%4)
-	ctr=(ctr+1)%4;
+		ctr=(ctr+1)%4;
 	
 	counter_rst=(chanskip-ctr)>>2;
 	//for test***************
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index eb9e605..4124685 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -71,13 +71,13 @@ uint8_t  len;
 uint8_t  RX_num;
 
 #if defined(FRSKYX_CC2500_INO) || defined(SFHSS_CC2500_INO)
-uint8_t calData[48][3];
+uint8_t calData[48];
 #endif
 
 //Channel mapping for protocols
 const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
-//const uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
-//const uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
+const uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
+const uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
 const uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
 
 // Mode_select variables
diff --git a/Multiprotocol/SFHSS_cc2500.ino b/Multiprotocol/SFHSS_cc2500.ino
index 47537ce..a9b793a 100644
--- a/Multiprotocol/SFHSS_cc2500.ino
+++ b/Multiprotocol/SFHSS_cc2500.ino
@@ -98,7 +98,7 @@ static void __attribute__((unused)) SFHSS_tune_chan_fast()
 {
 	CC2500_Strobe(CC2500_SIDLE);
 	CC2500_WriteReg(CC2500_0A_CHANNR, rf_ch_num*6+16);
-	CC2500_WriteRegisterMulti(CC2500_23_FSCAL3, calData[rf_ch_num], 3);
+	CC2500_WriteReg(CC2500_25_FSCAL1, calData[rf_ch_num]);
 }
 
 static void __attribute__((unused)) SFHSS_tune_freq()
@@ -170,7 +170,7 @@ uint16_t ReadSFHSS()
 			phase = SFHSS_CAL;
 			return 2000;
 		case SFHSS_CAL:
-			CC2500_ReadRegisterMulti(CC2500_23_FSCAL3, calData[rf_ch_num], 3);
+			calData[rf_ch_num]=CC2500_ReadReg(CC2500_25_FSCAL1);
 			if (++rf_ch_num < 30)
 				SFHSS_tune_chan();
 			else