NanoWii with hardware PPM sum capture

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
ChrisParish
Posts: 2
Joined: Tue Jun 17, 2014 9:46 pm

NanoWii with hardware PPM sum capture

Post by ChrisParish »

Hi Folks,

I have been working on a modification to the firmware to allow the use of the timer3 input capture unit to capture the PPM SUM signal on pin 13. The big advantage, and reason for doing this, is that it provides a jitter-free, high resolution input stream while reducing the load on the processor. It does this by using the timer3 counter feature to give an exact time between pulses. Every time the input capture pin detects a rising edge the value of the counter at the time is stored in the input capture register. At first glance this might seem identical to how things are done now, but the key difference is that all this happens at a hardware level without requireing the processor to do anything, thereby allowing it to get on with other jobs (specifically dealing with an existing interrupt). It is also worth noting that the resolution of the original software based PPM SUM capture is 4us, giving only 256 steps per channel, whereas the hardware capture based version gives the full 1us resolution (it actually has 0.5us resolution, but the MultiWii sofware only works with 1us) and therefor 1000 steps per channel for that extra locked-in feel.
There are dissadvantages to this system. The main dissadvantage is that you cannot use more than four motors without having to resort to software PWM generation.
However, for anyone wanting to fly a quad this is ideal. It has allowed me to connect my FrSky receiver to the NanoWii board via pin13 (normally used for motor6 on the NanoWii).

Here is a unified patch file of the changes I have made.

Code: Select all

--- config.h   Tue Jun 17 22:11:35 2014
+++ config.h   Fri Nov 08 01:00:00 2013
@@ -46,7 +46,7 @@
     //#define OCTOFLATP
     //#define OCTOFLATX
     //#define FLYING_WING
-    #define VTAIL4
+    //#define VTAIL4
     //#define AIRPLANE
     //#define SINGLECOPTER
     //#define DUALCOPTER
@@ -97,7 +97,6 @@
       //#define FREEIMUv04      // FreeIMU v0.4 with MPU6050, HMC5883L, MS561101BA                  <- confirmed by Alex
       //#define FREEIMUv043     // same as FREEIMUv04 with final MPU6050 (with the right ACC scale)
       //#define NANOWII         // the smallest multiwii FC based on MPU6050 + pro micro based proc <- confirmed by Alex
-      #define NANOWIIICR        // experimental version of NanoWii using Timer 3 ICR for PPM SUM capture. QUADS ONLY
       //#define PIPO            // 9DOF board from erazz
       //#define QUADRINO        // full FC board 9DOF+baro board from witespy  with BMP085 baro     <- confirmed by Alex
       //#define QUADRINO_ZOOM   // full FC board 9DOF+baro board from witespy  second edition
@@ -331,7 +330,7 @@
     /****************************    PPM Sum Reciver    ***********************************/
       /* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2
          Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */
-      #define SERIAL_SUM_PPM         PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For Graupner/Spektrum
+      //#define SERIAL_SUM_PPM         PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For Graupner/Spektrum
       //#define SERIAL_SUM_PPM         ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For Robe/Hitec/Futaba
       //#define SERIAL_SUM_PPM         ROLL,PITCH,YAW,THROTTLE,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For Multiplex
       //#define SERIAL_SUM_PPM         PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For some Hitec/Sanwa/Others
--- def.h   Tue Jun 17 22:07:33 2014
+++ def.h   Fri Nov 08 00:56:18 2013
@@ -954,28 +954,6 @@
   #endif
 #endif
 
-#if defined(NANOWIIICR)
-  #define MPU6050
-  #define ACC_ORIENTATION(X, Y, Z)  {imu.accADC[ROLL]  = -Y; imu.accADC[PITCH]  =  X; imu.accADC[YAW]  =  Z;}
-  #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = -X; imu.gyroADC[PITCH] = -Y; imu.gyroADC[YAW] = -Z;}
-  #undef INTERNAL_I2C_PULLUPS
-  // move motor 7 & 8 to pin 4 & A2
-  #define SOFT_PWM_3_PIN_HIGH        PORTD |= 1<<4;
-  #define SOFT_PWM_3_PIN_LOW         PORTD &= ~(1<<4);
-  #define SOFT_PWM_4_PIN_HIGH        PORTF |= 1<<5;
-  #define SOFT_PWM_4_PIN_LOW         PORTF &= ~(1<<5);
-  #define SW_PWM_P3                  4       
-  #define SW_PWM_P4                  A2
-  //#define HWPWM6
-  #define ICR_SUM_PPM
-  // move servo 3 & 4 to pin 13 & 11
-  // use pin 4 as status LED output as we have no octo
-  #define LEDPIN_PINMODE             DDRD |= (1<<4);            //D4 to output
-  #define LEDPIN_TOGGLE              PIND |= (1<<5)|(1<<4);     //switch LEDPIN state (Port D5) & pin D4
-  #define LEDPIN_OFF                 PORTD |= (1<<5); PORTD &= ~(1<<4);
-  #define LEDPIN_ON                  PORTD &= ~(1<<5); PORTD |= (1<<4); 
-#endif
-
 #if defined(PIPO)
   #define L3G4200D
   #define ADXL345
@@ -2117,11 +2095,5 @@
 #if defined(A32U4_4_HW_PWM_SERVOS) && !(defined(HELI_120_CCPM))
   #error "for your protection: A32U4_4_HW_PWM_SERVOS was not tested with your coptertype"
 #endif
-
-#if defined(HWPMW6) && defined (NANOWIIICR)
-  #error "HWPWM6 cannot be used with the NANO Wii in ICR mode. Switch to regular NanoWii"
-#endif
-
-
 
 #endif /* DEF_H_ */

--- RX.cpp   Tue Jun 17 21:55:47 2014
+++ RX.cpp   Fri Nov 08 00:56:18 2013
@@ -37,11 +37,7 @@
   static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS}; // if this slowes the PCINT readings we can switch to a define for each pcint bit
 #endif
 
-#if defined(ICR_SUM_PPM)
-  void rxInt(uint16_t now);
-#else
-  void rxInt(void);
-#endif
+void rxInt(void);
 
 /**************************************************************************************/
 /***************                   RX Pin Setup                    ********************/
@@ -73,7 +69,7 @@
     #endif
     
     /***************   atmega32u4's Specific RX Pin Setup   **********************/
-    #if defined(PROMICRO) && !defined(ICR_SUM_PPM)
+    #if defined(PROMICRO)
       //Trottle on pin 7
       DDRE &= ~(1 << 6); // pin 7 to input
       PORTE |= (1 << 6); // enable pullups
@@ -95,7 +91,7 @@
   /*************************   Special RX Setup   ********************************/
   #endif
   // Init PPM SUM RX
-  #if defined(SERIAL_SUM_PPM) && !defined(ICR_SUM_PPM)
+  #if defined(SERIAL_SUM_PPM)
     PPM_PIN_INTERRUPT;
   #endif
   // Init Sektrum Satellite RX
@@ -106,18 +102,6 @@
   #if defined(SBUS)
     SerialOpen(1,100000);
   #endif
-  // Init ICR PPM SUM for NanoWii
-  #if defined(SERIAL_SUM_PPM) && defined(ICR_SUM_PPM)
-    TCCR3A = 0; //compare registers off, WGM mode 0. Straight forward counter
-    TCCR3B = (1<<ICNC3 | 1<<ICES3 | 1<<CS31); //input capture edge select to rising, 1/8th clock select (2mhz)
-    TCCR3C = 0;  //all zeros
-    TIMSK3 = 1<<ICIE3; //enable interrupt capture

-    pinMode(13, INPUT);
-    digitalWrite(13, LOW);
-   
-  #endif

 }
 
 /**************************************************************************************/
@@ -232,7 +216,7 @@
   #endif
   
   /****************      atmega32u4's Throttle & Aux2 Pin      *******************/
-  #if defined(PROMICRO) && !defined(ICR_SUM_PPM)
+  #if defined(PROMICRO)
     // throttle
     ISR(INT6_vect){
       static uint16_t now,diff;
@@ -270,26 +254,17 @@
 /***************                PPM SUM RX Pin reading             ********************/
 /**************************************************************************************/
 // attachInterrupt fix for promicro
-#if defined(PROMICRO) && defined(SERIAL_SUM_PPM) && !defined(ICR_SUM_PPM)
+#if defined(PROMICRO) && defined(SERIAL_SUM_PPM)
   ISR(INT6_vect){rxInt();}
 #endif
 
-#if defined(PROMICRO) && defined(SERIAL_SUM_PPM) && defined(ICR_SUM_PPM)
-  ISR(TIMER3_CAPT_vect)
-  {
-     uint16_t cap = ICR3; //grab the value from the input capture register
-     TCNT3 = 0;           //reset the counter to 0
-     rxInt(cap);
-  }
-#endif
-
 // PPM_SUM at THROTTLE PIN on MEGA boards
 #if defined(PPM_ON_THROTTLE) && defined(MEGA) && defined(SERIAL_SUM_PPM)
   ISR(PCINT2_vect) { if(PINK & (1<<0)) rxInt(); }
 #endif
 
 // Read PPM SUM RX Data
-#if defined(SERIAL_SUM_PPM) && !defined(ICR_SUM_PPM)
+#if defined(SERIAL_SUM_PPM)
   void rxInt(void) {
     uint16_t now,diff;
     static uint16_t last = 0;
@@ -315,34 +290,8 @@
         #endif
       }
     chan++;
-    }
-  }
-#endif
-
-// Read PPM SUM RX Data via ICR
-#if defined(SERIAL_SUM_PPM) && defined(ICR_SUM_PPM)
-  void rxInt(uint16_t now) {
-    now = now/2;
-    static uint8_t chan = 0;
-  #if defined(FAILSAFE)
-    static uint8_t GoodPulses;
-  #endif

-    if(now>3000) chan = 0;
-    else {
-      if(900<now && now<2200 && chan<RC_CHANS ) {   //Only if the signal is between these values it is valid, otherwise the failsafe counter should move up
-        rcValue[chan] = now;
-        #if defined(FAILSAFE)
-          if(chan<4 && now>FAILSAFE_DETECT_TRESHOLD) GoodPulses |= (1<<chan); // if signal is valid - mark channel as OK
-          if(GoodPulses==0x0F) {                                               // If first four chanells have good pulses, clear FailSafe counter
-            GoodPulses = 0;
-            if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;
-          }
-        #endif
-      }
-    chan++;
-    }
   }
+}
 #endif
 
 /**************************************************************************************/


I appologise if I have made some mistakes in protocol for creating patch files: I have never done it before.

Comments and thoughts please.

Chris

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: NanoWii with hardware PPM sum capture

Post by timecop »

Other than your patch is "reverse" as in, it "removes" your feature instead of adding it, its OK.
You might want to re-run it by diff oldfile newfile > foo.patch.
right now, it was ran the opposite way.

ChrisParish
Posts: 2
Joined: Tue Jun 17, 2014 9:46 pm

Re: NanoWii with hardware PPM sum capture

Post by ChrisParish »

:oops: I feel foolish. ;)
Thanks for pointing out where I went wrong, timecop; lesson learned.

Here is the correct patch file. I have also taken the oppertunity to tidy it up a bit and add some basic error checking in the def.h file. Running it on a fresh download of MultiWii 2.3 will create an unconfigured file (the original patch I did, other than being the wrong way round, would have created the excact config I was using. This way you can set your own without having to undo mine first)

Code: Select all

--- config.h   Fri Nov 08 01:00:00 2013
+++ config.h   Wed Jun 18 07:49:42 2014
@@ -97,6 +97,7 @@
       //#define FREEIMUv04      // FreeIMU v0.4 with MPU6050, HMC5883L, MS561101BA                  <- confirmed by Alex
       //#define FREEIMUv043     // same as FREEIMUv04 with final MPU6050 (with the right ACC scale)
       //#define NANOWII         // the smallest multiwii FC based on MPU6050 + pro micro based proc <- confirmed by Alex
+      //#define NANOWIIICR      // experimental version of NanoWii using Timer 3 ICR for PPM SUM capture. QUADS ONLY
       //#define PIPO            // 9DOF board from erazz
       //#define QUADRINO        // full FC board 9DOF+baro board from witespy  with BMP085 baro     <- confirmed by Alex
       //#define QUADRINO_ZOOM   // full FC board 9DOF+baro board from witespy  second edition
--- def.h   Fri Nov 08 00:56:18 2013
+++ def.h   Wed Jun 18 07:50:37 2014
@@ -954,6 +954,19 @@
   #endif
 #endif
 
+#if defined(NANOWIIICR)
+  #define MPU6050
+  #define ACC_ORIENTATION(X, Y, Z)  {imu.accADC[ROLL]  = -Y; imu.accADC[PITCH]  =  X; imu.accADC[YAW]  =  Z;}
+  #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = -X; imu.gyroADC[PITCH] = -Y; imu.gyroADC[YAW] = -Z;}
+  #undef INTERNAL_I2C_PULLUPS
+  #define ICR_SUM_PPM
+  // use pin 4 as status LED output as we have no octo
+  #define LEDPIN_PINMODE             DDRD |= (1<<4);            //D4 to output
+  #define LEDPIN_TOGGLE              PIND |= (1<<5)|(1<<4);     //switch LEDPIN state (Port D5) & pin D4
+  #define LEDPIN_OFF                 PORTD |= (1<<5); PORTD &= ~(1<<4);
+  #define LEDPIN_ON                  PORTD &= ~(1<<5); PORTD |= (1<<4); 
+#endif
+
 #if defined(PIPO)
   #define L3G4200D
   #define ADXL345
@@ -2095,5 +2108,18 @@
 #if defined(A32U4_4_HW_PWM_SERVOS) && !(defined(HELI_120_CCPM))
   #error "for your protection: A32U4_4_HW_PWM_SERVOS was not tested with your coptertype"
 #endif
+
+#if defined(HWPMW6) && defined (NANOWIIICR)
+  #error "HWPWM6 cannot be used with the NANO Wii in ICR mode. Switch to regular NanoWii"
+#endif
+
+#if defined(NANOWIIICR) && !defined(SERIAL_SUM_PPM)
+  #error "SERIAL_SUM_PPM must be used with NANOWIIICR. Please uncomment the appropriate SERIAL_SUM_PPM line or change to standard NanoWii"
+#endif
+
+#if defined(NANOWIIICR) && !(defined(QUADP) || defined(QUADX) || defined(Y4) || defined(VTAIL4))
+  #error "NANOWIIICR can only be used with the following four motor configurations: QUADP, QUADX, Y4 and VTAIL4"
+ #endif
+
 
 #endif /* DEF_H_ */
--- Output.cpp   Mon Sep 09 23:41:40 2013
+++ Output.cpp   Tue Jun 17 22:38:31 2014
@@ -1474,4 +1474,4 @@
     lastRead = currentTime;
   }
   #endif
-}
+}
--- RX.cpp   Fri Nov 08 00:56:18 2013
+++ RX.cpp   Tue Jun 17 21:55:47 2014
@@ -37,7 +37,11 @@
   static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS}; // if this slowes the PCINT readings we can switch to a define for each pcint bit
 #endif
 
-void rxInt(void);
+#if defined(ICR_SUM_PPM)
+  void rxInt(uint16_t now);
+#else
+  void rxInt(void);
+#endif
 
 /**************************************************************************************/
 /***************                   RX Pin Setup                    ********************/
@@ -69,7 +73,7 @@
     #endif
     
     /***************   atmega32u4's Specific RX Pin Setup   **********************/
-    #if defined(PROMICRO)
+    #if defined(PROMICRO) && !defined(ICR_SUM_PPM)
       //Trottle on pin 7
       DDRE &= ~(1 << 6); // pin 7 to input
       PORTE |= (1 << 6); // enable pullups
@@ -91,7 +95,7 @@
   /*************************   Special RX Setup   ********************************/
   #endif
   // Init PPM SUM RX
-  #if defined(SERIAL_SUM_PPM)
+  #if defined(SERIAL_SUM_PPM) && !defined(ICR_SUM_PPM)
     PPM_PIN_INTERRUPT;
   #endif
   // Init Sektrum Satellite RX
@@ -102,6 +106,18 @@
   #if defined(SBUS)
     SerialOpen(1,100000);
   #endif
+  // Init ICR PPM SUM for NanoWii
+  #if defined(SERIAL_SUM_PPM) && defined(ICR_SUM_PPM)
+    TCCR3A = 0; //compare registers off, WGM mode 0. Straight forward counter
+    TCCR3B = (1<<ICNC3 | 1<<ICES3 | 1<<CS31); //input capture edge select to rising, 1/8th clock select (2mhz)
+    TCCR3C = 0;  //all zeros
+    TIMSK3 = 1<<ICIE3; //enable interrupt capture

+    pinMode(13, INPUT);
+    digitalWrite(13, LOW);
+   
+  #endif

 }
 
 /**************************************************************************************/
@@ -216,7 +232,7 @@
   #endif
   
   /****************      atmega32u4's Throttle & Aux2 Pin      *******************/
-  #if defined(PROMICRO)
+  #if defined(PROMICRO) && !defined(ICR_SUM_PPM)
     // throttle
     ISR(INT6_vect){
       static uint16_t now,diff;
@@ -254,17 +270,26 @@
 /***************                PPM SUM RX Pin reading             ********************/
 /**************************************************************************************/
 // attachInterrupt fix for promicro
-#if defined(PROMICRO) && defined(SERIAL_SUM_PPM)
+#if defined(PROMICRO) && defined(SERIAL_SUM_PPM) && !defined(ICR_SUM_PPM)
   ISR(INT6_vect){rxInt();}
 #endif
 
+#if defined(PROMICRO) && defined(SERIAL_SUM_PPM) && defined(ICR_SUM_PPM)
+  ISR(TIMER3_CAPT_vect)
+  {
+     uint16_t cap = ICR3; //grab the value from the input capture register
+     TCNT3 = 0;           //reset the counter to 0
+     rxInt(cap);
+  }
+#endif
+
 // PPM_SUM at THROTTLE PIN on MEGA boards
 #if defined(PPM_ON_THROTTLE) && defined(MEGA) && defined(SERIAL_SUM_PPM)
   ISR(PCINT2_vect) { if(PINK & (1<<0)) rxInt(); }
 #endif
 
 // Read PPM SUM RX Data
-#if defined(SERIAL_SUM_PPM)
+#if defined(SERIAL_SUM_PPM) && !defined(ICR_SUM_PPM)
   void rxInt(void) {
     uint16_t now,diff;
     static uint16_t last = 0;
@@ -290,8 +315,34 @@
         #endif
       }
     chan++;
+    }
+  }
+#endif
+
+// Read PPM SUM RX Data via ICR
+#if defined(SERIAL_SUM_PPM) && defined(ICR_SUM_PPM)
+  void rxInt(uint16_t now) {
+    now = now/2;
+    static uint8_t chan = 0;
+  #if defined(FAILSAFE)
+    static uint8_t GoodPulses;
+  #endif

+    if(now>3000) chan = 0;
+    else {
+      if(900<now && now<2200 && chan<RC_CHANS ) {   //Only if the signal is between these values it is valid, otherwise the failsafe counter should move up
+        rcValue[chan] = now;
+        #if defined(FAILSAFE)
+          if(chan<4 && now>FAILSAFE_DETECT_TRESHOLD) GoodPulses |= (1<<chan); // if signal is valid - mark channel as OK
+          if(GoodPulses==0x0F) {                                               // If first four chanells have good pulses, clear FailSafe counter
+            GoodPulses = 0;
+            if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;
+          }
+        #endif
+      }
+    chan++;
+    }
   }
-}
 #endif
 
 /**************************************************************************************/


The file is also attached if anyone wants to give it a try.
Attachments
nanowii-icr.zip
(2.61 KiB) Downloaded 149 times

Von B
Posts: 2
Joined: Tue Nov 20, 2012 7:58 pm

Re: NanoWii with hardware PPM sum capture

Post by Von B »

Just what I need. Is there a patched 2.3 (complete) zip available for download perhaps. When applying the patch I get some rejects on a clean 2.3.

Regards,
Von B

o_lampe
Posts: 117
Joined: Sat Nov 02, 2013 5:09 pm

Re: NanoWii with hardware PPM sum capture

Post by o_lampe »

Does it work on all nanoWii versions? 328p and 32U4 processors?

zidlov
Posts: 16
Joined: Mon Jan 07, 2013 10:08 am

Re: NanoWii with hardware PPM sum capture

Post by zidlov »

Really interested in that as I'm just re-building a quad with nanowii. Anyone else have tried it yet?
+1-ing the request for a patched complete 2.3 as I'll have to read into patching it myself otherwise....*sigh* I know, I'm just lazy, but the day that it will take me wrapping my head around this could just be spent with pid-tuning instead. :)

Post Reply