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