From d8a5ee4e1df2792caf23e154c73ae4bacfaa5259 Mon Sep 17 00:00:00 2001
From: th9xer <th9xer@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Tue, 14 Feb 2012 00:45:36 +0000
Subject: [PATCH] Fix haptic DDR error on V4 and fixing the SOMO14D code.

---
 src/open9x.cpp  | 6 +++++-
 src/somo14d.cpp | 7 +++++--
 2 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/open9x.cpp b/src/open9x.cpp
index 448174912..35e4a5575 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -2150,7 +2150,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:VIB, 6:LED BL, 5:RENC2_PUSH, 4:RENC1_PUSH, 3:RENC2_B, 2:RENC2_A, 1:I2C_SDA, 0:I2C_SCL
+  DDRD = 0b11000000;  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
@@ -2321,6 +2321,10 @@ int main(void)
   /***************************************************/
 #endif
 
+#if defined (PCBV4)
+OCR4A = 0x7d;
+TCCR4B = (1 << WGM42) | (3<<CS40); // CTC OCR1A, 16MHz / 64 (4us ticks)
+#endif
   startPulses();
 
   wdt_enable(WDTO_500MS);
diff --git a/src/somo14d.cpp b/src/somo14d.cpp
index 8fe776680..ad84f15c5 100644
--- a/src/somo14d.cpp
+++ b/src/somo14d.cpp
@@ -81,16 +81,18 @@ ISR(TIMER4_COMPA_vect) //Every 0.5ms
     if (i==0 && !busy) {
       if(somo14command) { // Allow a command to be send even if the SOMO is busy, i.e. STOP or PAUSE
         somo14_current=somo14command;
+		busy=1;
       }
       else if (!SOMOBUSY) {
         if (somo14RIdx == somo14WIdx) {
-          // TODO disable the interrupt (and re-enable when a new prompt is pushed)?
+          TIMSK4 &= ~(1<<OCIE4A);
           return BUSY;
         }
         somo14_current = somo14playlist[somo14RIdx];
         somo14RIdx = (somo14RIdx + 1) % QUEUE_LENGTH;
+		busy = 1;
       }
-      busy = 1;
+      
     }
 
     // Start and stop bits
@@ -151,4 +153,5 @@ void somoPushPrompt(uint16_t prompt)
 {
   somo14playlist[somo14WIdx] = prompt;
   somo14WIdx = (somo14WIdx + 1) % QUEUE_LENGTH;
+  TIMSK4 |= (1<<OCIE4A);
 }