AFHDS2A fix

Multiprotocol/A7105_SPI.ino

@@ -23,20 +23,25 @@ void A7105_WriteData(uint8_t len, uint8_t channel)
 	uint8_t i;
 	A7105_CSN_off;
 	SPI_Write(A7105_RST_WRPTR);
-	SPI_Write(0x05);
+	SPI_Write(A7105_05_FIFO_DATA);
 	for (i = 0; i < len; i++)
 		SPI_Write(packet[i]);
 	A7105_CSN_on;
-	A7105_WriteReg(0x0F, channel);
+	if(protocol!=MODE_FLYSKY)
+	{
+		A7105_Strobe(A7105_STANDBY);
+		A7105_SetTxRxMode(TX_EN);
+	}
+	A7105_WriteReg(A7105_0F_PLL_I, channel);
 	A7105_Strobe(A7105_TX);
 }
 
 void A7105_ReadData(uint8_t len)
 {
 	uint8_t i;
-	A7105_Strobe(0xF0); //A7105_RST_RDPTR
+	A7105_Strobe(A7105_RST_RDPTR);
 	A7105_CSN_off;
-	SPI_Write(0x45);
+	SPI_Write(0x40 | A7105_05_FIFO_DATA);	//bit 6 =1 for reading
 	for (i=0;i<len;i++)
 		packet[i]=SPI_SDI_Read();
 	A7105_CSN_on;
@@ -54,7 +59,7 @@ uint8_t A7105_ReadReg(uint8_t address)
 { 
 	uint8_t result;
 	A7105_CSN_off;
-	SPI_Write(address |=0x40);		//bit 6 =1 for reading
+	SPI_Write(address |=0x40);				//bit 6 =1 for reading
 	result = SPI_SDI_Read();  
 	A7105_CSN_on;
 	return(result); 
@@ -63,20 +68,26 @@ uint8_t A7105_ReadReg(uint8_t address)
 //------------------------
 void A7105_SetTxRxMode(uint8_t mode)
 {
-	if(mode == TX_EN) {
+	if(mode == TX_EN)
+	{
 		A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x33);
 		A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x31);
-	} else if (mode == RX_EN) {
-		A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x31);
-		A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x33);
-	} else {
-		//The A7105 seems to some with a cross-wired power-amp (A7700)
-		//On the XL7105-D03, TX_EN -> RXSW and RX_EN -> TXSW
-		//This means that sleep mode is wired as RX_EN = 1 and TX_EN = 1
-		//If there are other amps in use, we'll need to fix this
-		A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x33);
-		A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x33);
 	}
+	else
+		if (mode == RX_EN)
+		{
+			A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x31);
+			A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x33);
+		}
+		else
+		{
+			//The A7105 seems to some with a cross-wired power-amp (A7700)
+			//On the XL7105-D03, TX_EN -> RXSW and RX_EN -> TXSW
+			//This means that sleep mode is wired as RX_EN = 1 and TX_EN = 1
+			//If there are other amps in use, we'll need to fix this
+			A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x33);
+			A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x33);
+		}
 }
 
 //------------------------
@@ -84,21 +95,22 @@ uint8_t A7105_Reset()
 {
 	uint8_t result;
 	
-	A7105_WriteReg(0x00, 0x00);
+	A7105_WriteReg(A7105_00_MODE, 0x00);
 	delayMilliseconds(1);
-	A7105_SetTxRxMode(TXRX_OFF);		//Set both GPIO as output and low
+	A7105_SetTxRxMode(TXRX_OFF);			//Set both GPIO as output and low
-	result=A7105_ReadReg(0x10) == 0x9E;	//check if is reset.
+	result=A7105_ReadReg(A7105_10_PLL_II) == 0x9E;	//check if is reset.
 	A7105_Strobe(A7105_STANDBY);
 	return result;
 }
 
-void A7105_WriteID(uint32_t ida) {
+void A7105_WriteID(uint32_t ida)
+{
 	A7105_CSN_off;
-	SPI_Write(0x06);//ex id=0x5475c52a ;txid3txid2txid1txid0
+	SPI_Write(A7105_06_ID_DATA);			//ex id=0x5475c52a ;txid3txid2txid1txid0
-	SPI_Write((ida>>24)&0xff);//53 
+	SPI_Write((ida>>24)&0xff);				//53 
-	SPI_Write((ida>>16)&0xff);//75
+	SPI_Write((ida>>16)&0xff);				//75
-	SPI_Write((ida>>8)&0xff);//c5
+	SPI_Write((ida>>8)&0xff);				//c5
-	SPI_Write((ida>>0)&0xff);//2a
+	SPI_Write((ida>>0)&0xff);				//2a
 	A7105_CSN_on;
 }
 
@@ -139,7 +151,7 @@ void A7105_SetPower()
 		power=A7105_RANGE_POWER;
 	if(prev_power != power)
 	{
-		A7105_WriteReg(0x28, power);
+		A7105_WriteReg(A7105_28_TX_TEST, power);
 		prev_power=power;
 	}
 }
@@ -227,13 +239,13 @@ void A7105_Init(void)
 	//VCO Bank Calibrate channel 0
 	A7105_WriteReg(A7105_0F_CHANNEL, 0);
 	A7105_WriteReg(A7105_02_CALC,2);
-	while(A7105_ReadReg(A7105_02_CALC));	// Wait for calibration to end
+	while(A7105_ReadReg(A7105_02_CALC));			// Wait for calibration to end
 //	A7105_ReadReg(A7105_25_VCO_SBCAL_I);
 	
 	//VCO Bank Calibrate channel A0
 	A7105_WriteReg(A7105_0F_CHANNEL, 0xa0);
 	A7105_WriteReg(A7105_02_CALC, 2);
-	while(A7105_ReadReg(A7105_02_CALC));	// Wait for calibration to end
+	while(A7105_ReadReg(A7105_02_CALC));			// Wait for calibration to end
 //	A7105_ReadReg(A7105_25_VCO_SBCAL_I);
 
 	//Reset VCO Band calibration

Multiprotocol/AFHDS2A_a7105.ino

@@ -205,9 +205,9 @@ static void AFHDS2A_build_packet(uint8_t type)
 
 #define AFHDS2A_WAIT_WRITE 0x80
 #ifdef STM32_BOARD
-	#define AFHDS2A_DELAY 0
+	#define AFHDS2A_TIMING_OFFSET 0
 #else
-	#define AFHDS2A_DELAY 700
+	#define AFHDS2A_TIMING_OFFSET 100
 #endif
 uint16_t ReadAFHDS2A()
 {
@@ -219,29 +219,28 @@ uint16_t ReadAFHDS2A()
 		case AFHDS2A_BIND1:
 		case AFHDS2A_BIND2:
 		case AFHDS2A_BIND3:
-			A7105_Strobe(A7105_STANDBY);
-			A7105_SetTxRxMode(TX_EN);
 			AFHDS2A_build_bind_packet();
 			A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
-			if(A7105_ReadReg(A7105_00_MODE) == 0x1b)
+			if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)))
-			{ // todo: replace with check crc+fec
+			{ // FECF+CRCF Ok
-				A7105_Strobe(A7105_RST_RDPTR);
 				A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
-				if(packet[0] == 0xbc)
+				if(packet[0] == 0xbc && packet[9] == 0x01)
 				{
 					uint8_t temp=50+RX_num*4;
-					for(uint8_t i=0; i<4; i++)
+					uint8_t i;
+					for(i=0; i<4; i++)
 					{
 						rx_id[i] = packet[5+i];
 						eeprom_write_byte((EE_ADDR)(temp+i),rx_id[i]);
 					}
-					if(packet[9] == 0x01)
+					phase = AFHDS2A_BIND4;
-						phase = AFHDS2A_BIND4;
+					packet_count++;
+					return 3850;
 				}
 			}
 			packet_count++;
 			phase |= AFHDS2A_WAIT_WRITE;
-			return 1700+AFHDS2A_DELAY;
+			return 1700+AFHDS2A_TIMING_OFFSET;
 		case AFHDS2A_BIND1|AFHDS2A_WAIT_WRITE:
 		case AFHDS2A_BIND2|AFHDS2A_WAIT_WRITE:
 		case AFHDS2A_BIND3|AFHDS2A_WAIT_WRITE:
@@ -251,10 +250,8 @@ uint16_t ReadAFHDS2A()
 			phase++;
 			if(phase > AFHDS2A_BIND3)
 				phase = AFHDS2A_BIND1;
-			return 2150-AFHDS2A_DELAY;
+			return 2150-AFHDS2A_TIMING_OFFSET;
 		case AFHDS2A_BIND4:
-			A7105_SetTxRxMode(TX_EN);
-			A7105_Strobe(A7105_STANDBY);
 			AFHDS2A_build_bind_packet();
 			A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
 			packet_count++;
@@ -267,16 +264,8 @@ uint16_t ReadAFHDS2A()
 				phase = AFHDS2A_DATA;
 				BIND_DONE;
 			}                        
-			phase |= AFHDS2A_WAIT_WRITE;
+			return 3850;
-			return 1700+AFHDS2A_DELAY;
-		case AFHDS2A_BIND4|AFHDS2A_WAIT_WRITE:
-			A7105_SetTxRxMode(RX_EN);
-			A7105_Strobe(A7105_RX);
-			phase &= ~AFHDS2A_WAIT_WRITE;
-			return 2150-AFHDS2A_DELAY;
 		case AFHDS2A_DATA:    
-			A7105_SetTxRxMode(TX_EN);
-			A7105_Strobe(A7105_STANDBY);
 			AFHDS2A_build_packet(packet_type);
 			A7105_WriteData(AFHDS2A_TXPACKET_SIZE, hopping_frequency[hopping_frequency_no++]);
 			if(hopping_frequency_no >= AFHDS2A_NUMFREQ)
@@ -287,14 +276,8 @@ uint16_t ReadAFHDS2A()
 				packet_type = AFHDS2A_PACKET_FAILSAFE;
 			else
 				packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
-			// got some data from RX ?
-			// we've no way to know if RX fifo has been filled
-			// as we can't poll GIO1 or GIO2 to check WTR
-			// we can't check A7105_MASK_TREN either as we know
-			// it's currently in transmit mode.
 			if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)))
 			{ // FECF+CRCF Ok
-				A7105_Strobe(A7105_RST_RDPTR);
 				A7105_ReadData(1);
 				if(packet[0] == 0xaa)
 				{
@@ -310,12 +293,12 @@ uint16_t ReadAFHDS2A()
 			}
 			packet_counter++;
 			phase |= AFHDS2A_WAIT_WRITE;
-			return 1700+AFHDS2A_DELAY;
+			return 1700+AFHDS2A_TIMING_OFFSET;
 		case AFHDS2A_DATA|AFHDS2A_WAIT_WRITE:
 			A7105_SetTxRxMode(RX_EN);
-			phase &= ~AFHDS2A_WAIT_WRITE;
 			A7105_Strobe(A7105_RX);
-			return 2150-AFHDS2A_DELAY;
+			phase &= ~AFHDS2A_WAIT_WRITE;
+			return 2150-AFHDS2A_TIMING_OFFSET;
 	}
 	return 3850; // never reached, please the compiler
 }

Multiprotocol/Hubsan_a7105.ino

@@ -218,7 +218,6 @@ uint16_t ReadHubsan()
 		case BIND_5:
 		case BIND_7:
 			hubsan_build_bind_packet(phase == BIND_7 ? 9 : (phase == BIND_5 ? 1 : phase + 1 - BIND_1));
-			A7105_Strobe(A7105_STANDBY);
 			A7105_WriteData(16, channel);
 			phase |= WAIT_WRITE;
 			return 3000;
@@ -286,7 +285,6 @@ uint16_t ReadHubsan()
 				if( phase == DATA_1)
 						A7105_SetPower(); //Keep transmit power in sync
 				hubsan_build_packet();
-				A7105_Strobe(A7105_STANDBY);
 				A7105_WriteData(16, phase == DATA_5 && id_data == ID_NORMAL ? channel + 0x23 : channel);
 				if (phase == DATA_5)
 					phase = DATA_1;

Multiprotocol/Telemetry.ino

@@ -93,11 +93,11 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 		for (uint8_t i=3;i<len;i++)
 			pktt[i]=pkt[i];				 
 		telemetry_link=1;
-		telemetry_lost=0;
 		if(pktt[6])
 			telemetry_counter=(telemetry_counter+1)%32;
 		//
 #if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO
+		telemetry_lost=0;
 		if (protocol==MODE_FRSKYX)
 		{
 			if ((pktt[5] >> 4 & 0x0f) == 0x08)
@@ -134,8 +134,8 @@ void frsky_link_frame()
 		{	
 			frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V
 			frame[2] = frame[1];			
-			frame[3] = 0x00;
+			frame[3] = protocol==MODE_HUBSAN?0x00:(uint8_t)RSSI_dBm;
-			frame[4] = (uint8_t)RSSI_dBm;
+			frame[4] = protocol==MODE_HUBSAN?(uint8_t)RSSI_dBm:0x00;
 		}
 	frame[5] = frame[6] = frame[7] = frame[8] = 0;			
 	frskySendStuffed();