From 3e009decfbf2d6ef547ef66652dc947144d644b1 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Wed, 1 Feb 2012 18:55:08 +0000
Subject: [PATCH] HAPTIC is now able to work without AUDIO New function in
 function switches to play an Audio track Trims can be used to modify a mixer
 offset. Offsets for Frsky can go from -128 to +127 Vibrations motor on v4
 (HAPTIC option too)

---
 src/audio.cpp         |  13 +-
 src/audio.h           |  74 +++++-------
 src/beeper.cpp        |   3 +
 src/beeper.h          |  21 +++-
 src/general_menus.cpp |   4 +-
 src/model_menus.cpp   |  36 ++++--
 src/myeeprom.h        |  25 ++--
 src/o9xstrings.cpp    |   9 ++
 src/o9xstrings.h      |   6 +-
 src/open9x.cpp        | 272 +++++++++++++++++++++++-------------------
 src/open9x.h          |  35 +++++-
 src/translations/en.h |  11 +-
 src/translations/fr.h |  11 +-
 13 files changed, 296 insertions(+), 224 deletions(-)

diff --git a/src/audio.cpp b/src/audio.cpp
index 338b89bf3..04d6a635c 100644
--- a/src/audio.cpp
+++ b/src/audio.cpp
@@ -14,13 +14,6 @@
 
 #include "open9x.h"
 
-#if defined(PCBV4)
-#define SPEAKER_OFF  TCCR0A &= ~(1<<COM0A0)  // tone off
-#define SPEAKER_ON   TCCR0A |= (1<<COM0A0)   // tone on
-#else
-#define SPEAKER_OFF  PORTE &= ~(1 << OUT_E_BUZZER) // speaker output 'low'
-#endif
-
 audioQueue::audioQueue()
 {
   aqinit();
@@ -93,7 +86,6 @@ void audioQueue::heartbeat()
         tonePause = queueTonePause[t_queueRidx];
 #if defined(HAPTIC)
         toneHaptic = queueToneHaptic[t_queueRidx];
-        hapticTick = 0;
 #endif
         if (!queueToneRepeat[t_queueRidx]--) {
           t_queueRidx = (t_queueRidx + 1) % AUDIO_QUEUE_LENGTH;
@@ -132,7 +124,6 @@ void audioQueue::playNow(uint8_t tFreq, uint8_t tLen, uint8_t tPause,
     tonePause = tPause;
 #if defined(HAPTIC)
     toneHaptic = tHaptic;
-    hapticTick = 0;
 #endif
     toneFreqIncr = tFreqIncr;
     t_queueWidx = t_queueRidx;
@@ -237,8 +228,8 @@ void audioQueue::frskyevent(uint8_t e)
 }
 #endif
 
-void audioQueue::event(uint8_t e, uint8_t f) {
-
+void audioQueue::event(uint8_t e, uint8_t f)
+{
   uint8_t beepVal = g_eeGeneral.beeperVal;
 
   switch (e) {
diff --git a/src/audio.h b/src/audio.h
index e3b9258e2..158fc9270 100644
--- a/src/audio.h
+++ b/src/audio.h
@@ -22,19 +22,9 @@
 #define PCBSTD
 #endif
 
-
-// TODO remove them when everything commited in open9x.h!!!
-#if defined(HAPTIC)
-#if defined(PCBV4)
-#define HAPTIC_OFF  PORTD |=  (1<<7)
-#define HAPTIC_ON   PORTD &= ~(1<<7)
-#else
-#define HAPTIC_OFF  PORTG &= ~(1<<2)
-#define HAPTIC_ON   PORTG |=  (1<<2)
-#endif
-#else
-#define HAPTIC_OFF
-#define HAPTIC_ON
+#if defined(PCBSTD)
+#define SPEAKER_ON   BUZZER_ON
+#define SPEAKER_OFF  BUZZER_OFF
 #endif
 
 //audio
@@ -114,43 +104,41 @@ class audioQueue
 #endif
 
 #if defined(ISER9X)
-inline void driver() {
-  if (toneTimeLeft > 0) {	
-					switch (g_eeGeneral.speakerMode){					
-								case 0:
-						        	//stock beeper. simply turn port on for x time!
-							        if (toneTimeLeft > 0){
-							            PORTE |=  (1<<OUT_E_BUZZER); // speaker output 'high'
-							        } 	
-							        break;	
-							  case 1:
-									    static uint8_t toneCounter;
-									    toneCounter += toneFreq;
-									    if ((toneCounter & 0x80) == 0x80) {
-									      PORTE |= (1 << OUT_E_BUZZER);
-									    } else {
-									      PORTE &= ~(1 << OUT_E_BUZZER);
-									    }							  	
-											break;						  	
-					}		
-	} else {
-			PORTE &=  ~(1<<OUT_E_BUZZER); // speaker output 'low'
-	}								  	     
-}	
-#else 
-
-#if defined(PCBSTD)
+    inline void driver() {
+      if (toneTimeLeft > 0) {
+        switch (g_eeGeneral.speakerMode) {
+          case 0:
+            // stock beeper. simply turn port on for x time!
+            if (toneTimeLeft > 0){
+              PORTE |=  (1<<OUT_E_BUZZER); // speaker output 'high'
+            }
+            break;
+          case 1:
+            static uint8_t toneCounter;
+            toneCounter += toneFreq;
+            if ((toneCounter & 0x80) == 0x80) {
+              PORTE |= (1 << OUT_E_BUZZER);
+            }
+            else {
+              PORTE &= ~(1 << OUT_E_BUZZER);
+            }
+            break;
+        }
+      }
+      else {
+        PORTE &=  ~(1<<OUT_E_BUZZER); // speaker output 'low'
+      }
+    }
+#elif defined(PCBSTD)
     inline void driver() {
       if (toneTimeLeft > 0) {
         toneCounter += toneFreq;
         if ((toneCounter & 0x80) == 0x80)
-          PORTE |= (1 << OUT_E_BUZZER);
+          BUZZER_ON;
         else
-          PORTE &= ~(1 << OUT_E_BUZZER);
+          BUZZER_OFF;
       }
     }
-#endif
-
 #endif
 
     // heartbeat is responsibile for issueing the audio tones and general square waves
diff --git a/src/beeper.cpp b/src/beeper.cpp
index 7322bea5c..7ec0b4e83 100644
--- a/src/beeper.cpp
+++ b/src/beeper.cpp
@@ -27,6 +27,9 @@ uint8_t beepAgainOrig = 0;
 uint8_t beepOn = false;
 bool warble = false;
 bool warbleC;
+#if defined(HAPTIC)
+uint8_t hapticTick = 0;
+#endif
 
 // The various "beep" tone lengths
 static prog_uint8_t APM beepTab[]= {
diff --git a/src/beeper.h b/src/beeper.h
index d874d4802..632d2db89 100644
--- a/src/beeper.h
+++ b/src/beeper.h
@@ -28,6 +28,9 @@ extern uint8_t beepAgainOrig;
 extern uint8_t beepOn;
 extern bool warble;
 extern bool warbleC;
+#if defined(HAPTIC)
+extern uint8_t hapticTick;
+#endif
 
 extern void beep(uint8_t val);
 
@@ -72,17 +75,25 @@ FORCEINLINE void AUDIO_HEARTBEAT()
         }
     }
 
-    // G: use original external buzzer for beeps
     if (beepOn) {
-      //static bool warbleC;
       warbleC = warble && !warbleC;
       if (warbleC)
-        PORTE &= ~(1 << OUT_E_BUZZER); // speaker output 'low'
+        BUZZER_OFF;
       else
-        PORTE |= (1 << OUT_E_BUZZER); // speaker output 'high'
+        BUZZER_ON;
+#if defined(HAPTIC)
+      if (hapticTick-- > 0) {
+        HAPTIC_ON; // haptic output 'high'
+      }
+      else {
+        HAPTIC_OFF; // haptic output 'low'
+        hapticTick = g_eeGeneral.hapticStrength;
+      }
+#endif
     }
     else {
-      PORTE &= ~(1 << OUT_E_BUZZER); // speaker output 'low'
+      BUZZER_OFF;
+      HAPTIC_OFF;
     }
 }
 
diff --git a/src/general_menus.cpp b/src/general_menus.cpp
index 03057c78e..d7e6c358a 100644
--- a/src/general_menus.cpp
+++ b/src/general_menus.cpp
@@ -95,7 +95,7 @@ void menuProcSetup(uint8_t event)
 
 #ifdef AUDIO
   if(s_pgOfs<subN) {
-    lcd_puts_P(0, y, PSTR("Speaker Pitch"));
+    lcd_puts_P(0, y, STR_SPKRPITCH);
     lcd_outdezAtt(PARAM_OFS, y, g_eeGeneral.speakerPitch,(sub==subN ? INVERS : 0)|LEFT);
     if (sub==subN) {
       CHECK_INCDEC_GENVAR(event, g_eeGeneral.speakerPitch, 0, 100);
@@ -106,7 +106,7 @@ void menuProcSetup(uint8_t event)
 
 #ifdef HAPTIC
   if(s_pgOfs<subN) {
-    lcd_puts_P(0, y, PSTR("Haptic Strength"));
+    lcd_puts_P(0, y, STR_HAPTICSTRENGTH);
     lcd_outdezAtt(PARAM_OFS, y, g_eeGeneral.hapticStrength, (sub==subN ? INVERS : 0)|LEFT);
     if (sub==subN) {
       CHECK_INCDEC_GENVAR(event, g_eeGeneral.hapticStrength, 0, 5);
diff --git a/src/model_menus.cpp b/src/model_menus.cpp
index cecb7215c..32fdae3a0 100644
--- a/src/model_menus.cpp
+++ b/src/model_menus.cpp
@@ -1165,9 +1165,10 @@ void menuProcMixOne(uint8_t event)
         break;
       case 3:
         // TODO hidden when src is not a STICK as it has no sense
-        lcd_puts_P(  2*FW,y,STR_TRIM);
-        lcd_putsnAtt(FW*10,y, STR_OFFON+LEN_OFFON*(1-md2->carryTrim), LEN_OFFON, attr);
-        if(attr) CHECK_INCDEC_MODELVAR( event, md2->carryTrim, 0,1);
+        lcd_puts_P(2*FW, y, STR_TRIM);
+        // lcd_putsnAtt(FW*10, y, STR_OFFON+LEN_OFFON*(1-md2->carryTrim), LEN_OFFON, attr);
+        lcd_putsnAtt(FW*10, y, STR_VMIXTRIMS+LEN_VMIXTRIMS*md2->carryTrim, LEN_VMIXTRIMS, attr); // TODO perhaps could be optimized by reusing STR_OFFON
+        if (attr) CHECK_INCDEC_MODELVAR( event, md2->carryTrim, 0, 2);
         break;
       case 4:
         lcd_puts_P(2*FW, y, STR_CURVES);
@@ -1193,7 +1194,7 @@ void menuProcMixOne(uint8_t event)
         if(md2->mixWarn)
           lcd_outdezAtt(FW*10,y,md2->mixWarn,attr|LEFT);
         else
-          lcd_puts_P(  FW*10,y,STR_OFF);
+          lcd_putsAtt(FW*10, y, STR_OFF, attr);
         if(attr) CHECK_INCDEC_MODELVAR( event, md2->mixWarn, 0,3);
         break;
       case 8:
@@ -1637,7 +1638,7 @@ void menuProcCustomSwitches(uint8_t event)
     lcd_putsnAtt(4*FW - 1, y, STR_VCSWFUNC+LEN_VCSWFUNC*cs.func, LEN_VCSWFUNC, m_posHorz==0 ? attr : 0);
 
     uint8_t cstate = CS_STATE(cs.func);
-    int8_t v1_min=0, v1_max=NUM_XCHNRAW, v2_min=0, v2_max=NUM_XCHNRAW;
+    int8_t v1_min=0, v1_max=NUM_XCHNCSW, v2_min=0, v2_max=NUM_XCHNCSW;
 
     if(cstate == CS_VOFS)
     {
@@ -1645,26 +1646,26 @@ void menuProcCustomSwitches(uint8_t event)
 
 #if defined(FRSKY)
 #if defined(FRSKY_HUB)
-        if (cs.v1 > NUM_XCHNRAW-NUM_TELEMETRY+3) {
+        if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY+3) {
           lcd_outdezAtt(20*FW, y, (128+cs.v2) * 50, m_posHorz==2 ? attr : 0);
           v2_min = -128; v2_max = 127;
         }
         else
 #endif
 #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
-        if (cs.v1 > NUM_XCHNRAW-NUM_TELEMETRY+2) {
+        if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY+2) {
           lcd_outdezAtt(20*FW, y, (128+cs.v2) * 4, m_posHorz==2 ? attr : 0);
           v2_min = -128; v2_max = 127;
         }
         else
 #endif
-        if (cs.v1 > NUM_XCHNRAW-NUM_TELEMETRY) {
-          putsTelemetryChannel(20*FW, y, cs.v1+NUM_TELEMETRY-NUM_XCHNRAW-1, 128+cs.v2, m_posHorz==2 ? attr : 0);
+        if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY) {
+          putsTelemetryChannel(20*FW, y, cs.v1+NUM_TELEMETRY-NUM_XCHNCSW-1, 128+cs.v2, m_posHorz==2 ? attr : 0);
           v2_min = -128; v2_max = 127;
         }
         else
 #endif
-        if (cs.v1 > NUM_XCHNRAW-NUM_TELEMETRY-MAX_TIMERS) {
+        if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY-MAX_TIMERS) {
           putsTime(17*FW, y, 98+cs.v2, m_posHorz==2 ? attr : 0, m_posHorz==2 ? attr : 0); // TODO optim
           v2_min = -128; v2_max = 127;
         }
@@ -1719,7 +1720,7 @@ void menuProcCustomSwitches(uint8_t event)
 
 void menuProcFunctionSwitches(uint8_t event)
 {
-  MENU(STR_MENUFUNCSWITCHES, menuTabModel, e_FunctionSwitches, NUM_FSW+1, {0, 1/*repeated*/});
+  MENU(STR_MENUFUNCSWITCHES, menuTabModel, e_FunctionSwitches, NUM_FSW+1, {0, 2/*repeated*/});
 
   uint8_t y = 0;
   uint8_t k = 0;
@@ -1730,7 +1731,7 @@ void menuProcFunctionSwitches(uint8_t event)
     k=i+s_pgOfs;
     if(k==NUM_CHNOUT) break;
     FuncSwData *sd = &g_model.funcSw[k];
-    for (uint8_t j=0; j<2; j++) {
+    for (uint8_t j=0; j<3; j++) {
       uint8_t attr = ((sub==k && m_posHorz==j) ? ((s_editMode>0) ? BLINK : INVERS) : 0);
       uint8_t active = (attr && (s_editMode>0 || p1valdiff));
       switch (j) {
@@ -1751,6 +1752,17 @@ void menuProcFunctionSwitches(uint8_t event)
             m_posHorz = 0;
           }
           break;
+        case 2:
+          if (sd->swtch && sd->func == FUNC_PLAY_SOUND) {
+            lcd_outdezAtt(19*FW, y, sd->param, attr);
+            if (active) {
+              CHECK_INCDEC_MODELVAR( event, sd->param, 0, 15);
+            }
+          }
+          else if (attr) {
+            m_posHorz = 0;
+          }
+          break;
       }
     }
   }
diff --git a/src/myeeprom.h b/src/myeeprom.h
index fbadc00d7..0659777e3 100644
--- a/src/myeeprom.h
+++ b/src/myeeprom.h
@@ -36,12 +36,7 @@
 #define WARN_MEM     (!(g_eeGeneral.warnOpts & WARN_MEM_BIT))
 #define BEEP_VAL     ( (g_eeGeneral.warnOpts & WARN_BVAL_BIT) >>3 )
 
-#define EEPROM_ER9X_MIN  4
-#define EEPROM_ER9X_MAX  9
-
-#define EEPROM_VER_r584  3
-#define EEPROM_VER_r751  5
-#define EEPROM_VER       202
+#define EEPROM_VER       203
 
 #ifndef PACK
 #define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__))
@@ -142,13 +137,13 @@ PACK(typedef struct t_MixData {
   uint8_t srcRaw;            //
   int8_t  weight;
   int8_t  swtch;
-  uint8_t curve;             // 0=symmetrisch, 1=no neg, 2=no pos
+  uint8_t curve;
   uint8_t delayUp:4;
   uint8_t delayDown:4;
   uint8_t speedUp:4;         // Servogeschwindigkeit aus Tabelle (10ms Cycle)
   uint8_t speedDown:4;       // 0 nichts
-  uint8_t carryTrim:1;
-  uint8_t mltpx:3;           // multiplex method 0=+ 1=* 2=replace
+  uint8_t carryTrim:2;
+  uint8_t mltpx:2;           // multiplex method: 0 means +=, 1 means *=, 2 means :=
   int8_t  phase:4;           // -5=!FP4, 0=normal, 5=FP4
   int8_t  sOffset;
 }) MixData;
@@ -173,6 +168,7 @@ enum Functions {
   FUNC_TRAINER_AIL,
   FUNC_INSTANT_TRIM,
   FUNC_TRIMS_2_OFS,
+  FUNC_PLAY_SOUND,
 #ifdef LOGS
   FUNC_LOGS,
 #endif
@@ -181,19 +177,20 @@ enum Functions {
 
 PACK(typedef struct t_FuncSwData { // Function Switches data
   int8_t  swtch; //input
-  uint8_t func;
+  uint8_t func:4;
+  uint8_t param:4;
 }) FuncSwData;
 
 PACK(typedef struct t_FrSkyChannelData {
   uint8_t   ratio;              // 0.0 means not used, 0.1V steps EG. 6.6 Volts = 66. 25.1V = 251, etc.
-  uint8_t   type:4;             // channel unit (0=volts, ...)
-  int8_t    offset:4;           // calibration offset. Signed 0.1V steps. EG. -4 to substract 0.4V
+  uint8_t   type;               // channel unit (0=volts, ...)
   uint8_t   alarms_value[2];    // 0.1V steps EG. 6.6 Volts = 66. 25.1V = 251, etc. 
   uint8_t   alarms_level:4;
   uint8_t   alarms_greater:2;   // 0=LT(<), 1=GT(>)
   uint8_t   multiplier:2;       // 0=no multiplier, 1=*2 multiplier
-  uint8_t   barMin;             // minimum for bar display
-  uint8_t   barMax;             // ditto for max display (would usually = ratio)
+  uint8_t   barMin:4;           // minimum for bar display
+  uint8_t   barMax:4;           // ditto for max display (would usually = ratio)
+  int8_t    offset;
 }) FrSkyChannelData;
 
 #define PROTO_FRSKY_HUB   1
diff --git a/src/o9xstrings.cpp b/src/o9xstrings.cpp
index 7bc3494d2..227fa5bf6 100644
--- a/src/o9xstrings.cpp
+++ b/src/o9xstrings.cpp
@@ -19,6 +19,7 @@ const prog_char APM STR_OPEN9X[] =
     TR_EXPLABELS
     TR_VMLTPX
     TR_VMLTPX2
+    TR_VMIXTRIMS
     TR_VCSWFUNC
     TR_VFSWFUNC
 #ifdef FRSKY
@@ -235,3 +236,11 @@ const prog_char APM STR_RXNUM[] = TR_RXNUM;
 #if defined(PXX)
 const prog_char APM STR_SYNCMENU[] = TR_SYNCMENU;
 #endif
+
+#if defined(HAPTIC)
+const prog_char APM STR_HAPTICSTRENGTH[] = TR_HAPTICSTRENGTH;
+#endif
+
+#if defined(AUDIO)
+const prog_char APM STR_SPKRPITCH[] = TR_SPKRPITCH;
+#endif
diff --git a/src/o9xstrings.h b/src/o9xstrings.h
index bed8e4b98..450c445ff 100644
--- a/src/o9xstrings.h
+++ b/src/o9xstrings.h
@@ -32,7 +32,8 @@ extern const PROGMEM char STR_OPEN9X[];
 #define OFS_EXPLABELS  (OFS_CURVMODES + PSIZE(TR_CURVMODES))
 #define OFS_VMLTPX     (OFS_EXPLABELS + PSIZE(TR_EXPLABELS))
 #define OFS_VMLTPX2    (OFS_VMLTPX + PSIZE(TR_VMLTPX))
-#define OFS_VCSWFUNC   (OFS_VMLTPX2 + PSIZE(TR_VMLTPX2))
+#define OFS_VMIXTRIMS  (OFS_VMLTPX2 + PSIZE(TR_VMLTPX2))
+#define OFS_VCSWFUNC   (OFS_VMIXTRIMS + PSIZE(TR_VMIXTRIMS))
 #define OFS_VFSWFUNC   (OFS_VCSWFUNC + PSIZE(TR_VCSWFUNC))
 #ifdef FRSKY
 #define OFS_VTELEMUNIT (OFS_VFSWFUNC + PSIZE(TR_VFSWFUNC))
@@ -91,6 +92,7 @@ extern const PROGMEM char STR_OPEN9X[];
 #define STR_EXPLABELS  (STR_OPEN9X + OFS_EXPLABELS)
 #define STR_VMLTPX     (STR_OPEN9X + OFS_VMLTPX)
 #define STR_VMLTPX2    (STR_OPEN9X + OFS_VMLTPX2)
+#define STR_VMIXTRIMS  (STR_OPEN9X + OFS_VMIXTRIMS)
 #define STR_VCSWFUNC   (STR_OPEN9X + OFS_VCSWFUNC)
 #define STR_VFSWFUNC   (STR_OPEN9X + OFS_VFSWFUNC)
 
@@ -288,5 +290,7 @@ extern const PROGMEM char STR_MENUSTAT[];
 extern const PROGMEM char STR_MENUDEBUG[];
 extern const PROGMEM char STR_RXNUM[];
 extern const PROGMEM char STR_SYNCMENU[];
+extern const PROGMEM char STR_HAPTICSTRENGTH[];
+extern const PROGMEM char STR_SPKRPITCH[];
 
 #endif
diff --git a/src/open9x.cpp b/src/open9x.cpp
index e528c5136..6e4c40f03 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -34,13 +34,6 @@ prog_uchar APM speMarker[] = { "SPE" };
 gtime_t g_unixTime; // Global date/time register, incremented each second in per10ms()
 #endif
 
-/*
-mode1 rud ele thr ail
-mode2 rud thr ele ail
-mode3 ail ele thr rud
-mode4 ail thr ele rud
-*/
-
 EEGeneral  g_eeGeneral;
 ModelData  g_model;
 
@@ -70,6 +63,12 @@ const prog_uint8_t APM chout_ar[] = { //First number is 0..23 -> template setup,
                                       3,1,2,4 , 3,1,4,2 , 3,2,1,4 , 3,2,4,1 , 3,4,1,2 , 3,4,2,1,
                                       4,1,2,3 , 4,1,3,2 , 4,2,1,3 , 4,2,3,1 , 4,3,1,2 , 4,3,2,1    };
 
+/*
+mode1 rud ele thr ail
+mode2 rud thr ele ail
+mode3 ail ele thr rud
+mode4 ail thr ele rud
+*/
 const prog_uint8_t APM modn12x3[]= {
     1, 2, 3, 4,
     1, 3, 2, 4,
@@ -438,15 +437,27 @@ uint8_t getFlightPhase()
 
 int16_t getTrimValue(uint8_t phase, uint8_t idx)
 {
-  PhaseData *p = phaseaddress(phase);
-  return (((int16_t)p->trim[idx]) << 2) + ((p->trim_ext >> (2*idx)) & 0x03);
+  int16_t result;
+  if (s_trimPtr[idx]) {
+    result = *s_trimPtr[idx];
+  }
+  else {
+    PhaseData *p = phaseaddress(phase);
+    result = (((int16_t)p->trim[idx]) << 2) + ((p->trim_ext >> (2*idx)) & 0x03);
+  }
+  return result;
 }
 
 void setTrimValue(uint8_t phase, uint8_t idx, int16_t trim)
 {
-  PhaseData *p = phaseaddress(phase);
-  p->trim[idx] = (int8_t)(trim >> 2);
-  p->trim_ext = (p->trim_ext & ~(0x03 << (2*idx))) + (((trim & 0x03) << (2*idx)));
+  if (s_trimPtr[idx]) {
+    *s_trimPtr[idx] = limit((int16_t)-125, trim, (int16_t)+125);
+  }
+  else {
+    PhaseData *p = phaseaddress(phase);
+    p->trim[idx] = (int8_t)(trim >> 2);
+    p->trim_ext = (p->trim_ext & ~(0x03 << (2*idx))) + (((trim & 0x03) << (2*idx)));
+  }
   STORE_MODELVARS;
 }
 
@@ -668,6 +679,8 @@ void alert(const prog_char * s, bool defaults)
   }
 }
 
+int8_t *s_trimPtr[NUM_STICKS] = { NULL, NULL, NULL, NULL };
+
 uint8_t checkTrim(uint8_t event)
 {
   int8_t  k = (event & EVT_KEY_MASK) - TRM_BASE;
@@ -1037,10 +1050,21 @@ void evalFunctions()
     FuncSwData *sd = &g_model.funcSw[i];
     if (sd->swtch && sd->func) {
       uint16_t mask = (1 << (sd->func-1));
-      if (getSwitch(sd->swtch, 0))
+      if (getSwitch(sd->swtch, 0)) {
+        if (sd->func == FUNC_PLAY_SOUND && (~active_functions & mask)) {
+#if defined(AUDIO)
+          audioDefevent(sd->param);
+#else
+          if (g_eeGeneral.beeperVal>0) {
+            _beep(min(1, sd->param*10));
+          }
+#endif
+        }
         active_functions |= mask;
-      else
+      }
+      else {
         active_functions &= (~mask);
+      }
     }
   }
 }
@@ -1135,127 +1159,135 @@ void perOut(int16_t *chanOut, uint8_t phase)
     for (uint8_t i=NUM_CAL_PPM; i<NUM_PPM; i++) anas[i+PPM_BASE] = g_ppmIns[i]*2; // add ppm channels
   }
   
-  for (uint8_t i=CHOUT_BASE; i<NUM_XCHNRAW; i++) anas[i] = chans[i-CHOUT_BASE]; // other mixes previous outputs
+  for (uint8_t i=CHOUT_BASE; i<CHOUT_BASE+NUM_CHNOUT; i++) anas[i] = chans[i-CHOUT_BASE]; // other mixes previous outputs
 
   memset(chans, 0, sizeof(chans));        // All outputs to 0
 
-    //========== MIXER LOOP ===============
-    mixWarning = 0;
-    for(uint8_t i=0;i<MAX_MIXERS;i++){
-      MixData *md = mixaddress( i ) ;
+  s_trimPtr[0] = NULL;
+  s_trimPtr[1] = NULL;
+  s_trimPtr[2] = NULL;
+  s_trimPtr[3] = NULL;
 
-      if((md->destCh==0) || (md->destCh>NUM_CHNOUT)) break;
+  //========== MIXER LOOP ===============
+  mixWarning = 0;
+  for(uint8_t i=0;i<MAX_MIXERS;i++){
+    MixData *md = mixaddress( i ) ;
 
-      if (md->phase != 0) {
-        if (md->phase > 0) {
-          if (phase+1 != md->phase)
-            continue;
-        }
-        else {
-          if (phase+1 == -md->phase)
-            continue;
-        }
-      }
+    if((md->destCh==0) || (md->destCh>NUM_CHNOUT)) break;
 
-      //Notice 0 = NC switch means not used -> always on line
-      int16_t v  = 0;
-      uint8_t swTog;
-
-      //swOn[i]=false;
-      if(!getSwitch(md->swtch,1)){ // switch on?  if no switch selected => on
-        swTog = swOn[i];
-        swOn[i] = false;
-        if(md->srcRaw!=MIX_MAX && md->srcRaw!=MIX_FULL) continue;// if not MAX or FULL - next loop
-        if(md->mltpx==MLTPX_REP) continue; // if switch is off and REPLACE then off
-        v = (md->srcRaw == MIX_FULL ? -RESX : 0); // switch is off and it is either MAX=0 or FULL=-512
+    if (md->phase != 0) {
+      if (md->phase > 0) {
+        if (phase+1 != md->phase)
+          continue;
       }
       else {
-        swTog = !swOn[i];
-        swOn[i] = true;
-        uint8_t k = md->srcRaw-1;
-        v = anas[k]; //Switch is on. MAX=FULL=512 or value.
-        if(k>=CHOUT_BASE && (k<i)) v = chans[k]; // if we've already calculated the value - take it instead // anas[i+CHOUT_BASE] = chans[i]
-        if(md->mixWarn) mixWarning |= 1<<(md->mixWarn-1); // Mix warning
+        if (phase+1 == -md->phase)
+          continue;
       }
+    }
 
-      //========== INPUT OFFSET ===============
-      if(md->sOffset) v += calc100toRESX(md->sOffset);
+    //Notice 0 = NC switch means not used -> always on line
+    int16_t v  = 0;
+    uint8_t swTog;
 
-      //========== DELAY and PAUSE ===============
-      if (md->speedUp || md->speedDown || md->delayUp || md->delayDown)  // there are delay values
-      {
+    //swOn[i]=false;
+    if(!getSwitch(md->swtch,1)){ // switch on?  if no switch selected => on
+      swTog = swOn[i];
+      swOn[i] = false;
+      if(md->srcRaw!=MIX_MAX && md->srcRaw!=MIX_FULL) continue;// if not MAX or FULL - next loop
+      if(md->mltpx==MLTPX_REP) continue; // if switch is off and REPLACE then off
+      v = (md->srcRaw == MIX_FULL ? -RESX : 0); // switch is off and it is either MAX=0 or FULL=-512
+    }
+    else {
+      swTog = !swOn[i];
+      swOn[i] = true;
+      uint8_t k = md->srcRaw-1;
+      v = anas[k]; //Switch is on. MAX=FULL=512 or value.
+      if (k>=CHOUT_BASE && (k<i)) v = chans[k]; // if we've already calculated the value - take it instead // anas[i+CHOUT_BASE] = chans[i]
+      if (md->mixWarn) mixWarning |= 1<<(md->mixWarn-1); // Mix warning
+      if (md->carryTrim == 2/*TODO constant*/ && md->srcRaw <= NUM_STICKS) {
+        s_trimPtr[md->srcRaw-1] = &md->sOffset;  // use the value stored here for the trim
+      }
+    }
+
+    //========== INPUT OFFSET ===============
+    if(md->sOffset) v += calc100toRESX(md->sOffset);
+
+    //========== DELAY and PAUSE ===============
+    if (md->speedUp || md->speedDown || md->delayUp || md->delayDown)  // there are delay values
+    {
 #define DEL_MULT 256
 
-        int16_t diff = v-act[i]/DEL_MULT;
+      int16_t diff = v-act[i]/DEL_MULT;
 
-        if(swTog) {
-            //need to know which "v" will give "anas".
-            //curves(v)*weight/100 -> anas
-            // v * weight / 100 = anas => anas*100/weight = v
-          if(md->mltpx==MLTPX_REP)
-          {
-              act[i] = (int32_t)anas[md->destCh-1+CHOUT_BASE]*DEL_MULT;
-              act[i] *=100;
-              if(md->weight) act[i] /= md->weight;
-          }
-          diff = v-act[i]/DEL_MULT;
-          if(diff) sDelay[i] = (diff<0 ? md->delayUp :  md->delayDown) * 100;
+      if(swTog) {
+          //need to know which "v" will give "anas".
+          //curves(v)*weight/100 -> anas
+          // v * weight / 100 = anas => anas*100/weight = v
+        if(md->mltpx==MLTPX_REP)
+        {
+            act[i] = (int32_t)anas[md->destCh-1+CHOUT_BASE]*DEL_MULT;
+            act[i] *=100;
+            if(md->weight) act[i] /= md->weight;
         }
+        diff = v-act[i]/DEL_MULT;
+        if(diff) sDelay[i] = (diff<0 ? md->delayUp :  md->delayDown) * 100;
+      }
 
-        if (sDelay[i]) { // perform delay
-          if(tick10ms) sDelay[i]--;
-          if (sDelay[i] != 0) {
-            v = act[i]/DEL_MULT; // Stay in old position until delay over
-            diff = 0;
-          }
-        }
-
-        if (diff && (md->speedUp || md->speedDown)) {
-          //rate = steps/sec => 32*1024/100*md->speedUp/Down
-          //act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown);
-          //-100..100 => 32768 ->  100*83886/256 = 32768,   For MAX we divide by 2 sincde it's asymmetrical
-          if(tick10ms) {
-              int32_t rate = (int32_t)DEL_MULT*2048*100;
-              if(md->weight) rate /= abs(md->weight);
-              // TODO port optim er9x by Mike
-              act[i] = (diff>0) ? ((md->speedUp>0)   ? act[i]+(rate)/((int16_t)100*md->speedUp)   :  (int32_t)v*DEL_MULT) :
-                                  ((md->speedDown>0) ? act[i]-(rate)/((int16_t)100*md->speedDown) :  (int32_t)v*DEL_MULT) ;
-          }
-
-          if(((diff>0) && (v<(act[i]/DEL_MULT))) || ((diff<0) && (v>(act[i]/DEL_MULT)))) act[i]=(int32_t)v*DEL_MULT; //deal with overflow
-          v = act[i]/DEL_MULT;
-        }
-        else if (diff) {
-          act[i]=(int32_t)v*DEL_MULT;
+      if (sDelay[i]) { // perform delay
+        if(tick10ms) sDelay[i]--;
+        if (sDelay[i] != 0) {
+          v = act[i]/DEL_MULT; // Stay in old position until delay over
+          diff = 0;
         }
       }
 
-      //========== CURVES ===============
-      if (md->curve)
-        v = applyCurve(v, md->curve, md->srcRaw);
-
-      //========== TRIM ===============
-      if((md->carryTrim==0) && (md->srcRaw>0) && (md->srcRaw<=4)) v += trims[md->srcRaw-1];  //  0 = Trim ON  =  Default
-
-      //========== MULTIPLEX ===============
-      int32_t dv = (int32_t)v*md->weight;
-      int32_t *ptr = &chans[md->destCh-1]; // Save calculating address several times
-      switch(md->mltpx){
-        case MLTPX_REP:
-          *ptr = dv;
-          break;
-        case MLTPX_MUL:
-          dv /= 100;
-          dv *= *ptr;
-          dv /= RESXl;
-          *ptr = dv;
-          break;
-        default:  // MLTPX_ADD
-          *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100);
-          break;
+      if (diff && (md->speedUp || md->speedDown)) {
+        //rate = steps/sec => 32*1024/100*md->speedUp/Down
+        //act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown);
+        //-100..100 => 32768 ->  100*83886/256 = 32768,   For MAX we divide by 2 sincde it's asymmetrical
+        if(tick10ms) {
+            int32_t rate = (int32_t)DEL_MULT*2048*100;
+            if(md->weight) rate /= abs(md->weight);
+            // TODO port optim er9x by Mike
+            act[i] = (diff>0) ? ((md->speedUp>0)   ? act[i]+(rate)/((int16_t)100*md->speedUp)   :  (int32_t)v*DEL_MULT) :
+                                ((md->speedDown>0) ? act[i]-(rate)/((int16_t)100*md->speedDown) :  (int32_t)v*DEL_MULT) ;
         }
+
+        if(((diff>0) && (v<(act[i]/DEL_MULT))) || ((diff<0) && (v>(act[i]/DEL_MULT)))) act[i]=(int32_t)v*DEL_MULT; //deal with overflow
+        v = act[i]/DEL_MULT;
+      }
+      else if (diff) {
+        act[i]=(int32_t)v*DEL_MULT;
+      }
     }
 
+    //========== CURVES ===============
+    if (md->curve)
+      v = applyCurve(v, md->curve, md->srcRaw);
+
+    //========== TRIM ===============
+    if((md->carryTrim==0) && (md->srcRaw>0) && (md->srcRaw<=4)) v += trims[md->srcRaw-1];  //  0 = Trim ON  =  Default
+
+    //========== MULTIPLEX ===============
+    int32_t dv = (int32_t)v*md->weight;
+    int32_t *ptr = &chans[md->destCh-1]; // Save calculating address several times
+    switch(md->mltpx){
+      case MLTPX_REP:
+        *ptr = dv;
+        break;
+      case MLTPX_MUL:
+        dv /= 100;
+        dv *= *ptr;
+        dv /= RESXl;
+        *ptr = dv;
+        break;
+      default:  // MLTPX_ADD
+        *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100);
+        break;
+      }
+  }
+
   //========== MIXER WARNING ===============
   // 1,2 or 3 "bips" (short beeps)
   //1= 00,08
@@ -1631,7 +1663,7 @@ void perMain()
 #endif
 
   lcd_clear();
-  uint8_t evt=getEvent();
+  uint8_t evt = getEvent();
   evt = checkTrim(evt);
 
   // TODO port lightOnStickMove from er9x
@@ -1777,15 +1809,7 @@ ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer
   sei();
   
 #if defined (PCBSTD) && defined (AUDIO)
-  if (audio.toneTimeLeft > 0) {
-    static uint8_t toneCounter;
-    toneCounter += audio.toneFreq;
-    if ((toneCounter & 0x80) == 0x80)
-      PORTE |= (1 << OUT_E_BUZZER);
-    else
-      PORTE &= ~(1 << OUT_E_BUZZER);
-  }
-
+  AUDIO_DRIVER();
   static uint8_t cnt10ms = 77; // execute 10ms code once every 78 ISRs
   if (cnt10ms-- == 0) { // BEGIN { ... every 10ms ... }
     // Begin 10ms event
@@ -2094,7 +2118,7 @@ int main(void)
 #if defined (PCBV4)
   DDRB = 0b11000111;  PORTB = 0b00111111; // 7:SPKR, 6:PPM_OUT,  5:TrainSW,  4:IDL2_SW, SDCARD[3:MISO 2:MOSI 1:SCK 0:CS]
   DDRC = 0x3f;  PORTC = 0xc0; // 7:AilDR, 6:EleDR, LCD[5,4,3,2,1], 0:BackLight
-  DDRD = 0x00;  PORTD = 0b11111100; // 7/6:Spare3/4, 5:RENC2_PUSH, 4:RENC1_PUSH, 3:RENC2_B, 2:RENC2_A, 1:I2C_SDA, 0:I2C_SCL
+  DDRD = 0x00;  PORTD = 0b11111100; // 7:VIB, 6:LED BL, 5:RENC2_PUSH, 4:RENC1_PUSH, 3:RENC2_B, 2:RENC2_A, 1:I2C_SDA, 0:I2C_SCL
   DDRE = 0b00001010;  PORTE = 0b11110101; // 7:PPM_IN, 6: RENC1_B, 5:RENC1_A, 4:USB_DNEG, 3:BUZZER, 2:USB_DPOS, 1:TELEM_TX, 0:TELEM_RX
   DDRF = 0x00;  PORTF = 0x00; // 7-4:JTAG, 3:ADC_REF_1.2V input, 2-0:ADC_SPARE_2-0
   DDRG = 0b00010000;  PORTG = 0xff; // 7-6:N/A, 5:GearSW, 4: Sim_Ctrl[out], 3:IDL1_Sw, 2:TCut_Sw, 1:RF_Power[in], 0: RudDr_Sw
diff --git a/src/open9x.h b/src/open9x.h
index fcef30f43..d306b5ada 100644
--- a/src/open9x.h
+++ b/src/open9x.h
@@ -175,7 +175,7 @@ extern uint16_t DEBUG2;
 #  define INP_E_TELEM_RX    1
 #  define OUT_E_TELEM_TX    0
 
-#  define INP_D_SPARE3         7
+#  define INP_D_HAPTIC         7
 #  define INP_D_SPARE4         6
 #  define INP_D_ROT_ENC_2_PUSH 5
 #  define INP_D_ROT_ENC_1_PUSH 4
@@ -244,6 +244,7 @@ extern uint16_t DEBUG2;
 
 #define OUT_G_SIM_CTL  4 //1 : phone-jack=ppm_in
 #define INP_G_ID1      3
+#define INP_G_HAPTIC   2
 #define INP_G_RF_POW   1
 #define INP_G_RuddDR   0
 
@@ -492,6 +493,8 @@ extern uint8_t s_traceBuf[MAXTRACE];
 extern uint16_t s_traceWr;
 extern int8_t s_traceCnt;
 
+extern int8_t *s_trimPtr[NUM_STICKS];
+
 uint16_t getTmr16KHz();
 uint16_t stack_free();
 
@@ -522,11 +525,29 @@ extern uint8_t  s_eeDirtyMsk;
 #define STORE_GENERALVARS eeDirty(EE_GENERAL)
 
 #if defined (PCBV4)
-#define BACKLIGHT_ON    PORTC |=  (1<<OUT_C_LIGHT)
-#define BACKLIGHT_OFF   PORTC &= ~(1<<OUT_C_LIGHT)
+#define SPEAKER_ON   TCCR0A |=  (1 << COM0A0)
+#define SPEAKER_OFF  TCCR0A &= ~(1 << COM0A0)
+#define BACKLIGHT_ON  PORTC |=  (1 << OUT_C_LIGHT)
+#define BACKLIGHT_OFF PORTC &= ~(1 << OUT_C_LIGHT)
 #else
-#define BACKLIGHT_ON    PORTB |=  (1<<OUT_B_LIGHT)
-#define BACKLIGHT_OFF   PORTB &= ~(1<<OUT_B_LIGHT)
+#define BACKLIGHT_ON  PORTB |=  (1 << OUT_B_LIGHT)
+#define BACKLIGHT_OFF PORTB &= ~(1 << OUT_B_LIGHT)
+#endif
+
+#define BUZZER_ON     PORTE |=  (1 << OUT_E_BUZZER)
+#define BUZZER_OFF    PORTE &= ~(1 << OUT_E_BUZZER)
+
+#if defined(HAPTIC)
+#if defined(PCBV4)
+#define HAPTIC_ON     PORTD &= ~(1 << INP_D_HAPTIC)
+#define HAPTIC_OFF    PORTD |=  (1 << INP_D_HAPTIC)
+#else
+#define HAPTIC_ON     PORTG |=  (1 << INP_G_HAPTIC)
+#define HAPTIC_OFF    PORTG &= ~(1 << INP_G_HAPTIC)
+#endif
+#else
+#define HAPTIC_ON
+#define HAPTIC_OFF
 #endif
 
 #define BITMASK(bit) (1<<(bit))
@@ -560,7 +581,9 @@ void eeLoadModel(uint8_t id);
 int8_t eeFindEmptyModel(uint8_t id, bool down);
 
 ///number of real input channels (1-9) plus virtual input channels X1-X4
-#define NUM_XCHNRAW (NUM_STICKS+NUM_POTS+2/*MAX/FULL*/+3/*CYC1-CYC3*/+NUM_PPM+NUM_CHNOUT+MAX_TIMERS+NUM_TELEMETRY)
+#define NUM_XCHNRAW (NUM_STICKS+NUM_POTS+2/*MAX/FULL*/+3/*CYC1-CYC3*/+NUM_PPM+NUM_CHNOUT)
+#define NUM_XCHNCSW (NUM_XCHNRAW+MAX_TIMERS+NUM_TELEMETRY)
+
 ///number of real output channels (CH1-CH8) plus virtual output channels X1-X4
 #define NUM_XCHNOUT (NUM_CHNOUT) //(NUM_CHNOUT)//+NUM_VIRT)
 
diff --git a/src/translations/en.h b/src/translations/en.h
index 18c950ad2..705b380c4 100644
--- a/src/translations/en.h
+++ b/src/translations/en.h
@@ -53,16 +53,19 @@
 #define LEN_VMLTPX2      2
 #define TR_VMLTPX2       "+=""*="":="
 
+#define LEN_VMIXTRIMS    6
+#define TR_VMIXTRIMS     "ON    ""OFF   ""Offset"
+
 #define LEN_VCSWFUNC     7
 #define TR_VCSWFUNC      "----   ""v>ofs  ""v<ofs  ""|v|>ofs""|v|<ofs""AND    ""OR     ""XOR    ""v1==v2 ""v1!=v2 ""v1>v2  ""v1<v2  ""v1>=v2 ""v1<=v2 "
 
-#define LEN_VFSWFUNC     14
+#define LEN_VFSWFUNC     13
 #ifdef LOGS
-#define TR_SDCLOGGS      "SDCARD Logs   "
+#define TR_SDCLOGGS      "SDCARD Logs  "
 #else
 #define TR_SDCLOGGS
 #endif
-#define TR_VFSWFUNC      "----          ""Trainer       ""Trainer RUD   ""Trainer ELE   ""Trainer THR   ""Trainer AIL   ""Instant Trim  ""Trims2Offsets " TR_SDCLOGGS
+#define TR_VFSWFUNC      "----         ""Trainer      ""Trainer RUD  ""Trainer ELE  ""Trainer THR  ""Trainer AIL  ""Instant Trim ""Trims2Offsets""Play sound   " TR_SDCLOGGS
 
 #define LEN_VTELEMUNIT   1
 #define TR_VTELEMUNIT    "v""-"
@@ -274,3 +277,5 @@
 #define TR_MENUDEBUG     "DEBUG"
 #define TR_RXNUM         "RxNum"
 #define TR_SYNCMENU      "Sync [MENU]"
+#define TR_HAPTICSTRENGTH "Haptic Strength"
+#define TR_SPKRPITCH     "Speaker Pitch"
diff --git a/src/translations/fr.h b/src/translations/fr.h
index 59c85fd05..43e54e952 100644
--- a/src/translations/fr.h
+++ b/src/translations/fr.h
@@ -53,16 +53,19 @@
 #define LEN_VMLTPX2      2
 #define TR_VMLTPX2       "+=""*="":="
 
+#define LEN_VMIXTRIMS    6
+#define TR_VMIXTRIMS     "ON    ""OFF   ""Offset"
+
 #define LEN_VCSWFUNC     7
 #define TR_VCSWFUNC      "----   ""v>ofs  ""v<ofs  ""|v|>ofs""|v|<ofs""ET     ""OU     ""XOU    ""v1==v2 ""v1!=v2 ""v1>v2  ""v1<v2  ""v1>=v2 ""v1<=v2 "
 
-#define LEN_VFSWFUNC     14
+#define LEN_VFSWFUNC     13
 #ifdef LOGS
-#define TR_SDCLOGGS      "SDCARD Logs   "
+#define TR_SDCLOGGS      "SDCARD Logs  "
 #else
 #define TR_SDCLOGGS
 #endif
-#define TR_VFSWFUNC      "----          ""Ecolage       ""Ecolage DER   ""Ecolage PRF   ""Ecolage GAZ   ""Ecolage AIL   ""Instant Trim  ""Trims2Offsets " TR_SDCLOGGS
+#define TR_VFSWFUNC      "----         ""Ecolage      ""Ecolage DIR  ""Ecolage PRF  ""Ecolage GAZ  ""Ecolage AIL  ""Trim instant ""Trims2Offsets""Jouer son    " TR_SDCLOGGS
 
 #define LEN_VTELEMUNIT   1
 #define TR_VTELEMUNIT    "v""-"
@@ -274,3 +277,5 @@
 #define TR_MENUDEBUG     "DEBUG"
 #define TR_RXNUM         "RxNum"
 #define TR_SYNCMENU      "Sync [MENU]"
+#define TR_HAPTICSTRENGTH "Haptic Strength"
+#define TR_SPKRPITCH     "Speaker Pitch"