diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 3387e8e..8fa151c 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -327,7 +327,7 @@ enum FailSafeMode {
 #define IS_EXTRA_TELEMETRY_ON       (multi_config & 0x04)
 
 // Failsafe
-#define	failsafeToPPM(i)			(Failsafe_data[i]* 5/8+860)
+#define failsafeToPPM(i)			(Failsafe_data[i]* 5/8+860)
 #define isNormalFailsafeChanel(i)	(Failsafe_data[i] != FAILSAFE_CHANNEL_HOLD && Failsafe_data[i] != FAILSAFE_CHANNEL_NOPULSES)
 
 
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 90cb6f6..52af18a 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -974,7 +974,9 @@ void Serial_write( uint8_t byte )
 	#ifdef INVERT_SERIAL
 		byte |= 1 ;		// Start bit
 	#endif
-	uint8_t next = (SerialControl.head + 2) & 0x7f ;
+	uint8_t next = SerialControl.head + 2;
+	if(next>TXBUFFER_SIZE)
+		next=0;
 	if ( next != SerialControl.tail )
 	{
 		SerialControl.data[SerialControl.head] = byte ;