Merge pull request #291 from pascallanger/SPort_Send

New features

.travis.yml

@@ -11,7 +11,7 @@ env:
     - BOARD="multi4in1:avr:multiatmega328p:bootloader=optiboot"
     - BOARD="multi4in1:avr:multixmega32d4"
     - BOARD="multi4in1:STM32F1:multistm32f103c:debug_option=none"
-    - BOARD="multi4in1:STM32F1:multistm32f103c:debug_option=native"
+   # - BOARD="multi4in1:STM32F1:multistm32f103c:debug_option=native"
     - BOARD="multi4in1:STM32F1:multistm32f103c:debug_option=ftdi"
     #
 notifications:

Multiprotocol/AFHDS2A_Rx_a7105.ino

@@ -21,9 +21,6 @@
 #define AFHDS2A_RX_RXPACKET_SIZE	37
 #define AFHDS2A_RX_NUMFREQ			16
 
-static uint8_t afhds2a_rx_data_started;
-static uint8_t afhds2a_rx_disable_lna;
-
 enum {
 	AFHDS2A_RX_BIND1,
 	AFHDS2A_RX_BIND2,
@@ -36,23 +33,22 @@ static void __attribute__((unused)) AFHDS2A_Rx_build_telemetry_packet()
 	uint8_t bitsavailable = 0;
 	uint8_t idx = 0;
 
-	pkt[idx++] = RX_LQI; // 0 - 130
-	pkt[idx++] = RX_RSSI;
-	pkt[idx++] = 0; // start channel
-	pkt[idx++] = 14; // number of channels in packet
+	packet_in[idx++] = RX_LQI; // 0 - 130
+	packet_in[idx++] = RX_RSSI;
+	packet_in[idx++] = 0; // start channel
+	packet_in[idx++] = 14; // number of channels in packet
 	// pack channels
 	for (uint8_t i = 0; i < 14; i++) {
-		uint16_t val = packet[9+i*2] | (packet[10+i*2] << 8);
+		uint32_t val = packet[9+i*2] | (packet[10+i*2] << 8);
 		if (val < 860)
 			val = 860;
-		else if (val > 2140)
-			val = 2140;
-		val -= 860;
+		// convert ppm (860-2140) to Multi (0-2047)
+		val = min(((val-860)<<3)/5, 2047);
 
-		bits |= ((uint32_t)val) << bitsavailable;
+		bits |= val << bitsavailable;
 		bitsavailable += 11;
 		while (bitsavailable >= 8) {
-			pkt[idx++] = bits & 0xff;
+			packet_in[idx++] = bits & 0xff;
 			bits >>= 8;
 			bitsavailable -= 8;
 		}
@@ -71,9 +67,9 @@ uint16_t initAFHDS2A_Rx()
 	A7105_Init();
 	hopping_frequency_no = 0;
 	packet_count = 0;
-	afhds2a_rx_data_started = 0;
-	afhds2a_rx_disable_lna = IS_POWER_FLAG_on;
-	A7105_SetTxRxMode(afhds2a_rx_disable_lna ? TXRX_OFF : RX_EN);
+	rx_data_started = false;
+	rx_disable_lna = IS_POWER_FLAG_on;
+	A7105_SetTxRxMode(rx_disable_lna ? TXRX_OFF : RX_EN);
 	A7105_Strobe(A7105_RX);
 
 	if (IS_BIND_IN_PROGRESS) {
@@ -103,9 +99,9 @@ uint16_t AFHDS2A_Rx_callback()
 #ifndef FORCE_AFHDS2A_TUNING
 	A7105_AdjustLOBaseFreq(1);
 #endif
-	if (afhds2a_rx_disable_lna != IS_POWER_FLAG_on) {
-		afhds2a_rx_disable_lna = IS_POWER_FLAG_on;
-		A7105_SetTxRxMode(afhds2a_rx_disable_lna ? TXRX_OFF : RX_EN);
+	if (rx_disable_lna != IS_POWER_FLAG_on) {
+		rx_disable_lna = IS_POWER_FLAG_on;
+		A7105_SetTxRxMode(rx_disable_lna ? TXRX_OFF : RX_EN);
 	}
 
 	switch(phase) {
@@ -169,11 +165,11 @@ uint16_t AFHDS2A_Rx_callback()
 			if (memcmp(&packet[1], rx_id, 4) == 0 && memcmp(&packet[5], rx_tx_addr, 4) == 0) {
 				if (packet[0] == 0x58 && packet[37] == 0x00 && telemetry_link == 0) { // standard packet, send channels to TX
 					int rssi = min(A7105_ReadReg(A7105_1D_RSSI_THOLD),160);
-					RX_RSSI = map(rssi, 160, 8, 0, 100);
+					RX_RSSI = map16b(rssi, 160, 8, 0, 128);
 					AFHDS2A_Rx_build_telemetry_packet();
 					telemetry_link = 1;
 				}
-				afhds2a_rx_data_started = 1;
+				rx_data_started = true;
 				read_retry = 10; // hop to next channel
 				pps_counter++;
 			}
@@ -194,7 +190,7 @@ uint16_t AFHDS2A_Rx_callback()
 				hopping_frequency_no = 0;
 			A7105_WriteReg(A7105_0F_PLL_I, hopping_frequency[hopping_frequency_no]);
 			A7105_Strobe(A7105_RX);
-			if (afhds2a_rx_data_started)
+			if (rx_data_started)
 				read_retry = 0;
 			else
 				read_retry = -127; // retry longer until first packet is catched

Multiprotocol/AFHDS2A_a7105.ino

@@ -90,14 +90,14 @@ static void AFHDS2A_update_telemetry()
 	#ifdef AFHDS2A_FW_TELEMETRY
 		if (option & 0x80)
 		{// forward 0xAA and 0xAC telemetry to TX, skip rx and tx id to save space
-			pkt[0]= TX_RSSI;
+			packet_in[0]= TX_RSSI;
 			debug("T(%02X)=",packet[0]);
 			for(uint8_t i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
 			{
-				pkt[i-8]=packet[i];
+				packet_in[i-8]=packet[i];
 				debug(" %02X",packet[i]);
 			}
-			pkt[29]=packet[0];	// 0xAA Normal telemetry, 0xAC Extended telemetry
+			packet_in[29]=packet[0];	// 0xAA Normal telemetry, 0xAC Extended telemetry
 			telemetry_link=2;
 			debugln("");
 			return;
@@ -265,11 +265,15 @@ uint16_t ReadAFHDS2A()
 				A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
 				if(packet[0] == 0xbc && packet[9] == 0x01)
 				{
-					uint8_t temp=AFHDS2A_EEPROM_OFFSET+RX_num*4;
+					uint8_t addr;
+					if(RX_num<16)
+						addr=AFHDS2A_EEPROM_OFFSET+RX_num*4;
+					else
+						addr=AFHDS2A_EEPROM_OFFSET2+(RX_num-16)*4;
 					for(uint8_t i=0; i<4; i++)
 					{
 						rx_id[i] = packet[5+i];
-						eeprom_write_byte((EE_ADDR)(temp+i),rx_id[i]);
+						eeprom_write_byte((EE_ADDR)(addr+i),rx_id[i]);
 					}
 					phase = AFHDS2A_BIND4;
 					packet_count++;
@@ -317,6 +321,7 @@ uint16_t ReadAFHDS2A()
 			packet_type = AFHDS2A_PACKET_STICKS;
 			phase = AFHDS2A_DATA;
 		case AFHDS2A_DATA:
+			telemetry_set_input_sync(3850);
 			AFHDS2A_build_packet(packet_type);
 			if((A7105_ReadReg(A7105_00_MODE) & 0x01))		// Check if something has been received...
 				data_rx=0;
@@ -331,7 +336,10 @@ uint16_t ReadAFHDS2A()
 			{
 				#ifdef FAILSAFE_ENABLE
 					if(!(packet_counter % 1569) && IS_FAILSAFE_VALUES_on)
+					{
 						packet_type = AFHDS2A_PACKET_FAILSAFE;
+						FAILSAFE_VALUES_off;
+					}
 					else
 				#endif
 						packet_type = AFHDS2A_PACKET_STICKS;		// todo : check for settings changes
@@ -345,7 +353,7 @@ uint16_t ReadAFHDS2A()
 					if(packet[0] == 0xAA || packet[0] == 0xAC)
 					{
 						if(!memcmp(&packet[1], rx_tx_addr, 4))
-						{ // Validate TX address
+						{ // TX address validated
 							#ifdef AFHDS2A_LQI_CH
 								if(packet[0]==0xAA && packet[9]!=0xFD)
 								{// Normal telemetry packet
@@ -397,14 +405,15 @@ uint16_t initAFHDS2A()
 	{
 		phase = AFHDS2A_DATA_INIT;
 		//Read RX ID from EEPROM based on RX_num, RX_num must be uniq for each RX
-		uint8_t temp=AFHDS2A_EEPROM_OFFSET+RX_num*4;
+		uint8_t addr;
+		if(RX_num<16)
+			addr=AFHDS2A_EEPROM_OFFSET+RX_num*4;
+		else
+			addr=AFHDS2A_EEPROM_OFFSET2+(RX_num-16)*4;
 		for(uint8_t i=0;i<4;i++)
-			rx_id[i]=eeprom_read_byte((EE_ADDR)(temp+i));
+			rx_id[i]=eeprom_read_byte((EE_ADDR)(addr+i));
 	}
 	hopping_frequency_no = 0;
-#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
-	init_frskyd_link_telemetry();
-#endif
 	return 50000;
 }
 #endif

Multiprotocol/ASSAN_nrf24l01.ino

@@ -127,6 +127,7 @@ uint16_t ASSAN_callback()
 			phase=ASSAN_DATA2;
 			return 2000;
 		case ASSAN_DATA2:
+			telemetry_set_input_sync(12000);
 		case ASSAN_DATA3:
 			ASSAN_send_packet();
 			phase++;	// DATA 3 or 4

Multiprotocol/BUGSMINI_nrf24l01.ino

@@ -181,7 +181,7 @@ static void __attribute__((unused)) BUGSMINI_make_address()
     uint8_t start, length, index;
 
 	//read rxid
-	uint8_t base_adr=BUGSMINI_EEPROM_OFFSET+RX_num*2;
+	uint8_t base_adr=BUGSMINI_EEPROM_OFFSET+(RX_num&0x0F)*2;
     uint8_t rxid_high = eeprom_read_byte((EE_ADDR)(base_adr+0));
     uint8_t rxid_low  = eeprom_read_byte((EE_ADDR)(base_adr+1));
 
@@ -272,7 +272,7 @@ uint16_t BUGSMINI_callback()
 			if( NRF24L01_ReadReg(NRF24L01_07_STATUS) & _BV(NRF24L01_07_RX_DR))
 			{ // RX fifo data ready
 				XN297_ReadPayload(packet, BUGSMINI_RX_PAYLOAD_SIZE);
-				base_adr=BUGSMINI_EEPROM_OFFSET+RX_num*2;
+				base_adr=BUGSMINI_EEPROM_OFFSET+(RX_num&0x0F)*2;
 				eeprom_write_byte((EE_ADDR)(base_adr+0),packet[1]);	// Save rxid in EEPROM
 				eeprom_write_byte((EE_ADDR)(base_adr+1),packet[2]);	// Save rxid in EEPROM
 				NRF24L01_SetTxRxMode(TXRX_OFF);
@@ -299,6 +299,7 @@ uint16_t BUGSMINI_callback()
 			phase = BUGSMINI_BIND1;
 			return BUGSMINI_PACKET_INTERVAL - BUGSMINI_WRITE_WAIT;
 		case BUGSMINI_DATA1:
+			telemetry_set_input_sync(BUGSMINI_PACKET_INTERVAL);
 			if( NRF24L01_ReadReg(NRF24L01_07_STATUS) & _BV(NRF24L01_07_RX_DR))
 			{ // RX fifo data ready => read only 12 bytes to not overwrite channel change flag
 				XN297_ReadPayload(packet, 12);
@@ -375,9 +376,6 @@ uint16_t initBUGSMINI()
 	armed = 0;
 	arm_flags = BUGSMINI_FLAG_DISARM;    // initial value from captures
 	arm_channel_previous = BUGSMINI_CH_SW_ARM;
-	#ifdef BUGS_HUB_TELEMETRY
-		init_frskyd_link_telemetry();
-	#endif
 	return BUGSMINI_INITIAL_WAIT;
 }
 

Multiprotocol/Bayang_nrf24l01.ino

@@ -283,7 +283,10 @@ uint16_t BAYANG_callback()
 	if(IS_BIND_DONE)
 	{
 		if(packet_count==0)
+		{
+			telemetry_set_input_sync((option & BAYANG_OPTION_FLAG_TELEMETRY)?5*BAYANG_PACKET_PERIOD:2*BAYANG_PACKET_PERIOD);
 			BAYANG_send_packet(0);
+		}
 		packet_count++;
 		#ifdef BAYANG_HUB_TELEMETRY
 			if (option & BAYANG_OPTION_FLAG_TELEMETRY)
@@ -349,10 +352,6 @@ uint16_t initBAYANG(void)
 	BAYANG_initialize_txid();
 	BAYANG_init();
 	packet_count=0;
-	#ifdef BAYANG_HUB_TELEMETRY
-		init_frskyd_link_telemetry();
-		telemetry_lost=1;	// do not send telemetry to TX right away until we have a TX_RSSI value to prevent warning message...
-	#endif
 	return BAYANG_INITIAL_WAIT+BAYANG_PACKET_PERIOD;
 }
 

Multiprotocol/Bugs_a7105.ino

@@ -284,7 +284,7 @@ static void  __attribute__((unused))BUGS_set_radio_data()
 	{
 		offset=BUGS_NUM_RFCHAN;
 		// Read radio_id from EEPROM
-		uint8_t base_adr=BUGS_EEPROM_OFFSET+RX_num*2;
+		uint8_t base_adr=BUGS_EEPROM_OFFSET+(RX_num&0x0F)*2;
 		uint16_t rxid=0;
 		for(uint8_t i=0; i<2; i++)
 			rxid|=eeprom_read_byte((EE_ADDR)(base_adr+i))<<(i*8);
@@ -374,7 +374,7 @@ uint16_t ReadBUGS(void)
 			BIND_DONE;
 			// set radio_id
 			rxid = (packet[1] << 8) + packet[2];
-			base_adr=BUGS_EEPROM_OFFSET+RX_num*2;
+			base_adr=BUGS_EEPROM_OFFSET+(RX_num&0x0F)*2;
 			for(uint8_t i=0; i<2; i++)
 				eeprom_write_byte((EE_ADDR)(base_adr+i),rxid>>(i*8));	// Save rxid in EEPROM
 			BUGS_set_radio_data();
@@ -385,6 +385,7 @@ uint16_t ReadBUGS(void)
 			break;
 
 		case BUGS_DATA_1:
+			telemetry_set_input_sync(BUGS_PACKET_PERIOD);
 			A7105_SetPower();
 			BUGS_build_packet(0);
 			A7105_WriteReg(A7105_03_FIFOI, BUGS_FIFO_SIZE_TX);
@@ -437,7 +438,7 @@ uint16_t ReadBUGS(void)
 uint16_t initBUGS(void)
 {
 	uint16_t rxid=0;
-	uint8_t base_adr=BUGS_EEPROM_OFFSET+RX_num*2;
+	uint8_t base_adr=BUGS_EEPROM_OFFSET+(RX_num&0x0F)*2;
 	for(uint8_t i=0; i<2; i++)
 		rxid|=eeprom_read_byte((EE_ADDR)(base_adr+i))<<(i*8);
 	if(rxid==0xffff)
@@ -456,9 +457,6 @@ uint16_t initBUGS(void)
 	armed = 0;
 	arm_flags = BUGS_FLAG_DISARM;		// initial value from captures
 	arm_channel_previous = BUGS_CH_SW_ARM;
-	#ifdef BUGS_HUB_TELEMETRY
-		init_frskyd_link_telemetry();
-	#endif
 
 	return 10000;
 }

Multiprotocol/CABELL_nrf224l01.ino

@@ -406,9 +406,10 @@ uint16_t CABELL_callback()
 	if (IS_BIND_DONE)
 	{
 		CABELL_send_packet(0);  // packet_period is set/adjusted in CABELL_send_packet
+		telemetry_set_input_sync(packet_period);
 		return packet_period;
 	}
-	if (bind_counter == 0)
+	else if (bind_counter == 0)
 	{
 		BIND_DONE;
 		CABELL_init();   // non-bind address 
@@ -429,10 +430,6 @@ uint16_t initCABELL(void)
 	else  
 		bind_counter = CABELL_BIND_COUNT;
 	CABELL_init();
-	#if defined CABELL_HUB_TELEMETRY 
-		init_frskyd_link_telemetry();
-		telemetry_lost=1;				// do not send telemetry to TX right away until we have a TX_RSSI value to prevent warning message...
-	#endif
 
 	packet_period = CABELL_PACKET_PERIOD;
 

Multiprotocol/CFlie_nrf24l01.ino

@@ -97,7 +97,7 @@ static inline uint8_t crtp_create_header(uint8_t port, uint8_t channel)
 #define TX_ADDR_SIZE 5
 
 // Timeout for callback in uSec, 10ms=10000us for Crazyflie
-#define PACKET_PERIOD 10000
+#define CFLIE_PACKET_PERIOD 10000
 
 #define MAX_PACKET_SIZE 32  // CRTP is 32 bytes
 
@@ -781,7 +781,8 @@ static uint16_t cflie_callback()
         break;
 
     case CFLIE_DATA:
-        // if (Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_ON_CRTPLOG) {
+        telemetry_set_input_sync(CFLIE_PACKET_PERIOD);
+		// if (Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_ON_CRTPLOG) {
         //     update_telemetry_crtplog();
         // } else if (Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_ON_ACKPKT) {
         //     update_telemetry_ackpkt();
@@ -792,7 +793,7 @@ static uint16_t cflie_callback()
         send_cmd_packet();
         break;
     }
-    return PACKET_PERIOD;                  // Packet at standard protocol interval
+    return CFLIE_PACKET_PERIOD;                  // Packet at standard protocol interval
 }
 
 // Generate address to use from TX id and manufacturer id (STM32 unique id)

Multiprotocol/CG023_nrf24l01.ino

@@ -138,7 +138,10 @@ static void __attribute__((unused)) CG023_init()
 uint16_t CG023_callback()
 {
 	if(IS_BIND_DONE)
+	{
+		telemetry_set_input_sync(packet_period);
 		CG023_send_packet(0);
+	}
 	else
 	{
 		if (bind_counter == 0)

Multiprotocol/CX10_nrf24l01.ino

@@ -229,6 +229,7 @@ uint16_t CX10_callback()
 			}
 			break;
 		case CX10_DATA:
+			telemetry_set_input_sync(packet_period);
 			CX10_Write_Packet(0);
 			break;
 	}