Update to last Pascal mod.

Multiprotocol/Multiprotocol.ino

@@ -21,7 +21,7 @@
 	along with Multiprotocol.  If not, see <http://www.gnu.org/licenses/>.
 */
 #define STM32_board
-
+#undef __cplusplus
 #if defined STM32_board
 #include "Multiprotocol_STM32.h"
 #include <EEPROM.h>
@@ -258,16 +258,6 @@ void setup()
 	if( IS_BIND_BUTTON_on )
 	BIND_BUTTON_FLAG_on;	// If bind button pressed save the status for protocol id reset under hubsan
 
-/*	#ifdef XMEGA
-		if( (PORTD.IN & _BV(2)) == 0x00 )
-		#else
-		# if defined STM32_board
-			if(digitalRead(BIND_pin)==0x00)
-			#else
-			if( (PINB & _BV(5)) == 0x00 )
-		#endif
-	#endif
-	*/
 	// Read status of mode select binary switch
 	// after this mode_select will be one of {0000, 0001, ..., 1111}
 	#ifdef XMEGA
@@ -312,8 +302,6 @@ void setup()
 		servo_max_100=PPM_MAX_100; servo_min_100=PPM_MIN_100;
 		servo_max_125=PPM_MAX_125; servo_min_125=PPM_MIN_125;
 		
-
-		
 		protocol_init();
 		
 		#ifndef XMEGA
@@ -348,12 +336,140 @@ void setup()
 
 // Main
 void loop()
+{
+	uint16_t next_callback,diff=0xFFFF;
+	
+	while(1)
+	{
+		if(remote_callback==0 || diff>2*200)
+		{
+			do
+			{
+				Update_All();
+			}
+			while(remote_callback==0);
+		}
+		#ifdef XMEGA		
+		if( (TCC1.INTFLAGS & TC1_CCAIF_bm) != 0)
+		{
+			cli();					// disable global int
+			TCC1.CCA = TCC1.CNT ;	// Callback should already have been called... Use "now" as new sync point.
+			sei();					// enable global int
+		}
+		else
+		while((TCC1.INTFLAGS & TC1_CCAIF_bm) == 0); // wait before callback
+		#else
+		#if defined STM32_board
+		if((TIMER2_BASE->SR & TIMER_SR_CC1IF)!=0){
+			cli();
+			OCR1A = TIMER2_BASE->CNT;
+			TIMER2_BASE->CCR1=OCR1A; 
+			sei();
+		}
+		else
+		while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0);//walit till compare match	
+		#else
+		if( (TIFR1 & (1<<OCF1A)) != 0)
+		{
+			cli();			// disable global int
+			OCR1A=TCNT1;	// Callback should already have been called... Use "now" as new sync point.
+			sei();			// enable global int
+		}
+		else
+		while((TIFR1 & (1<<OCF1A)) == 0); // wait before callback
+		#endif		
+		#endif
+		do
+		{
+			#ifndef STM32_board			
+			TX_ON;
+			TX_MAIN_PAUSE_on;
+			tx_pause();
+			#endif
+			next_callback=remote_callback();
+			#ifndef STM32_board	
+			TX_MAIN_PAUSE_off;
+			tx_resume();
+			TX_OFF;
+			#endif		   
+			while(next_callback>4000)
+			{ 										// start to wait here as much as we can...
+				next_callback-=2000;
+				
+				#ifdef XMEGA
+				cli();								// disable global int
+				TCC1.CCA +=2000*2;					// set compare A for callback
+				TCC1.INTFLAGS = TC1_CCAIF_bm ;		// clear compare A=callback flag
+				sei();								// enable global int
+				Update_All();
+				if(IS_CHANGE_PROTOCOL_FLAG_on)
+				break; // Protocol has been changed
+				while((TCC1.INTFLAGS & TC1_CCAIF_bm) == 0); // wait 2ms...
+				#else
+				
+				#if defined STM32_board	
+				cli();
+				OCR1A+=2000*2;// clear compare A=callback flag 
+				TIMER2_BASE->CCR1=OCR1A;
+				TCNT1 = TIMER2_BASE->CNT; 
+				TIMER2_BASE->SR &= ~TIMER_SR_CC1IF;	//clear compare Flag
+				sei();
+				Update_All();
+				if(IS_CHANGE_PROTOCOL_FLAG_on)
+				break; // Protocol has been changed
+				while((TIMER2_BASE->SR &TIMER_SR_CC1IF)==0);//2ms wait
+				#else
+				cli();
+				OCR1A+=2000*2;	// clear compare A=callback flag 
+				TIFR1=(1<<OCF1A);					// clear compare A=callback flag
+				sei();								// enable global int
+				Update_All();
+				if(IS_CHANGE_PROTOCOL_FLAG_on)
+				break; // Protocol has been changed
+				while((TIFR1 & (1<<OCF1A)) == 0);	// wait 2ms...
+				#endif			
+				#endif
+			}
+			// at this point we have between 2ms and 4ms in next_callback
+			next_callback *= 2 ;									// disable global int
+			#ifdef XMEGA
+			cli();
+			TCC1.CCA +=next_callback;				// set compare A for callback
+			TCC1.INTFLAGS = TC1_CCAIF_bm ;			// clear compare A=callback flag
+			diff=TCC1.CCA-TCC1.CNT;					// compare timer and comparator
+			sei();// enable global int			
+			#else
+			#if defined STM32_board
+			OCR1A+=next_callback;
+			cli();
+			TIMER2_BASE->CCR1 = OCR1A;
+			TCNT1 = TIMER2_BASE->CNT;
+			TIMER2_BASE->SR &= ~TIMER_SR_CC1IF;//clear compare Flag write zero 
+			diff=OCR1A-TCNT1;						// compare timer and comparator
+			sei();
+			#else
+			cli();
+			OCR1A+=next_callback;					// set compare A for callback
+			TIFR1=(1<<OCF1A);						// clear compare A=callback flag
+			diff=OCR1A-TCNT1;						// compare timer and comparator
+			sei();									// enable global int
+			#endif		
+			#endif
+		}
+		while(diff&0x8000);	 						// Callback did not took more than requested time for next callback
+		// so we can launch Update_All before next callback
+	}
+}
+
+
+	void Update_All()
 	{
 #ifndef STM32_board
 		TX_ON;
 		NOP();
 		TX_OFF;
 #endif
+
 #ifdef ENABLE_SERIAL
 		if(mode_select==MODE_SERIAL && IS_RX_FLAG_on)	// Serial mode and something has been received
 		{
@@ -362,12 +478,9 @@ TX_OFF;
 			if(IS_CHANGE_PROTOCOL_FLAG_on)
 			{ // Protocol needs to be changed
 				LED_OFF;									//led off during protocol init
-			modules_reset();								//reset previous module
+				modules_reset();							//reset all modules
-			protocol_init();
+				protocol_init();							//init new protocol
-			//init new protocol
-			CHANGE_PROTOCOL_FLAG_off;					//done
 			}
-		
 		}
 #endif //ENABLE_SERIAL
 #ifdef ENABLE_PPM
@@ -386,9 +499,11 @@ TX_OFF;
 			update_aux_flags();
 			PPM_FLAG_off;	// wait for next frame before update
 		}
+#endif //ENABLE_PPM
 		update_led_status();
 		#if defined(TELEMETRY)
-		if( ((cur_protocol[0]&0x1F)==MODE_FRSKY) || ((cur_protocol[0]&0x1F)==MODE_HUBSAN) || ((cur_protocol[0]&0x1F)==MODE_FRSKYX) || ((cur_protocol[0]&0x1F)==MODE_DSM2) )
+		uint8_t protocol=cur_protocol[0]&0x1F;
+		if( (protocol==MODE_FRSKY) || (protocol==MODE_HUBSAN) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM2) )
 		TelemetryUpdate();
 		#endif
 #ifndef STM32_board
@@ -396,10 +511,9 @@ TX_ON;
 		NOP();
 		TX_OFF;
 #endif
-	if (remote_callback != 0)
-	CheckTimer(remote_callback);
 	}
 
+
 	// Update Servo_AUX flags based on servo AUX positions
 	static void update_aux_flags(void)
 	{
@@ -1269,6 +1383,10 @@ void init()
 		Prev_TCNT1+=Cur_TCNT1;
 	}
 #endif //ENABLE_PPM
+
+
+
+#ifdef ENABLE_SERIAL
 	//Serial RX
 #ifdef XMEGA
 	ISR(USARTC0_RXC_vect)