diff --git a/companion/src/firmwares/opentx/opentxinterface.cpp b/companion/src/firmwares/opentx/opentxinterface.cpp
index 41d561aeb..f26adbe7d 100644
--- a/companion/src/firmwares/opentx/opentxinterface.cpp
+++ b/companion/src/firmwares/opentx/opentxinterface.cpp
@@ -1178,6 +1178,16 @@ void addOpenTxLcdOptions(OpenTxFirmware * firmware)
   firmware->addOptions(lcd_options);
 }
 
+void addOpenTxVoiceOptions(OpenTxFirmware * firmware)
+{
+  Option voice_options[] = {
+    {"WTV20",     	QObject::tr("WTV20 voice module")},
+    {"JQ6500", 		QObject::tr("JQ6500 voice module")},
+    {NULL}
+  };
+  firmware->addOptions(voice_options);
+}
+
 QList<OpenTxEepromInterface *> opentxEEpromInterfaces;
 
 void registerOpenTxFirmware(OpenTxFirmware * firmware)
@@ -1480,7 +1490,8 @@ void registerOpenTxFirmwares()
   firmware->addOption("nocurves", QObject::tr("Disable curves menus"));
   firmware->addOption("sdcard", QObject::tr("Support for SD memory card"));
   firmware->addOption("audio", QObject::tr("Support for radio modified with regular speaker"));
-  firmware->addOption("voice", QObject::tr("Used if you have modified your radio with voice mode"));
+  //firmware->addOption("voice", QObject::tr("Used if you have modified your radio with voice mode"));
+  addOpenTxVoiceOptions(firmware);
   firmware->addOption("haptic", QObject::tr("Used if you have modified your radio with haptic mode"));
   firmware->addOption("ppmca", QObject::tr("PPM center adjustment in limits"));
   firmware->addOption("gvars", QObject::tr("Global variables"), GVARS_VARIANT);
diff --git a/doc/Mega2560/MEGA2560.lib b/doc/Mega2560/MEGA2560.lib
new file mode 100644
index 000000000..31a9dbb38
--- /dev/null
+++ b/doc/Mega2560/MEGA2560.lib
@@ -0,0 +1,381 @@
+EESchema-LIBRARY Version 2.3  Date: 06/01/2017 11:42:37
+#encoding utf-8
+#
+# +BATT
+#
+DEF +BATT #PWR 0 0 Y Y 1 F P
+F0 "#PWR" 0 -50 20 H I C CNN
+F1 "+BATT" 0 100 30 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+X +BATT 1 0 0 0 U 20 20 0 0 W N
+C 0 60 20 0 1 0 N
+P 3 0 1 0  0 0  0 40  0 40 N
+ENDDRAW
+ENDDEF
+#
+# +HF
+#
+DEF +HF #PWR 0 0 Y Y 1 F P
+F0 "#PWR" 0 -50 20 H I C CNN
+F1 "+HF" 0 100 50 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+X +BATT 1 0 0 0 U 20 20 0 0 W N
+C 0 60 20 0 1 0 N
+P 3 0 1 0  0 0  0 40  0 40 N
+ENDDRAW
+ENDDEF
+#
+# 4016
+#
+DEF 4016 U 0 0 Y Y 4 F N
+F0 "U" 200 -149 40 H V C CNN
+F1 "4016" 200 150 40 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+ALIAS 4066
+DRAW
+P 3 0 1 0  -100 -150  0 -50  0 -50 N
+P 4 0 1 0  -100 0  100 100  100 -100  -100 0 N
+P 4 0 1 0  -100 100  100 0  -100 -100  -100 100 N
+X V- 7 0 -250 200 U 30 30 0 1 W
+X V+ 14 0 250 200 D 30 30 0 1 W
+X I/O 1 -300 0 200 R 40 40 1 1 P
+X O/I 2 300 0 200 L 40 40 1 1 P
+X ON 13 -300 -150 200 R 40 40 1 1 I
+X O/I 3 300 0 200 L 40 40 2 1 P
+X I/O 4 -300 0 200 R 40 40 2 1 P
+X ON 5 -300 -150 200 R 40 40 2 1 I
+X ON 6 -300 -150 200 R 40 40 3 1 I
+X I/O 8 -300 0 200 R 40 40 3 1 P
+X O/I 9 300 0 200 L 40 40 3 1 P
+X O/I 10 300 0 200 L 40 40 4 1 P
+X I/O 11 -300 0 200 R 40 40 4 1 P
+X ON 12 -300 -150 200 R 40 40 4 1 I
+ENDDRAW
+ENDDEF
+#
+# 7805
+#
+DEF 7805 U 0 30 N Y 1 F N
+F0 "U" 150 -196 60 H V C CNN
+F1 "7805" 0 200 60 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+ALIAS LM7805 LM7812 78L05
+DRAW
+S -200 -150 200 150 0 1 0 N
+X VI VI -400 50 200 R 40 40 1 1 I
+X VO VO 400 50 200 L 40 40 1 1 w
+X GND GND 0 -250 100 U 40 40 1 1 I
+ENDDRAW
+ENDDEF
+#
+# C
+#
+DEF C C 0 10 N Y 1 F N
+F0 "C" 0 100 40 H V L CNN
+F1 "C" 6 -85 40 H V L CNN
+F2 "~" 38 -150 30 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+$FPLIST
+ SM*
+ C?
+ C1-1
+$ENDFPLIST
+DRAW
+P 2 0 1 20  -80 -30  80 -30 N
+P 2 0 1 20  -80 30  80 30 N
+X ~ 1 0 200 170 D 40 40 1 1 P
+X ~ 2 0 -200 170 U 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# CONN_1
+#
+DEF ~CONN_1 P 0 30 N N 1 F N
+F0 "P" 80 0 40 H V L CNN
+F1 "CONN_1" 0 55 30 H I C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+C 0 0 31 0 1 0 N
+P 2 0 1 0  -30 0  -50 0 N
+X 1 1 -150 0 100 R 60 60 1 1 P
+ENDDRAW
+ENDDEF
+#
+# DIODESCH
+#
+DEF DIODESCH D 0 40 N N 1 F N
+F0 "D" 0 100 40 H V C CNN
+F1 "DIODESCH" 0 -100 40 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+$FPLIST
+ D?
+ S*
+$ENDFPLIST
+DRAW
+P 3 0 1 0  -50 50  50 0  -50 -50 F
+P 6 0 1 8  75 25  75 50  50 50  50 -50  25 -50  25 -25 N
+X A 1 -200 0 150 R 40 40 1 1 P
+X K 2 200 0 150 L 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# GND
+#
+DEF ~GND #PWR 0 0 Y Y 1 F P
+F0 "#PWR" 0 0 30 H I C CNN
+F1 "GND" 0 -70 30 H I C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+P 4 0 1 0  -50 0  0 -50  50 0  -50 0 N
+X GND 1 0 0 0 U 30 30 1 1 W N
+ENDDRAW
+ENDDEF
+#
+# LED
+#
+DEF LED D 0 40 Y N 1 F N
+F0 "D" 0 100 50 H V C CNN
+F1 "LED" 0 -100 50 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+$FPLIST
+ LED-3MM
+ LED-5MM
+ LED-10MM
+ LED-0603
+ LED-0805
+ LED-1206
+ LEDV
+$ENDFPLIST
+DRAW
+P 2 0 1 0  50 50  50 -50 N
+P 3 0 1 0  -50 50  50 0  -50 -50 F
+P 3 0 1 0  65 -40  110 -80  105 -55 N
+P 3 0 1 0  80 -25  125 -65  120 -40 N
+X A 1 -200 0 150 R 40 40 1 1 P
+X K 2 200 0 150 L 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# LM2903
+#
+DEF LM2903 U 0 20 Y Y 2 F N
+F0 "U" 150 150 60 H V C CNN
+F1 "LM2903" 200 -200 60 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+ALIAS LM293 LM393 LM193
+DRAW
+P 4 0 1 0  -200 200  200 0  -200 -200  -200 200 N
+X ~ 1 500 0 300 L 40 40 1 1 C
+X _ 2 -500 -100 300 R 40 40 1 1 I
+X + 3 -500 100 300 R 40 40 1 1 I
+X V- 4 -100 -400 250 U 40 40 1 1 W
+X V+ 8 -100 400 250 D 40 40 1 1 W
+X V- 4 -100 -400 250 U 40 40 2 1 W
+X + 5 -500 100 300 R 40 40 2 1 I
+X _ 6 -500 -100 300 R 40 40 2 1 I
+X ~ 7 500 0 300 L 40 40 2 1 C
+X V+ 8 -100 400 250 D 40 40 2 1 W
+ENDDRAW
+ENDDEF
+#
+# MOS_N
+#
+DEF MOS_N Q 0 0 N Y 1 F N
+F0 "Q" 10 170 60 H V R CNN
+F1 "MOS_N" 10 -150 60 H V R CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+ALIAS MOSFET_N
+DRAW
+P 2 0 1 8  -50 -100  -50 100 N
+P 2 0 1 10  0 -150  0 150 N
+P 2 0 1 0  100 -100  0 -100 N
+P 2 0 1 0  100 100  0 100 N
+P 3 0 1 8  100 -100  100 0  50 0 N
+P 5 0 1 8  50 30  50 -30  0 0  50 30  50 30 N
+X D D 100 200 100 D 40 40 1 1 P
+X G G -200 0 150 R 40 40 1 1 I
+X S S 100 -200 100 U 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# MOS_P
+#
+DEF MOS_P Q 0 40 Y N 1 F N
+F0 "Q" 0 190 60 H V R CNN
+F1 "MOS_P" 0 -180 60 H V R CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+ALIAS MOSFET_P
+DRAW
+P 2 0 1 8  -50 -100  -50 100 N
+P 2 0 1 10  0 -150  0 150 N
+P 2 0 1 8  30 0  0 0 N
+P 2 0 1 0  100 -100  0 -100 N
+P 2 0 1 0  100 100  0 100 N
+P 3 0 1 0  80 0  100 0  100 -100 N
+P 5 0 1 8  30 40  30 -30  80 0  30 40  30 40 N
+X D D 100 200 100 D 40 40 1 1 P
+X G G -200 0 150 R 40 40 1 1 I
+X S S 100 -200 100 U 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# POT
+#
+DEF POT RV 0 40 Y N 1 F N
+F0 "RV" 0 -100 50 H V C CNN
+F1 "POT" 0 0 50 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+S -150 50 150 -50 0 1 0 N
+P 3 0 1 0  0 50  -20 70  20 70 F
+X 1 1 -250 0 100 R 40 40 1 1 P
+X 2 2 0 150 80 D 40 40 1 1 P
+X 3 3 250 0 100 L 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# R
+#
+DEF R R 0 0 N Y 1 F N
+F0 "R" 80 0 40 V V C CNN
+F1 "R" 7 1 40 V V C CNN
+F2 "~" -70 0 30 V V C CNN
+F3 "~" 0 0 30 H V C CNN
+$FPLIST
+ R?
+ SM0603
+ SM0805
+ R?-*
+ SM1206
+$ENDFPLIST
+DRAW
+S -40 150 40 -150 0 1 12 N
+X ~ 1 0 250 100 D 60 60 1 1 P
+X ~ 2 0 -250 100 U 60 60 1 1 P
+ENDDRAW
+ENDDEF
+#
+# SPEAKER
+#
+DEF SPEAKER SP 0 0 N Y 1 F N
+F0 "SP" -100 250 70 H V C CNN
+F1 "SPEAKER" -100 -250 70 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+P 4 0 0 0  100 150  250 300  250 -300  100 -150 N
+P 5 0 0 0  -100 150  100 150  100 -150  -100 -150  -100 150 F
+X 1 1 -300 100 200 R 40 40 1 1 I
+X 2 2 -300 -100 200 R 40 40 1 1 I
+ENDDRAW
+ENDDEF
+#
+# SPST
+#
+DEF SPST SW 0 0 N Y 1 F N
+F0 "SW" 0 100 70 H V C CNN
+F1 "SPST" 0 -100 70 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+C -150 0 50 0 0 0 N
+C 150 0 50 0 0 0 N
+P 2 0 0 0  -100 0  100 100 N
+X 1 1 -500 0 300 R 70 70 1 1 I
+X 2 2 500 0 300 L 70 70 1 1 I
+ENDDRAW
+ENDDEF
+#
+# SW
+#
+DEF SW SW 0 40 N N 1 F N
+F0 "SW" 0 100 50 H V C CNN
+F1 "SW" 0 0 50 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+P 2 0 1 0  -100 0  100 50 N
+X 1 1 -300 0 200 R 60 60 0 1 P I
+X 2 2 300 0 200 L 60 60 0 1 P I
+ENDDRAW
+ENDDEF
+#
+# SW_PUSH
+#
+DEF SW_PUSH SW 0 40 N N 1 F N
+F0 "SW" 150 110 50 H V C CNN
+F1 "SW_PUSH" 0 -80 50 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+S -170 50 170 60 0 1 0 N
+P 4 0 1 0  -40 60  -30 90  30 90  40 60 N
+X 1 1 -300 0 200 R 60 60 0 1 P I
+X 2 2 300 0 200 L 60 60 0 1 P I
+ENDDRAW
+ENDDEF
+#
+# TLP290
+#
+DEF TLP290 IC 0 40 Y Y 1 F N
+F0 "IC" -213 178 40 H V C CNN
+F1 "TLP290" 149 -184 40 H V C CNN
+F2 "SOP4" -164 -174 29 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+C -175 -100 5 0 1 0 N
+C -175 100 5 0 1 0 N
+S -250 150 250 -150 0 1 10 f
+P 2 0 1 10  -200 25  -150 25 N
+P 2 0 1 0  -175 -25  -175 -100 N
+P 2 0 1 0  -175 25  -175 100 N
+P 2 0 1 10  -125 -25  -75 -25 N
+P 2 0 1 0  100 25  175 100 N
+P 2 0 1 0  175 -100  100 -25 F
+P 2 0 1 0  175 -100  250 -100 N
+P 2 0 1 0  175 100  250 100 N
+P 3 0 1 0  -250 100  -100 100  -100 25 N
+P 3 0 1 0  -100 -25  -100 -100  -250 -100 N
+P 3 0 1 20  100 75  100 -75  100 -75 N
+P 4 0 1 10  -175 25  -200 -25  -150 -25  -175 25 F
+P 4 0 1 10  -100 -25  -125 25  -75 25  -100 -25 F
+P 4 0 1 0  -20 10  1 30  1 10  31 40 N
+P 4 0 1 0  31 40  11 30  21 20  31 40 N
+P 5 0 1 0  120 -65  140 -45  160 -85  120 -65  120 -65 F
+P 7 0 1 0  -20 -30  1 -10  1 -30  31 0  11 -10  21 -20  31 0 N
+X ~ 1 -350 100 100 R 40 40 1 1 P
+X ~ 2 -350 -100 100 R 40 40 1 1 P
+X ~ 3 350 -100 100 L 40 40 1 1 P
+X ~ 4 350 100 100 L 40 40 1 1 P
+ENDDRAW
+ENDDEF
+#
+# VCC
+#
+DEF VCC #PWR 0 0 Y Y 1 F P
+F0 "#PWR" 0 100 30 H I C CNN
+F1 "VCC" 0 100 30 H V C CNN
+F2 "~" 0 0 60 H V C CNN
+F3 "~" 0 0 60 H V C CNN
+DRAW
+X VCC 1 0 0 0 U 20 20 0 0 W N
+C 0 50 20 0 1 0 N
+P 3 0 1 0  0 0  0 30  0 30 N
+ENDDRAW
+ENDDEF
+#
+#End Library
diff --git a/doc/Mega2560/MEGA2560.pro b/doc/Mega2560/MEGA2560.pro
new file mode 100644
index 000000000..3cb03fb27
--- /dev/null
+++ b/doc/Mega2560/MEGA2560.pro
@@ -0,0 +1,42 @@
+update=07/01/2017 00:36:05
+last_client=eeschema
+[eeschema]
+version=1
+LibDir=
+NetFmtName=
+RptD_X=0
+RptD_Y=100
+RptLab=1
+LabSize=60
+[eeschema/libraries]
+LibName1=power
+LibName2=device
+LibName3=transistors
+LibName4=conn
+LibName5=linear
+LibName6=regul
+LibName7=74xx
+LibName8=cmos4000
+LibName9=adc-dac
+LibName10=memory
+LibName11=xilinx
+LibName12=special
+LibName13=microcontrollers
+LibName14=dsp
+LibName15=microchip
+LibName16=analog_switches
+LibName17=motorola
+LibName18=texas
+LibName19=intel
+LibName20=audio
+LibName21=interface
+LibName22=digital-audio
+LibName23=philips
+LibName24=display
+LibName25=cypress
+LibName26=siliconi
+LibName27=opto
+LibName28=atmel
+LibName29=contrib
+LibName30=valves
+LibName31=MEGA2560
diff --git a/doc/Mega2560/MEGA2560.sch b/doc/Mega2560/MEGA2560.sch
index 8d8b5f710..e745af05a 100644
--- a/doc/Mega2560/MEGA2560.sch
+++ b/doc/Mega2560/MEGA2560.sch
@@ -29,14 +29,14 @@ LIBS:opto
 LIBS:atmel
 LIBS:contrib
 LIBS:valves
-LIBS:MEGA2560-cache
+LIBS:MEGA2560
 EELAYER 27 0
 EELAYER END
 $Descr A4 11693 8268
 encoding utf-8
 Sheet 1 1
 Title "OpenTX board based on Arduino MEGA2560"
-Date "27 feb 2016"
+Date "6 jan 2017"
 Rev "0"
 Comp "Copyright F. Aguerre"
 Comment1 ""
@@ -246,7 +246,7 @@ L SPEAKER .
 U 1 1 54994432
 P 10600 6200
 F 0 "." H 10500 6450 70  0000 C CNN
-F 1 "BUZZER" H 10500 5950 40  0000 C CNN
+F 1 "BUZZER or SPEAKER" H 10700 5800 40  0000 C CNN
 F 2 "" H 10600 6200 60  0001 C CNN
 F 3 "" H 10600 6200 60  0001 C CNN
 	1    10600 6200
@@ -886,7 +886,7 @@ Key_►
 Text Label 6600 6150 2    42   ~ 0
 Key_▼
 Text Label 2000 3700 3    42   ~ 0
-Somo_Busy
+Voice_Busy
 $Comp
 L R R1
 U 1 1 54988260
@@ -1248,7 +1248,7 @@ port H 4 (1)
 Text GLabel 2750 4600 1    42   Output ~ 0
 port H 3 (0)
 Text Label 2750 3700 3    42   ~ 0
-Speaker
+Buzzer
 Text Label 3150 3700 3    42   ~ 0
 Rot_1_A
 Text GLabel 3150 4600 1    42   Input ~ 0
@@ -1272,7 +1272,7 @@ port B 5 (1)
 Text GLabel 2300 4600 1    42   Output ~ 0
 port B 4 (0)
 Text Label 2300 3700 3    42   ~ 0
-Buzzer
+-----
 Text Label 2100 3700 3    42   ~ 0
 PPM_OUT
 Text GLabel 2100 4600 1    42   Output ~ 0
@@ -41010,14 +41010,14 @@ F 3 "" H 10450 3850 60  0001 C CNN
 	1    10450 3850
 	-1   0    0    -1  
 $EndComp
-Text Label 2250 3300 2    42   ~ 0
+Text Label 2300 3150 2    42   ~ 0
 M2560 output
-Text GLabel 1050 3300 2    42   Output ~ 0
+Text GLabel 1100 3150 2    42   Output ~ 0
 port pin (init. state) 
-Text Notes 1250 3200 0    60   ~ 0
+Text Notes 1300 3050 0    60   ~ 0
 Pinout caption :
 Text Label 5750 2400 2    42   ~ 0
-Gain = 1 + R2/R6
+Gain = 1 + R2/(R6/2)
 $Bitmap
 Pos 4600 4650
 Scale 1.000000
@@ -41629,9 +41629,9 @@ Wire Wire Line
 Wire Wire Line
 	10450 4100 10450 4200
 Wire Wire Line
-	1800 3300 2250 3300
+	1850 3150 2300 3150
 Wire Wire Line
-	950  3300 1050 3300
+	1000 3150 1100 3150
 Wire Wire Line
 	4450 4300 4450 6450
 Wire Wire Line
@@ -41696,15 +41696,15 @@ F 3 "" H 1750 7050 60  0000 C CNN
 	-1   0    0    1   
 $EndComp
 Text Notes 4850 6650 0    42   ~ 0
-LCD supported : ST7565P, ST7565R, ERC12864FSF, ST7920\nProtocol = 6800, 8 bits parallel mode\n\nLCD pinout labels (may differ, depending of brand) :\nRD = Read or Enable on ST7920\nWR = Write or Write(0)/Read(1) on ST7920\nA0 = R/S (register select) or D/I (data/instruction select)\nRST = RES (reset)\nCS = Chip Select (not used on ST7920)
-Text Label 2250 3400 2    42   ~ 0
+LCD supported : ST7565P, ST7565R, ERC12864FSF, ST7920, KS108\nProtocol = 6800, 8 bits parallel mode\n\nLCD pinout labels (may differ, default = ST7565P) :\nRD = (Read) or (ST7920 : Enable)\nWR = (Write) or (ST7920 : Write(0)/Read(1)) or (KS108 : CS2)\nA0 = (R/S register select) or (D/I data/instruction select)\nRST = (RES reset)\nCS = (Chip Select) or (ST7920 : not used) or (KS108 : CS1)
+Text Label 2300 3250 2    42   ~ 0
 M2560 input
-Text GLabel 1050 3400 2    42   Input ~ 0
+Text GLabel 1100 3250 2    42   Input ~ 0
 port pin (init. state) 
 Wire Wire Line
-	1800 3400 2250 3400
+	1850 3250 2300 3250
 Wire Wire Line
-	950  3400 1050 3400
+	1000 3250 1100 3250
 Text Notes 4850 7350 0    42   ~ 0
 NOTA : all features are optional.\nM2560 can properly work without PPM / power management,\nswitch deboucing, stick voltage protection or  rotary encoder.\nMinimum requirement is just LCD and navigation keys. Some\nswitches could be usefull, especially for flight conditions, but\nnot fundamental.
 $Comp
@@ -41938,17 +41938,19 @@ Wire Wire Line
 Wire Wire Line
 	2000 4600 2000 4750
 Text Label 2850 3700 3    42   ~ 0
-Somo_Data
+Voice_Data
 Wire Wire Line
 	2850 4150 2850 3700
 Text Label 2950 3700 3    42   ~ 0
-Somo_Clock
+Voice_Clock
 Wire Wire Line
 	2950 4150 2950 3700
 Text Label 3750 3700 3    42   ~ 0
-Somo_Reset
+Voice_Reset
 Wire Wire Line
 	3750 4150 3750 3700
 Text Notes 10400 1300 0    60   ~ 0
 R6 is optional
+Text Notes 1000 3500 0    42   ~ 0
+Voice module supported :\nWTV20, pins = data, busy, clock, reset\nJQ6500, pins = data (serial), busy
 $EndSCHEMATC
diff --git a/doc/Mega2560/MEGA2560_Bootloader.rar b/doc/Mega2560/MEGA2560_Bootloader.rar
new file mode 100644
index 000000000..fcf649e98
Binary files /dev/null and b/doc/Mega2560/MEGA2560_Bootloader.rar differ
diff --git a/doc/Mega2560/MEGA2560_Flash_Procedure.pdf b/doc/Mega2560/MEGA2560_Flash_Procedure.pdf
new file mode 100644
index 000000000..891cdb401
Binary files /dev/null and b/doc/Mega2560/MEGA2560_Flash_Procedure.pdf differ
diff --git a/doc/Mega2560/MEGA2560_LCD_Configuration.pdf b/doc/Mega2560/MEGA2560_LCD_Configuration.pdf
new file mode 100644
index 000000000..e413f35c6
Binary files /dev/null and b/doc/Mega2560/MEGA2560_LCD_Configuration.pdf differ
diff --git a/doc/Mega2560/MEGA2560_Listing.xls b/doc/Mega2560/MEGA2560_Listing.xls
index aa25c927f..93cacccfa 100644
Binary files a/doc/Mega2560/MEGA2560_Listing.xls and b/doc/Mega2560/MEGA2560_Listing.xls differ
diff --git a/doc/Mega2560/MEGA2560_NextStepRC_Radio.rar b/doc/Mega2560/MEGA2560_NextStepRC_Radio.rar
new file mode 100644
index 000000000..0c51b26d1
Binary files /dev/null and b/doc/Mega2560/MEGA2560_NextStepRC_Radio.rar differ
diff --git a/doc/Mega2560/MEGA2560_Pin_Mapping.xls b/doc/Mega2560/MEGA2560_Pin_Mapping.xls
new file mode 100644
index 000000000..43c87abfc
Binary files /dev/null and b/doc/Mega2560/MEGA2560_Pin_Mapping.xls differ
diff --git a/doc/Mega2560/MEGA2560_SCH.pdf b/doc/Mega2560/MEGA2560_SCH.pdf
index d12679e60..2829d13e1 100644
Binary files a/doc/Mega2560/MEGA2560_SCH.pdf and b/doc/Mega2560/MEGA2560_SCH.pdf differ
diff --git a/radio/src/audio_avr.cpp b/radio/src/audio_avr.cpp
index a6046dfaa..5dbc464d0 100644
--- a/radio/src/audio_avr.cpp
+++ b/radio/src/audio_avr.cpp
@@ -283,7 +283,7 @@ void audioTimerCountdown(uint8_t timer, int value)
     // pass
   }
 
-#if defined(CPUM2560)
+#if defined(CPUM2560) && (defined(VOICE_WTV20) || defined(VOICE_JQ6500))
   else if (g_model.timers[timer].countdownBeep == COUNTDOWN_VOICE) {
     if (value >= 0 && value <= TIMER_COUNTDOWN_START(timer)) {
       playNumber(value, 0, 0);
diff --git a/radio/src/fonts.h b/radio/src/fonts.h
index 130409341..17548110f 100644
--- a/radio/src/fonts.h
+++ b/radio/src/fonts.h
@@ -36,7 +36,7 @@ void loadFontCache();
 extern const pm_uchar font_5x7[];
 extern const pm_uchar font_10x14[];
 
-#if defined(BOLD_FONT) && (!defined(CPUM64) || defined(TELEMETRY_NONE)) && !defined(BOOT)
+#if defined(BOLD_FONT) && ((!defined(CPUM64) && !defined(PCBMEGA2560)) || defined(TELEMETRY_NONE)) && !defined(BOOT)
  #define BOLD_SPECIFIC_FONT
  extern const pm_uchar font_5x7_B[];
 #endif
diff --git a/radio/src/gui/128x64/lcd.cpp b/radio/src/gui/128x64/lcd.cpp
index 531180b23..81aa86fc2 100644
--- a/radio/src/gui/128x64/lcd.cpp
+++ b/radio/src/gui/128x64/lcd.cpp
@@ -531,6 +531,7 @@ void lcdDrawNumber(coord_t x, coord_t y, lcdint_t val, LcdFlags flags, uint8_t l
         x += 2;
       }
     }
+
   #if defined(BOLD_FONT) && !defined(CPUM64) || defined(TELEMETRY_NONE)
     if (flags & BOLD) fw += 1;
   #endif
diff --git a/radio/src/targets/9x/lcd_st7920_driver.cpp b/radio/src/targets/9x/lcd_st7920_driver.cpp
index 91fac1d40..7cd02044a 100644
--- a/radio/src/targets/9x/lcd_st7920_driver.cpp
+++ b/radio/src/targets/9x/lcd_st7920_driver.cpp
@@ -62,48 +62,31 @@ uint8_t lcdRefresh_ST7920(uint8_t full)
 {
   LCD_LOCK();
   static uint8_t state;
-  uint8_t yst,yend;
+  uint8_t yst;
+  uint8_t yend;
+  uint8_t y_table[6]={0,13,26,39,52,64}; 
   uint8_t x_addr = 0;
   uint8_t y_addr = 0;
   uint16_t line_offset = 0;
   uint8_t col_offset = 0;
   uint8_t bit_count = 0;
   uint8_t result;
-  uint8_t * p;
-  if (full != 0) {
+  uint8_t *p;
+  
+  if(full!=0){
     yst=0;
     yend=64;
     state=0;
   } 
-  else {
-    switch (state) { // Since writing to ST7920 is too slow we need to split it to five bands.
-      default:
-      case 0:
-        yst=0;
-        yend=13;
-        state=1;
-      break;
-      case 1:
-        yst=13;
-        yend=26;
-        state=2;
-      break;
-      case 2:
-        yst=26;
-        yend=39;
-        state=3;
-      break;
-      case 3:
-        yst=39;
-        yend=52;
-        state=4;
-      break;
-      case 4:
-        yst=52;
-        yend=64;
-        state=0;
-      break;
+  else{ //Since writing to ST7920 is too slow we need to split it to five bands
+    yst=y_table[state];
+    yend=y_table[state+1];
+    if (state==4){
+      state=0;
     }
+    else{
+      state++;
+    }    
   }
   
   for (uint8_t y=yst; y<yend; y++) {
diff --git a/radio/src/targets/common/avr/CMakeLists.txt b/radio/src/targets/common/avr/CMakeLists.txt
index b1b35b400..a3953b35c 100644
--- a/radio/src/targets/common/avr/CMakeLists.txt
+++ b/radio/src/targets/common/avr/CMakeLists.txt
@@ -1,5 +1,5 @@
 option(AUDIO "Audio support" OFF)
-option(VOICE "Voice support" OFF)
+set(VOICE "OFF" CACHE STRING "Voice support (OFF/ON/WTV20/JQ6500)")
 option(GRAPHICS "Additional graphics" ON)
 option(BOLD "Bold font" ON)
 option(BATTGRAPH "Battery graph" OFF)
@@ -45,14 +45,17 @@ if(BUZZER)
   add_definitions(-DBUZZER)
   set(SRC ${SRC} buzzer.cpp)
 endif()
-if(VOICE)
+if((VOICE STREQUAL ON AND NOT PCB STREQUAL MEGA2560) OR ((VOICE STREQUAL WTV20 OR VOICE STREQUAL JQ6500) AND (PCB STREQUAL MEGA2560)))
   add_definitions(-DVOICE)
+  if(PCB STREQUAL MEGA2560)
+    add_definitions(-DVOICE_${VOICE})
+  endif()
   set(TARGET_SRC ${TARGET_SRC} voice.cpp)
   foreach(LANGUAGE ${TTS_LANGUAGES})
-    if(TTS STREQUAL ${LANGUAGE})
-      add_definitions(-DTTS_${LANGUAGE})
-      string(TOLOWER ${LANGUAGE} lang_lower)
-      set(SRC ${SRC} translations/tts_${lang_lower}.cpp)
+    string(TOUPPER ${LANGUAGE} LANGUAGE_CAPITALIZED)
+    if(TTS STREQUAL ${LANGUAGE_CAPITALIZED})
+      add_definitions(-DTTS_${LANGUAGE_CAPITALIZED})
+      set(SRC ${SRC} translations/tts_${LANGUAGE}.cpp)
     endif()
   endforeach()
 endif()
diff --git a/radio/src/targets/mega2560/CMakeLists.txt b/radio/src/targets/mega2560/CMakeLists.txt
index 682f07379..299d8ae46 100644
--- a/radio/src/targets/mega2560/CMakeLists.txt
+++ b/radio/src/targets/mega2560/CMakeLists.txt
@@ -1,5 +1,6 @@
 option(PWRMANAGE "Pwr Manage" OFF)
-option(SDCARD "SD card support" ON)
+option(SDCARD "SD card support" OFF)
+option(ROTENC_DIV2 "Rotary encoder resolution/2" OFF)
 set(ARCH AVR)
 set(MCU atmega2560)
 string(TOLOWER ${PCB} FLAVOUR)
diff --git a/radio/src/targets/mega2560/board.cpp b/radio/src/targets/mega2560/board.cpp
index deff73162..1b22928ec 100644
--- a/radio/src/targets/mega2560/board.cpp
+++ b/radio/src/targets/mega2560/board.cpp
@@ -18,23 +18,24 @@
  * GNU General Public License for more details.
  */
 
-#include "opentx.h"
+#include "opentx.h"         
+#include "i2c_driver.h"
 
 void boardInit()
 {
 #if !defined(SIMU)
   // Set up I/O port data directions and initial states (unused pin setting : input, pull-up on)
   DDRA = 0b11111111;  PORTA = 0b00000000; // LCD data
-  DDRB = 0b01110111;  PORTB = 0b00101111; // 7:WTV20SDBusy, 6:PPM_OUT, 5:SimCTRL, 4:Buzzer, SDCARD[3:MISO 2:MOSI 1:SCK 0:CS]
+  DDRB = 0b01100111;  PORTB = 0b10111111; // 7:VoiceBusy, 6:PPM_OUT, 5:SimCTRL, 4:Unused, SDCARD[3:MISO 2:MOSI 1:SCK 0:CS]
   DDRC = 0b11111100;  PORTC = 0b00000011; // 7-3:LCD, 2:BackLight, 1:ID2_SW, 0:ID1_SW
-  DDRD = 0b00000000;  PORTD = 0b11111100; // 7:AilDR_SW, 6:N/A, 5:N/A, 4:N/A, 3:RENC2_B, 2:RENC2_A, 1:I2C_SDA, 0:I2C_SCL
-  DDRE = 0b00001010;  PORTE = 0b01110100; // 7:PPM_IN, 6:N/A, 5:RENC1_B, 4:RENC1_A, 3:WTV20SDData, 2:N/A, 1:TELEM_TX, 0:TELEM_RX
+  DDRD = 0b00000000;  PORTD = 0b11111111; // 7:AilDR_SW, 6:N/A, 5:N/A, 4:N/A, 3:RENC2_B, 2:RENC2_A, 1:I2C_SDA, 0:I2C_SCL
+  DDRE = 0b00001010;  PORTE = 0b01110100; // 7:PPM_IN, 6:N/A, 5:RENC1_B, 4:RENC1_A, 3:VoiceData, 2:N/A, 1:TELEM_TX, 0:TELEM_RX
   DDRF = 0b00000000;  PORTF = 0b11111111; // 7-0:Trim switch inputs
-  DDRG = 0b00100000;  PORTG = 0b11011111; // 7:N/A, 6:N/A, 5:WTV20SDClock, 4:N/A, 3:N/A, 2:TCut_SW, 1:Gear_SW, 0: RudDr_SW
+  DDRG = 0b00100000;  PORTG = 0b11011111; // 7:N/A, 6:N/A, 5:VoiceClock, 4:N/A, 3:N/A, 2:TCut_SW, 1:Gear_SW, 0: RudDr_SW
 #if defined(DEBUG)  
-  DDRH = 0b01011010;  PORTH = 0b11110100; // 7:N/A, 6:RF_Activated, 5:DSC_Activated, 4:Hold_Power, 3:Speaker, 2:N/A, 1:WTV20SDReset, 0:Haptic
+  DDRH = 0b01011011;  PORTH = 0b11110100; // 7:N/A, 6:RF_Activated, 5:DSC_Activated, 4:Hold_Power, 3:Speaker, 2:N/A, 1:VoiceReset, 0:Haptic
 #else
-  DDRH = 0b00011010;  PORTH = 0b11110100; // 7:N/A, 6:RF_Activated, 5:DSC_Activated, 4:Hold_Power, 3:Speaker, 2:N/A, 1:WTV20SDReset, 0:Haptic
+  DDRH = 0b00011011;  PORTH = 0b11110100; // 7:N/A, 6:RF_Activated, 5:DSC_Activated, 4:Hold_Power, 3:Speaker, 2:N/A, 1:VoiceReset, 0:Haptic
 #endif  
   DDRJ = 0b00000000;  PORTJ = 0b11111111; // 7:N/A, 6:N/A, 5:N/A, 4:N/A, 3:N/A, 2:N/A, 1:RENC2_push, 0:RENC1_push
   DDRK = 0b00000000;  PORTK = 0b00000000; // Analogic input (no pull-ups)
@@ -52,17 +53,24 @@ void boardInit()
   #if defined(AUDIO)
   // TIMER4 set into CTC mode, prescaler 16MHz/64=250 kHz 
   // Used for audio tone generation
-  TCCR4B  = (1<<WGM42) | (0b011 << CS40);
+  TCCR4B  = (0b10<<WGM42) | (0b011 << CS40);
   TCCR4A  = 0x00;
   #endif
   
-  #if defined(VOICE)
-  // WTV20SD set-up, with TIMER5
+  #if defined(VOICE_WTV20)
+  // Voice set-up with TIMER5
   OCR5A = 0x1F4; //2ms
   TCCR5B = (1 << WGM52) | (0b011 << CS50); // CTC OCR5A
   TIMSK5 |= (1<<OCIE5A); // Start the interrupt so the unit reset can occur
   #endif
-
+  
+  #if defined(VOICE_JQ6500)
+  // Voice set-up with TIMER5
+  JQ6500_Serial_on;	// Idle state (1)
+  OCR5A = 0x19; // 0x1A=104µs needed for the 9600Bps serial command
+  TCCR5B = (1 << WGM52) | (0b011 << CS50); // CTC OCR5A
+  #endif
+  
   /* Rotary encoder interrupt set-up                 */
   EIMSK = 0; // disable ALL external interrupts.
   // encoder 1
@@ -72,8 +80,14 @@ void boardInit()
   EICRA = (1<<ISC30) | (1<<ISC20);
   EIFR = (3<<INTF2);
   EIMSK = (3<<INT4) | (3<<INT2); // enable the two rot. enc. ext. int. pairs.
-#endif // !defined(SIMU)
-}
+  
+  #if defined(RTCLOCK)
+  i2c_init();  // hardware I2C init  
+  #endif
+  
+#endif // !SIMU               
+}              
+
 
 uint8_t pwrCheck()
 {
@@ -162,7 +176,7 @@ static const pm_uchar crossTrim[] PROGMEM = {
   TRIMS_GPIO_PIN_RHL,
   TRIMS_GPIO_PIN_RHR
 };
-     
+
 uint8_t trimDown(uint8_t idx)
 {
   uint8_t in = ~PINF;                //was PIND
@@ -205,7 +219,7 @@ ISR(INT4_vect)     // Arduino2560 IO02 (portE pin4)
   if (input == 0 || input == 0x30) incRotaryEncoder(0, -1);
 #endif
 }
-                 
+
 ISR(INT5_vect)     // Arduino2560 IO03 (portE pin5)
 {
   uint8_t input = (PINE & 0x30);
@@ -235,7 +249,7 @@ ISR(INT3_vect)     // Arduino2560 IO18 (portD pin3)
   if (input == 0 || input == 0x0C) incRotaryEncoder(1, +1);
 #endif
 }  
-       
+
 // RTC driver
 void rtcSetTime(const struct gtm * tm)
 {
diff --git a/radio/src/targets/mega2560/board.h b/radio/src/targets/mega2560/board.h
index 8f1dad9a9..a7eb37cb3 100644
--- a/radio/src/targets/mega2560/board.h
+++ b/radio/src/targets/mega2560/board.h
@@ -23,9 +23,6 @@
 
 #include "../common/avr/board_avr.h"
 
-//Mods for futur use ? Beta tester love it
-//#define ROTENC_DIV2 // rotenc resolution/2
-
 // Board driver
 void boardInit(void);
 #define boardOff()  pwrOff()
@@ -104,22 +101,22 @@ void sdPoll10ms(void);
 #define INP_L_Trainer             7
 
 // Servitudes driver
-#define INP_E_PPM_IN              7    //not used (reserved)
-#define INP_B_WTV_BUSY            7    //WTV20SD, not used (reserved)
+#define INP_E_PPM_IN              7    //directly used
+#define INP_B_VoiceBuzy           7    //directly used
 #define OUT_B_PPM                 6
 #define OUT_B_SIM_CTL             5
-#define OUT_B_BUZZER              4
+#define I_O_B_UNUSED              4    //unused, was Buzzer
 #define INP_D_I2C_SCL             1
 #define INP_D_I2C_SDA             0
-#define OUT_E_WTV_DATA            3    //WTV20SD
+#define OUT_E_VoiceData           3
 #define INP_E_TELEM_RX            1
 #define OUT_E_TELEM_TX            0
-#define OUT_G_WTV_CLK             5    //WTV20SD
+#define OUT_G_VoiceClock          5
 #define INP_H_RF_Activated        6
-#define INP_H_DSC_Activated       5    //not used, reserved for pwrCheck()
-#define INP_H_Hold_Power          4    //not used, reserved for pwrCheck()
-#define OUT_H_Speaker             3
-#define OUT_H_WTV_RESET           1    //WTV20SD
+#define INP_H_DSC_Activated       5    //pwrCheck(), directly used
+#define INP_H_Hold_Power          4    //pwrCheck(), directly used
+#define OUT_H_SpeakerBuzzer       3
+#define OUT_H_VoiceReset          1
 #define OUT_H_HAPTIC              0
 
 // Rotary encoders driver
@@ -132,13 +129,14 @@ void sdPoll10ms(void);
 #define REA_DOWN()                (~PINJ & (1<<INP_J_ROT_ENC_1_PUSH))
 #define REB_DOWN()                (~PINJ & (1<<INP_J_ROT_ENC_2_PUSH))
 #define ROTENC_DOWN()             (REA_DOWN() || REB_DOWN())
+#define ROTENC_DIV2                 // rotary encoders resolution/2
 
 // LCD driver
 #define PORTA_LCD_DAT            PORTA
 #define PORTC_LCD_CTRL           PORTC
-#if defined(LCD_KS108)				// (For KS108 LCd only) MEGA R/W pin always at 0 state in Opentx then
-#define OUT_C_LCD_CS2            6	// Use this pin to control second KS108
-#else								// And connect LCD R/W pin to ground via a 1k resistor
+#if defined(LCD_KS108)              // (For KS108 LCD only) MEGA R/W pin always at 0 state in Opentx then
+#define OUT_C_LCD_CS2            6  // use this pin to control second KS108 (CS2)
+#else                               // and connect KS108 R/W pin to ground via a 1k resistor
 #define OUT_C_LCD_RnW            6
 #endif
 #define OUT_C_LCD_E              7
@@ -167,8 +165,8 @@ void pwrOff();
 #define hapticOff()               PORTH &= ~(1 << OUT_H_HAPTIC)
 
 // Buzzer driver
-#define buzzerOn()                PORTB |=  (1 << OUT_B_BUZZER)
-#define buzzerOff()               PORTB &= ~(1 << OUT_B_BUZZER)
+#define buzzerOn()                PORTH |=  (1 << OUT_H_SpeakerBuzzer)
+#define buzzerOff()               PORTH &= ~(1 << OUT_H_SpeakerBuzzer)
 
 // Speaker driver
 #if defined(AUDIO)
@@ -176,16 +174,23 @@ void pwrOff();
 #define speakerOff()              TCCR4A &= ~(1 << COM4A0)
 #endif
 
-// Voice driver
-#if defined(VOICE)
-#define WTV20SD_Clock_on              PORTG |=  (1<<OUT_G_WTV_CLK)
-#define WTV20SD_Clock_off             PORTG &= ~(1<<OUT_G_WTV_CLK)
-#define WTV20SD_Data_on               PORTE |=  (1<<OUT_E_WTV_DATA)
-#define WTV20SD_Data_off              PORTE &= ~(1<<OUT_E_WTV_DATA)
-#define WTV20SD_Reset_on              PORTH |=  (1<<OUT_H_WTV_RESET)
-#define WTV20SD_Reset_off             PORTH &= ~(1<<OUT_H_WTV_RESET)
-#define	WTV20SD_BUSY                  (PINB & 0x80)
-#define	WTV20SD_CLK                   (PING & 0x20)
+// Voice driver (WTV20)
+#if defined(VOICE_WTV20)
+#define WTV20_Data_on             PORTE |=  (1<<OUT_E_VoiceData)
+#define WTV20_Data_off            PORTE &= ~(1<<OUT_E_VoiceData)
+#define WTV20_BUSY                (PINB & (1<<INP_B_VoiceBuzy))
+#define WTV20_Clock_on            PORTG |=  (1<<OUT_G_VoiceClock)
+#define WTV20_Clock_off           PORTG &= ~(1<<OUT_G_VoiceClock)
+#define WTV20_CLK                 (PING & (1<<OUT_G_VoiceClock))
+#define WTV20_Reset_on            PORTH |=  (1<<OUT_H_VoiceReset)
+#define WTV20_Reset_off           PORTH &= ~(1<<OUT_H_VoiceReset)
+#endif
+
+// Voice driver (JQ6500)
+#if defined(VOICE_JQ6500)
+#define JQ6500_Serial_on          PORTE |=  (1<<OUT_E_VoiceData)
+#define JQ6500_Serial_off         PORTE &= ~(1<<OUT_E_VoiceData)
+#define JQ6500_BUSY               (PINB & (1<<INP_B_VoiceBuzy))
 #endif
 
 #endif // _BOARD_MEGA2560_H_
diff --git a/radio/src/targets/mega2560/i2c_driver.cpp b/radio/src/targets/mega2560/i2c_driver.cpp
new file mode 100644
index 000000000..a3cb4bb49
--- /dev/null
+++ b/radio/src/targets/mega2560/i2c_driver.cpp
@@ -0,0 +1,196 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+// From https://github.com/g4lvanix/I2C-master-lib
+
+#include <avr/io.h>
+#include <util/twi.h>
+
+#include "i2c_driver.h"
+
+//#define F_SCL 400000UL // SCL frequency
+#define F_SCL 800000UL // SCL frequency
+#define Prescaler 1
+#define TWBR_val ((((F_CPU / F_SCL) / Prescaler) - 16 ) / 2)
+
+void wait()
+{
+  while( !(TWCR & (1<<TWINT)) );
+}
+
+void i2c_init(void)
+{
+  TWBR = (uint8_t)TWBR_val;
+}
+
+uint8_t i2c_start(uint8_t address)
+{
+  // reset TWI control register
+  TWCR = 0;
+  // transmit START condition
+  TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
+  // wait for end of transmission
+  wait();
+
+  // check if the start condition was successfully transmitted
+  if((TWSR & 0xF8) != TW_START){ return 1; }
+
+  // load slave address into data register
+  TWDR = address;
+  // start transmission of address
+  TWCR = (1<<TWINT) | (1<<TWEN);
+  // wait for end of transmission
+  wait();
+  // check if the device has acknowledged the READ / WRITE mode
+  uint8_t twst = TW_STATUS & 0xF8;
+  if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return 1;
+
+  return 0;
+}
+
+uint8_t i2c_write(uint8_t data)
+{
+  // load data into data register
+  TWDR = data;
+  // start transmission of data
+  TWCR = (1<<TWINT) | (1<<TWEN);
+  // wait for end of transmission
+  wait();
+
+  if( (TWSR & 0xF8) != TW_MT_DATA_ACK ){ return 1; }
+
+  return 0;
+}
+
+uint8_t i2c_read_ack(void)
+{
+
+  // start TWI module and acknowledge data after reception
+  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
+  // wait for end of transmission
+  wait();
+  // return received data from TWDR
+  return TWDR;
+}
+
+uint8_t i2c_read_nack(void)
+{
+
+  // start receiving without acknowledging reception
+  TWCR = (1<<TWINT) | (1<<TWEN);
+  // wait for end of transmission
+  wait();
+  // return received data from TWDR
+  return TWDR;
+}
+
+uint8_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length)
+{
+  if (i2c_start(address | I2C_WRITE)) return 1;
+
+  for (uint16_t i = 0; i < length; i++)
+  {
+    if (i2c_write(data[i])) return 1;
+  }
+
+  i2c_stop();
+
+  return 0;
+}
+
+uint8_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length)
+{
+  if (i2c_start(address | I2C_READ)) return 1;
+
+  for (uint16_t i = 0; i < (length-1); i++)
+  {
+    data[i] = i2c_read_ack();
+  }
+  data[(length-1)] = i2c_read_nack();
+
+  i2c_stop();
+
+  return 0;
+}
+
+uint8_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length)
+{
+  if (i2c_start(devaddr | 0x00)) return 1;
+
+  i2c_write(regaddr);
+
+  for (uint16_t i = 0; i < length; i++)
+  {
+    if (i2c_write(data[i])) return 1;
+  }
+
+  i2c_stop();
+
+  return 0;
+}
+
+uint8_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length)
+{
+  if (i2c_start(devaddr)) return 1;
+
+  i2c_write(regaddr);
+
+  if (i2c_start(devaddr | 0x01)) return 1;
+
+  for (uint16_t i = 0; i < (length-1); i++)
+  {
+    data[i] = i2c_read_ack();
+  }
+  data[(length-1)] = i2c_read_nack();
+
+  i2c_stop();
+
+  return 0;
+}
+
+void i2c_stop(void)
+{
+  // transmit STOP condition
+  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
+}
+
+uint8_t iic_read (
+uint8_t dev,    /* Device address */
+uint8_t adr,    /* Read start address */
+uint16_t cnt,   /* Read uint8_t count */
+uint8_t *buff   /* Read data buffer */
+)
+{
+  uint8_t ret;
+  ret = i2c_readReg(dev, adr, buff, cnt);
+  return (!ret);
+}
+
+uint8_t iic_write (
+uint8_t dev,      /* Device address */
+uint8_t adr,      /* Write start address */
+uint16_t cnt,     /* Write uint8_t count */
+const uint8_t *buff /* Data to be written */
+)
+{
+  uint8_t ret;
+  ret = i2c_writeReg(dev, adr, (uint8_t *)buff, cnt);
+  return (!ret);
+}
diff --git a/radio/src/targets/mega2560/i2c_driver.h b/radio/src/targets/mega2560/i2c_driver.h
new file mode 100644
index 000000000..80f08ae9f
--- /dev/null
+++ b/radio/src/targets/mega2560/i2c_driver.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+// From https://github.com/g4lvanix/I2C-master-lib
+
+#ifndef I2C_DRIVER_H
+#define I2C_DRIVER_H
+
+#define I2C_READ 0x01
+#define I2C_WRITE 0x00
+
+void i2c_init(void);
+uint8_t i2c_start(uint8_t address);
+uint8_t i2c_write(uint8_t data);
+uint8_t i2c_read_ack(void);
+uint8_t i2c_read_nack(void);
+uint8_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length);
+uint8_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length);
+uint8_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length);
+uint8_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length);
+void i2c_stop(void);
+
+// M2560 functions
+
+uint8_t iic_read (
+uint8_t dev,    /* Device address */
+uint8_t adr,    /* Read start address */
+uint16_t cnt,   /* Read uint8_t count */
+uint8_t *buff   /* Read data buffer */
+);
+
+uint8_t iic_write (
+uint8_t dev,      /* Device address */
+uint8_t adr,      /* Write start address */
+uint16_t cnt,     /* Write uint8_t count */
+const uint8_t *buff /* Data to be written */
+);
+
+#endif // I2C_DRIVER_H
+/*
+##### void I2C_init(void)
+This function needs to be called only once to set up the correct SCL frequency
+for the bus
+
+##### uint8_t I2C_start(uint8_t address)
+This function needs to be called any time a connection to a new slave device should
+be established. The function returns 1 if an error has occurred, otherwise it returns
+0.
+
+The syntax to start a operation write to a device is either:
+`I2C_start(SLAVE_ADDRESS+I2C_WRITE);`
+or
+`I2C_start(SLAVE_WRITE_ADDRESS);`
+
+The syntax to start a read operation from a device is either:
+`I2C_start(SLAVE_ADDRESS+I2C_READ);`
+or
+`I2C_start(SLAVE_READ_ADDRESS);`
+
+##### uint8_t I2C_write(uint8_t data)
+This function is used to write data to the currently active device.
+The only parameter this function takes is the 8 bit unsigned integer to be sent.
+The function returns 1 if an error has occurred, otherwise it returns
+0.
+
+##### uint8_t I2C_read_ack(void)
+This function is used to read one byte from a device and request another byte of data
+after the transmission is complete by sending the acknowledge bit.
+This function returns the received byte.
+
+##### uint8_t I2C_read_nack(void)
+This function is used to read one byte from a device an then not requesting another
+byte and therefore stopping the current transmission.
+This function returns the received byte.
+
+##### uint8_t i2c_transmit(uint8_t address, uint8_t* data, uint16_t length);
+This function is used to transmit [length] number of bytes to an I2C device with the given
+I2C address from [data].
+The [address] passed to this function is the 7-bit slave address, left
+shifted by one bit (i.e. 7-bit slave address is `0x2F` -> `(0x2F)<<1` = `0x5E`)
+
+##### uint8_t i2c_receive(uint8_t address, uint8_t* data, uint16_t length);
+This function is used to read [length] number of bytes from the I2C device with the given
+I2C address into the [data].
+The [address] passed to this function is the 7-bit slave address, left
+shifted by one bit (i.e. 7-bit slave address is `0x2F` -> `(0x2F)<<1` = `0x5E`)
+
+##### void I2C_stop(void)
+This function disables the TWI peripheral completely
+and therefore disconnects the device from the bus.
+*/
diff --git a/radio/src/targets/mega2560/voice.cpp b/radio/src/targets/mega2560/voice.cpp
index cfa299712..789b3bf24 100644
--- a/radio/src/targets/mega2560/voice.cpp
+++ b/radio/src/targets/mega2560/voice.cpp
@@ -2,7 +2,7 @@
  * Copyright (C) OpenTX
  *
  * Based on code named
- *   th9x - http://code.google.com/p/th9x
+ *   th9x - http://code.google.com/p/th9x 
  *   er9x - http://code.google.com/p/er9x
  *   gruvin9x - http://code.google.com/p/gruvin9x
  *
@@ -18,147 +18,10 @@
  * GNU General Public License for more details.
  */
 
-// driver adapted from somo14d driver for gruvin9x board for driving WTV20SDMini module
-
 #include "opentx.h"
 
-// Start and stop bits need to be 2ms in duration. Start bit is low, stop bit is high
-#define WTV20SD_STOP_TIME    68 // This is the needed 2ms (4) + 30ms (60) to allow for the
-                                // point at which the busy flag is checkable + 2ms for saftey (4)
-#define WTV20SD_START_TIME   5  // The 2ms of a stop/start bit
-
-enum WTV20SD_State
-{
-  PAUSE,
-  SENDSTART,
-  SENDDATA,
-  SENDSTOP
-};
-
-#define QUEUE_LENGTH 10
-uint16_t WTV20SD_playlist[QUEUE_LENGTH] = {0};
-volatile uint8_t WTV20SD_InputIndex = 0;
-uint8_t WTV20SD_PlayIndex = 0;
-uint8_t Startstop = WTV20SD_START_TIME;
-uint16_t WTV20SD_current = 0;
-uint8_t state = PAUSE;
-
-void WTV20SD_sendstart()
-{
-  WTV20SD_Clock_off; // Start Bit, CLK low for 2ms
-
-  --Startstop;
-
-  if (!Startstop) state = SENDDATA;
-}
-
-void WTV20SD_senddata()
-{
-  static uint8_t i = 0;
-
-  if (!WTV20SD_CLK) {
-    // Only change data when the CLK is low
-    if (WTV20SD_current & 0x8000) {
-      WTV20SD_Data_on; // Data high
-    }
-    WTV20SD_current = (WTV20SD_current<<1);
-    ++i;
-    _delay_us(1); // Data setup delay
-    WTV20SD_Clock_on; // CLK high
-  }
-  else {
-    // Don't alter after sending last bit in preparation for sending stop bit
-    WTV20SD_Clock_off; // CLK low
-    WTV20SD_Data_off; // Data low
-  }
-
-  if (i == 16) { i = 0; Startstop = WTV20SD_STOP_TIME; state = SENDSTOP; }
-}
-
-void WTV20SD_sendstop()
-{
-  WTV20SD_Data_off; // Data low
-  WTV20SD_Clock_on; // Stop Bit, CLK high for 2ms
-  --Startstop;
-	
-  if (!Startstop && !WTV20SD_BUSY) state = PAUSE;
-}
-
-void pushPrompt(uint16_t prompt)
-{
-  // if mute active => no voice
-  if (g_eeGeneral.beepMode == e_mode_quiet) return;
-  
-  /* Load playlist and activate interrupt */
-  WTV20SD_playlist[WTV20SD_InputIndex] = prompt;
-  ++WTV20SD_InputIndex;
-  WTV20SD_InputIndex %= QUEUE_LENGTH;
-
-  if (!isPlaying()) {
-    TIMSK5 |= (1<<OCIE5A); // enable interrupts on Output Compare A Match
-  }
-}
-
-uint8_t isPlaying()
-{
-  /* interrupts active on Output Compare A Match ? */
-#if defined(SIMU)
-  return false;
-#else
-  return (TIMSK5 & (1<<OCIE5A));
-#endif
-}
-
-#if !defined(SIMU)
-ISR(TIMER5_COMPA_vect) // every 0.5ms normally, every 2ms during startup reset
-{
-  static uint8_t reset_dly=4;
-  static uint8_t reset_pause=150;
-
-  if (reset_dly) {
-    OCR5A=0x1f4;
-    reset_dly--;
-    WTV20SD_Reset_off;
-    WTV20SD_Data_off;
-    WTV20SD_Clock_on;
-  } // RESET low
-  else if (reset_pause) {
-    OCR5A=0x1f4;
-    reset_pause--;
-    WTV20SD_Reset_on;
-  } // RESET high
-  else {
-    OCR5A = 0x7d; // another 0.5ms
-
-    if (state == PAUSE) {
-      if (WTV20SD_PlayIndex == WTV20SD_InputIndex) {
-        TIMSK5 &= ~(1<<OCIE5A); // stop reentrance
-        TCNT5=0; // reset timer
-        return; // nothing else to play
-      }
-      else {
-        WTV20SD_current = WTV20SD_playlist[WTV20SD_PlayIndex];
-        ++WTV20SD_PlayIndex;
-        WTV20SD_PlayIndex %= QUEUE_LENGTH;
-        Startstop = WTV20SD_START_TIME;
-        state = SENDSTART;
-      }
-    } // end PAUSE
-
-    if (state == SENDSTART) {
-      WTV20SD_sendstart();
-      return;
-    }
-
-    if (state == SENDDATA) {
-      WTV20SD_senddata();
-      return;
-    }
-
-    if (state == SENDSTOP) {
-      WTV20SD_sendstop();
-      return;
-    }
-  }
-}
+#if defined(VOICE_WTV20)
+#include "targets/mega2560/voice_wtv20.cpp"
+#elif defined(VOICE_JQ6500)
+#include "targets/mega2560/voice_jq6500.cpp"
 #endif
diff --git a/radio/src/targets/mega2560/voice.h b/radio/src/targets/mega2560/voice.h
index 62ebfadc6..cd23b0936 100644
--- a/radio/src/targets/mega2560/voice.h
+++ b/radio/src/targets/mega2560/voice.h
@@ -23,7 +23,7 @@
 #define PROMPT_SYSTEM_BASE      480 
 
 extern void pushPrompt(uint16_t prompt);
-extern uint8_t isPlaying();
+extern bool isPlaying();
 
 #define I18N_PLAY_FUNCTION(lng, x, ...) void x(__VA_ARGS__)
 #define PLAY_FUNCTION(x, ...)           void x(__VA_ARGS__)
diff --git a/radio/src/targets/mega2560/voice_jq6500.cpp b/radio/src/targets/mega2560/voice_jq6500.cpp
new file mode 100644
index 000000000..a2adb0a1b
--- /dev/null
+++ b/radio/src/targets/mega2560/voice_jq6500.cpp
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+// JQ6500 mp3 module driver
+// 6 X 10 (bits) X 104µs = 6.24 mS to send a playfile command
+
+#include "opentx.h"
+
+#define QUEUE_LENGTH 16*2  //bytes
+
+enum JQ6500_State
+{
+  START = 0x7E, //Start
+  NUMBY = 0x04, //Num bytes follow
+  SELEC = 0X03, //Select file
+  FILEH = 0x00, //Dummy file
+  FILEL = 0x01, //Dummy file
+  TERMI = 0xEF  //Termination
+};
+
+JQ6500_State state = START;
+uint8_t JQ6500_playlist[QUEUE_LENGTH] = {0};
+volatile uint8_t JQ6500_InputIndex = 0;
+uint8_t JQ6500_PlayIndex = 0;
+
+bool isPlaying()
+{
+  /* interrupts active on Output Compare A Match ? */
+#if defined(SIMU)
+  return false;
+#else
+  return (TIMSK5 & (1<<OCIE5A));
+#endif
+}
+
+void pushPrompt(uint16_t prompt)
+{
+  // if mute active => no voice
+  if (g_eeGeneral.beepMode == e_mode_quiet) return;
+  ++prompt;  // With SDformatter, first FAT address = 1 ? Not all th time ??
+  /* Load playlist and activate interrupt */
+    JQ6500_playlist[JQ6500_InputIndex] = (uint8_t)(prompt >> 8);    // MSB first
+  ++JQ6500_InputIndex;
+  JQ6500_playlist[JQ6500_InputIndex] = (uint8_t)(prompt & 0xFF);  // LSB after
+  ++JQ6500_InputIndex;
+  if (JQ6500_InputIndex == QUEUE_LENGTH) JQ6500_InputIndex = 0;
+  if (!isPlaying()) {
+    TCNT5=0;
+    OCR5A = 0xFA;
+    TIMSK5 |= (1<<OCIE5A); // enable interrupts on Output Compare A Match
+  }
+}
+
+uint8_t JQ6500_sendbyte(uint8_t Data_byte)
+{
+  static uint8_t i = 0;
+  
+  if (!i) { JQ6500_Serial_off; ++i; return 0; }  // serial start bit
+
+  if (i==9) { JQ6500_Serial_on; i = 0; return 1; } // serial stop bit
+
+  if ((Data_byte >> (i-1)) & 0x01) { JQ6500_Serial_on;} else { JQ6500_Serial_off;} // send data bits
+  ++i;
+  return 0;
+}
+
+#if !defined(SIMU)
+ISR(TIMER5_COMPA_vect) // every 104µS
+{
+  sei();
+
+  if (state == START) { OCR5A = 0x19; if (JQ6500_BUSY) return; if (JQ6500_sendbyte(state)) { state = NUMBY; return; } }
+  
+  if (state == NUMBY) { if (JQ6500_sendbyte(state)) { state = SELEC; return; } }
+ 
+  if (state == SELEC) { if (JQ6500_sendbyte(state)) { state = FILEH; return; } }
+
+  if (state == FILEH) { if (JQ6500_sendbyte(JQ6500_playlist[JQ6500_PlayIndex])) { ++JQ6500_PlayIndex; state = FILEL; return; } }
+
+  if (state == FILEL) { if (JQ6500_sendbyte(JQ6500_playlist[JQ6500_PlayIndex])) { ++JQ6500_PlayIndex; state = TERMI; return; } }
+
+  if (state == TERMI) {
+    if (JQ6500_sendbyte(state)) {
+    state = START;
+    if (JQ6500_PlayIndex == QUEUE_LENGTH) JQ6500_PlayIndex = 0;
+    if (JQ6500_PlayIndex == JQ6500_InputIndex) { OCR5A = 0x19; TIMSK5 &= ~(1<<OCIE5A); } // stop reentrance
+    return; }
+    }
+
+  cli();
+}
+#endif
diff --git a/radio/src/targets/mega2560/voice_wtv20.cpp b/radio/src/targets/mega2560/voice_wtv20.cpp
new file mode 100644
index 000000000..9fb072d5c
--- /dev/null
+++ b/radio/src/targets/mega2560/voice_wtv20.cpp
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+// WTV20SDMini module driver, adapted from somo14d driver (gruvin9x board)
+
+#include "opentx.h"
+
+// Start and stop bits need to be 2ms in duration. Start bit is low, stop bit is high
+#define WTV20_STOP_TIME    84 // This is the needed 2ms (4) + 40ms (80) to allow for the
+// point at which the busy flag is checkable + 2ms for saftey (4)
+#define WTV20_START_TIME   5  // The 2ms of a stop/start bit
+
+enum WTV20_State
+{
+  RESET,
+  RESETPAUSE,
+  PAUSE,
+  SENDSTART,
+  SENDDATA,
+  SENDSTOP
+};
+
+#define QUEUE_LENGTH 10
+uint16_t WTV20_playlist[QUEUE_LENGTH] = {0};
+volatile uint8_t WTV20_InputIndex = 0;
+uint8_t WTV20_PlayIndex = 4; // Also used for reset,(save 2 static uint8_t)
+uint8_t Startstop = WTV20_START_TIME;
+uint16_t WTV20_current = 0;
+uint8_t state = RESET;
+
+void WTV20_sendstart()
+{
+  WTV20_Clock_off; // Start Bit, CLK low for 2ms
+
+  --Startstop;
+
+  if (!Startstop) state = SENDDATA;
+}
+
+void WTV20_senddata()
+{
+  static uint8_t i = 0;
+
+  if (!WTV20_CLK) {
+    // Only change data when the CLK is low
+    if (WTV20_current & 0x8000) {
+      WTV20_Data_on; // Data high
+    }
+    WTV20_current = (WTV20_current << 1);
+    ++i;
+    _delay_us(1); // Data setup delay
+    WTV20_Clock_on; // CLK high
+  }
+  else {
+    // Don't alter after sending last bit in preparation for sending stop bit
+    WTV20_Clock_off; // CLK low
+    WTV20_Data_off; // Data low
+  }
+
+  if (i == 16) { i = 0; Startstop = WTV20_STOP_TIME; state = SENDSTOP; }
+}
+
+void WTV20_sendstop()
+{
+  WTV20_Data_off; // Data low
+  WTV20_Clock_on; // Stop Bit, CLK high for 2ms
+  --Startstop;
+
+  if (!Startstop && !WTV20_BUSY) state = PAUSE;
+}
+
+void pushPrompt(uint16_t prompt)
+{
+  // if mute active => no voice
+  if (g_eeGeneral.beepMode == e_mode_quiet) return;
+
+  /* Load playlist and activate interrupt */
+  WTV20_playlist[WTV20_InputIndex] = prompt;
+  ++WTV20_InputIndex;
+  if (WTV20_InputIndex == QUEUE_LENGTH) WTV20_InputIndex = 0;
+
+  if (!isPlaying()) {
+    TIMSK5 |= (1<<OCIE5A); // enable interrupts on Output Compare A Match
+  }
+}
+
+bool isPlaying()
+{
+  /* interrupts active on Output Compare A Match ? */
+#if defined(SIMU)
+  return false;
+#else
+  return (TIMSK5 & (1<<OCIE5A));
+#endif
+}
+
+#if !defined(SIMU)
+ISR(TIMER5_COMPA_vect) // every 0.5ms normally, every 2ms during startup reset
+{
+  sei();
+  if (state == PAUSE) {
+    if (WTV20_PlayIndex == WTV20_InputIndex) {
+      TIMSK5 &= ~(1<<OCIE5A); // stop reentrance
+      TCNT5=0; // reset timer
+      return; // nothing else to play
+    }
+    else {
+      WTV20_current = WTV20_playlist[WTV20_PlayIndex];
+      ++WTV20_PlayIndex;
+      if (WTV20_PlayIndex == QUEUE_LENGTH) WTV20_PlayIndex = 0;
+      Startstop = WTV20_START_TIME;
+      state = SENDSTART;
+    }
+  } // end PAUSE
+
+  if (state == SENDSTART) {
+    WTV20_sendstart();
+    return;
+  }
+
+  if (state == SENDDATA) {
+    WTV20_senddata();
+    return;
+  }
+
+  if (state == SENDSTOP) {
+    WTV20_sendstop();
+    return;
+  }
+
+  if (state == RESET) {
+    if (WTV20_PlayIndex) { // WTV20_PlayIndex used as reset counter
+      // OCR5A=0x1f4; setted in board_mega2560.cpp
+      WTV20_Reset_off;
+      WTV20_Data_off;
+      WTV20_Clock_on;
+      --WTV20_PlayIndex;
+      return;
+    } // RESET low
+    else {
+      state = RESETPAUSE;
+      return;
+    }
+  }
+
+  if (state == RESETPAUSE) {
+    WTV20_Reset_on; // RESET high
+    if (!WTV20_BUSY) {
+      state = PAUSE;
+      OCR5A = 0x7d; // 0.5 ms after init
+    }
+  }
+  cli();
+}
+#endif