MultiWii

All things related to MultiWii
http://www.multiwii.com/forum/

ArduIMU V2 (flat) and Multiwii 6DOF

http://www.multiwii.com/forum/viewtopic.php?f=6&t=909

Page 1 of 1

ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Sat Nov 19, 2011 11:00 am
by flyboy_____
Does anyone knows how to connect this IMU to MultiWii?

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Wed Nov 23, 2011 11:01 am
by LenzGr
I have the same board - I'd like to learn if anyone else has already looked into this. At least accessing the analog accelerometers should be easily doable without adding a lot of new code. There currently does not seem to be any support for analog gyroscopes, though.

I currently fly a quadrocopter with the ArduIMU+ V2 (flat) using my own code, which is a conglomerate of code from several projects (Jose Julio's JJ ArduIMU Quadcopter, Paul René Jørgensen's Quaduino as well as Loris Rion's EasyIMUPilot). You can get the Bazaar source tree from here: https://code.launchpad.net/~lenzgr/+junk/QuaduIMU

If I get around to it, I'll look into how to add full support for the ArduIMU to MultiWii - it would be nice to have a GUI for configuration and tuning :)

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Sun Nov 27, 2011 2:25 pm
by LenzGr
OK, I've spent some time looking at the code base and if it's feasible/possible to port the MultiWii code to the ArduIMU board. I was partially successful - I managed to read out the analog accelerometers and gyroscopes as well as the HMC5843 magnetometer attached to the I2C bus without much additional coding. However, the pin assignments is very much different (and impossible to change) on the ArduIMU when compared to the MultiWii default configuration. You need to use a PPM receiver, but you can't connect it to the default pins that the MultiWii code base expects. The choice of available pins also determines the use of interrupts and timers, which would require a heavy rewrite of the current defaults. I'm using the ServoTimer2 and ServoDecode libraries in my code in order to decode the PPM signal and drive the ESCs - I doubt this is a dependency that the MultiWii developers are keen on dragging in...

Anyway, my patches are available from this Bazaar branch on Launchpad - maybe they are useful for anyone.

https://code.launchpad.net/~lenzgr/+jun ... WiiArduIMU

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Mon Nov 28, 2011 4:30 pm
by flyboy_____
This team used the ArduIMU to control a quad an they have the code in bottom of the page.
http://www.diydrones.com/profiles/blogs ... r-part-iii

I believe they do not use Multiwii, but could be interesting to look because they have Sonar and IR and its code, that soon will be developed for multiwii (I hope).

ArduIMU V3 is starting to sell soon.

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Tue Nov 29, 2011 4:02 pm
by LenzGr
Yes, that's the code from which my firmware is partially derived from. It's not based on MultiWii, it's an evolution of the original ArduIMU software. Parts of this have evolved into the ArduCopter project by now. I ripped out the Sonar and IR code from my version, though - I wanted to keep it simple first.

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Sun May 27, 2012 2:07 am
by LenzGr
FYI, I've now updated the code base at https://code.launchpad.net/~lenzgr/+jun ... WiiArduIMU to be in sync with the current 2.0-dev version in the SVN trunk. I can now read out all sensors (incl. the I2C magnetometer), next is connecting a PPM receiver and testing if it can read any radio input. I also need to check if my changes to use pins 13,10,11 and 12 for PWM output actually works.

This code should also be useful for sensor boards that use both analog accelerometers and gyros like the 6DOF Razor board from SparkFun: viewtopic.php?f=6&t=1618

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Tue Jun 05, 2012 3:34 pm
by LenzGr
A quick update: reading the PPM signal now works, too. The left and right motors are also working correctly already. However, the fixed pin layout gives me a hard time, as it requires tweaks to the timers, which is somewhat beyond my skill level. A big thank-you goes out to ronco, who provided great assistance so far in this thread.

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Fri Jun 08, 2012 12:08 am
by LenzGr
(moving the discussion over from this thread)

Hi Felix,

ronco wrote:it isnt that simple .. in Multiwii we use the hardware PWM capabilities of the MCUs as good as possible .. so that differs from servo timer2 lib and other software PWM methods becaus it works only on some specific pins (on arduinos thay are marked with a "~" or a white ring arround it).. the benefits of this is that the cause no interrupt jitter and also work without any jitter in the resulting signals (http://code.google.com/p/multiwii/wiki/PWM_Generation :D )

so to use pin 12 & 13 you will need to do it with software PWM .. i have less time atm -> here is just a fast overview how to do it:

disable the HW PWM init in init output for pin 11 & 3

Code: Select all

/********  Specific PWM Timers & Registers for the atmega328P (Promini)   ************/
  #if defined(PROMINI)
    #if (NUMBER_MOTOR > 0)
      TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A
    #endif
    #if (NUMBER_MOTOR > 1)
      TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B
    #endif
    #if (NUMBER_MOTOR > 2)
     // TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A <-- disabled
    #endif
    #if (NUMBER_MOTOR > 3)
      //TCCR2A |= _BV(COM2B1); // connect pin 3 to timer 2 channel B <-- disabled
    #endif



init software PWM for timer 2 comperator A & B

Code: Select all

// to 8bit variables to precalculate the dutytimes (ist better then doing it inside of a ISR)
volatile uint8_t pin12_high;
volatile uint8_t pin12_low;

volatile uint8_t pin13_high;
volatile uint8_t pin13_low;

// pin 12 & 13 must be set as output but thay are if you changed the Motors order

// set timer 2 to normal counting mode
TCCR2A = 0; // normal counting mode

// note because it is now in normal mode (that is more like fast PWM mode you may need rise the prescaler of timer 2 -> http://arduino.cc/playground/Code/PwmFrequency)

//enable the two interrupts for timer2's comperators a & B
TIMSK2 |= (1<<OCIE2A); // enable comperatorA'a ISR
TIMSK2 |= (1<<OCIE2B); // enable comperatorA'a ISR


The ISR's that will do the PWM to the pins

Code: Select all

// for comperator A // pin 12
ISR(TIMER2_COMPA_vect){
     static uint8_t state = 0;
     if(state==0){
           PORTB |= (1<<4); // pin 12 high
           OCR2A += pin12_high; // wait for the dutytime
           state = 1;
     }else{
           PORTB &= ~(1<<4); // pin 12 low
           OCR2A += pin12_low; // wait for the rest
           state = 0;
     }
}
// for comperator B // pin 13
ISR(TIMER2_COMPB_vect){
     static uint8_t state = 0;
     if(state==0){
           PORTB |= (1<<5); // pin 13 high
           OCR2B += pin13_high; // wait for the dutytime
           state = 1;
     }else{
           PORTB &= ~(1<<5); // pin 13 low
           OCR2B += pin13_low; // wait for the rest
           state = 0;
     }
}


to write the motors values to it: (to put in write motors .. instead of OCR2A = motor[2]>>3; and OCR2B = motor[3]>>3; )

Code: Select all

          pin12_high = motor[2]>>3;
          pin12_low = 255-pin12_high;

          pin13_high = motor[3]>>3;
          pin12_low = 255-pin13_high;     


ok that was not so fast ;) .. but it is untested ..

regrads felix


Thanks a lot for your suggestions. I tried to incorporate your proposed changes, but the outcome was that now only the right motor attached to pin 10 still turns correctly, while the left motor connected to pin 11 is silent and the front motor attached to pin 13 just twitches/shakes. Probably because we disabled PWM on motor 11 above?. I'm restricted to only use pins 13,10,11,12 (rear,right,left,front) for PWM output. Can they all be driven with timer2? I assume not, since there are only two comparators available? The description of the various PWM modes on the MultiWii wiki on google code was useful, but I admit I still have not fully figured out how we're actually doing it on the ProMini. Could you please take another look if I applied your suggested modification correctly?

I've pushed the changes into my bzr branch (I am tracking the SVN trunk and apply my changes on top of this), the diff is here.

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Mon Jun 11, 2012 12:42 pm
by ronco
Hi Lenz,

sorry for the late replay :(

i chacked it again and there was some mistakes ;)

here is a full working output :D

Code: Select all

/**************************************************************************************/
/***************                  Motor Pin order                  ********************/
/**************************************************************************************/
// since we are uing the PWM generation in a direct way, the pin order is just to inizialie the right pins 
// its not possible to change a PWM output pin just by changing the order
#if defined(PROMINI)
  ////////////////////////////////////////////////////////////////////////////////////////////////////
  uint8_t PWM_PIN[8] = {9,10,12,13,6,5,A2,12};   //for a quad+: rear,right,left,front
  ////////////////////////////////////////////////////////////////////////////////////////////////////
#endif
#if defined(PROMICRO)
  #if !defined(HWPWM6)
    #if !defined(TEENSY20)
      uint8_t PWM_PIN[8] = {9,10,5,6,4,A2,SW_PWM_P3,SW_PWM_P4};   //for a quad+: rear,right,left,front
    #else
      uint8_t PWM_PIN[8] = {14,15,9,12,22,18,16,17};   //for a quad+: rear,right,left,front
    #endif
  #else
    #if !defined(TEENSY20)
      uint8_t PWM_PIN[8] = {9,10,5,6,11,13,SW_PWM_P3,SW_PWM_P4};   //for a quad+: rear,right,left,front
    #else
      uint8_t PWM_PIN[8] = {14,15,9,12,4,10,16,17};   //for a quad+: rear,right,left,front
    #endif
  #endif
#endif
#if defined(MEGA)
  uint8_t PWM_PIN[8] = {3,5,6,2,7,8,9,10};      //for a quad+: rear,right,left,front   //+ for y6: 7:under right  8:under left
#endif

/**************************************************************************************/
/***************         Software PWM & Servo variables            ********************/
/**************************************************************************************/
#if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6))
  #if defined(SERVO)
    #if defined(AIRPLANE)|| defined(HELICOPTER)
      // To prevent motor to start at reset. atomicServo[7]=5 or 249 if reversed servo
      volatile uint8_t atomicServo[8] = {125,125,125,125,125,125,125,5}; 
    #else
      volatile uint8_t atomicServo[8] = {125,125,125,125,125,125,125,125};
    #endif
  #endif
  #if (NUMBER_MOTOR > 4)
    //for HEX Y6 and HEX6/HEX6X flat for promini
    volatile uint8_t atomicPWM_PIN5_lowState;
    volatile uint8_t atomicPWM_PIN5_highState;
    volatile uint8_t atomicPWM_PIN6_lowState;
    volatile uint8_t atomicPWM_PIN6_highState;
  #endif
  #if (NUMBER_MOTOR > 6)
    //for OCTO on promini
    volatile uint8_t atomicPWM_PINA2_lowState;
    volatile uint8_t atomicPWM_PINA2_highState;
    volatile uint8_t atomicPWM_PIN12_lowState;
    volatile uint8_t atomicPWM_PIN12_highState;
  #endif
#else
  #if defined(SERVO)
    #if defined(AIRPLANE)|| defined(HELICOPTER)
      // To prevent motor to start at reset. atomicServo[7]=5 or 249 if reversed servo
      volatile uint8_t atomicServo[8] = {8000,8000,8000,8000,8000,8000,8000,8000}; 
    #else
      volatile uint16_t atomicServo[8] = {8000,8000,8000,8000,8000,8000,8000,320};
    #endif
  #endif
  #if (NUMBER_MOTOR > 4)
    //for HEX Y6 and HEX6/HEX6X and for Promicro
    volatile uint16_t atomicPWM_PIN5_lowState;
    volatile uint16_t atomicPWM_PIN5_highState;
    volatile uint16_t atomicPWM_PIN6_lowState;
    volatile uint16_t atomicPWM_PIN6_highState;
  #endif
  #if (NUMBER_MOTOR > 6)
    //for OCTO on Promicro
    volatile uint16_t atomicPWM_PINA2_lowState;
    volatile uint16_t atomicPWM_PINA2_highState;
    volatile uint16_t atomicPWM_PIN12_lowState;
    volatile uint16_t atomicPWM_PIN12_highState;
  #endif
#endif

/**************************************************************************************/
/***************   Writes the Servos values to the needed format   ********************/
/**************************************************************************************/
void writeServos() {
  #if defined(SERVO)
    #if defined(PRI_SERVO_FROM)    // write primary servos
      for(uint8_t i = (PRI_SERVO_FROM-1); i < PRI_SERVO_TO; i++){
        #if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6))
          atomicServo[i] = (servo[i]-1000)>>2;
        #else
          atomicServo[i] = (servo[i]-1000)<<4;
        #endif
      }
    #endif
    #if defined(SEC_SERVO_FROM)   // write secundary servos
      #if defined(SERVO_TILT) && defined(MMSERVOGIMBAL)
        // Moving Average Servo Gimbal by Magnetron1
        static int16_t mediaMobileServoGimbalADC[3][MMSERVOGIMBALVECTORLENGHT];
        static int32_t mediaMobileServoGimbalADCSum[3];
        static uint8_t mediaMobileServoGimbalIDX;
        uint8_t axis;

        mediaMobileServoGimbalIDX = ++mediaMobileServoGimbalIDX % MMSERVOGIMBALVECTORLENGHT;
        for (axis=(SEC_SERVO_FROM-1); axis < SEC_SERVO_TO; axis++) {
          mediaMobileServoGimbalADCSum[axis] -= mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX];
          mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX] = servo[axis];
          mediaMobileServoGimbalADCSum[axis] += mediaMobileServoGimbalADC[axis][mediaMobileServoGimbalIDX];
          #if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6))
            atomicServo[axis] = (mediaMobileServoGimbalADCSum[axis] / MMSERVOGIMBALVECTORLENGHT - 1000)>>2;
          #else
            atomicServo[axis] = (mediaMobileServoGimbalADCSum[axis] / MMSERVOGIMBALVECTORLENGHT - 1000)<<4;
          #endif
        }
      #else
        for(uint8_t i = (SEC_SERVO_FROM-1); i < SEC_SERVO_TO; i++){
          #if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6))
            atomicServo[i] = (servo[i]-1000)>>2;
          #else
            atomicServo[i] = (servo[i]-1000)<<4;
          #endif
        }
      #endif
    #endif
  #endif
}



 ////////////////////////////////////////////////////////////////////////////////////////////////////
// to 8bit variables to precalculate the dutytimes (ist better then doing it inside of a ISR)
volatile uint8_t pin12_high;
volatile uint8_t pin12_low;

volatile uint8_t pin13_high;
volatile uint8_t pin13_low;
 ////////////////////////////////////////////////////////////////////////////////////////////////////






/**************************************************************************************/
/************  Writes the Motors values to the PWM compare register  ******************/
/**************************************************************************************/
void writeMotors() { // [1000;2000] => [125;250]

/****************  Specific PWM Timers & Registers for the MEGA's   *******************/
  #if defined(MEGA)// [1000:2000] => [8000:16000] for timer 3 & 4 for mega
    #if (NUMBER_MOTOR > 0) 
      #ifndef EXT_MOTOR_RANGE 
        OCR3C = motor[0]<<3; //  pin 3
      #else
        OCR3C = ((motor[0]<<4) - 16000) + 128;
      #endif
    #endif
    #if (NUMBER_MOTOR > 1)
      #ifndef EXT_MOTOR_RANGE 
        OCR3A = motor[1]<<3; //  pin 5
      #else
        OCR3A = ((motor[1]<<4) - 16000) + 128;
      #endif
    #endif
    #if (NUMBER_MOTOR > 2)
      #ifndef EXT_MOTOR_RANGE 
        OCR4A = motor[2]<<3; //  pin 6
      #else
        OCR4A = ((motor[2]<<4) - 16000) + 128;
      #endif
    #endif
    #if (NUMBER_MOTOR > 3)
      #ifndef EXT_MOTOR_RANGE 
        OCR3B = motor[3]<<3; //  pin 2
      #else
        OCR3B = ((motor[3]<<4) - 16000) + 128;
      #endif
    #endif
    #if (NUMBER_MOTOR > 4)
      #ifndef EXT_MOTOR_RANGE 
        OCR4B = motor[4]<<3; //  pin 7
        OCR4C = motor[5]<<3; //  pin 8
      #else
        OCR4B = ((motor[4]<<4) - 16000) + 128;
        OCR4C = ((motor[5]<<4) - 16000) + 128;
      #endif
    #endif
    #if (NUMBER_MOTOR > 6)
      #ifndef EXT_MOTOR_RANGE 
        OCR2B = motor[6]>>3; //  pin 9
        OCR2A = motor[7]>>3; //  pin 10
      #else
        OCR2B = ((motor[6]>>2) - 250) + 2);
        OCR2A = ((motor[7]>>2) - 250) + 2);
      #endif
    #endif
  #endif
  
/******** Specific PWM Timers & Registers for the atmega32u4 (Promicro)   ************/
  #if defined(PROMICRO)
    #if (NUMBER_MOTOR > 0) // Timer 1 A & B [1000:2000] => [8000:16000]
      OCR1A = motor[0]<<3; //  pin 9
    #endif
    #if (NUMBER_MOTOR > 1)
      OCR1B = motor[1]<<3; //  pin 10
    #endif
    #if (NUMBER_MOTOR > 2) // Timer 4 A & D [1000:2000] => [1000:2000]
      #if !defined(HWPWM6)
        // to write values > 255 to timer 4 A/B we need to split the bytes
        TC4H = (2047-motor[2])>>8; OCR4A = ((2047-motor[2])&0xFF); //  pin 5
      #else
        OCR3A = motor[2]<<3; //  pin 5
      #endif
    #endif
    #if (NUMBER_MOTOR > 3)
      TC4H = motor[3]>>8; OCR4D = (motor[3]&0xFF); //  pin 6
    #endif    
    #if (NUMBER_MOTOR > 4)
      #if !defined(HWPWM6)
        #if (NUMBER_MOTOR == 6) && !defined(SERVO)
          atomicPWM_PIN5_highState = motor[4]<<3;
          atomicPWM_PIN5_lowState = 16383-atomicPWM_PIN5_highState;
          atomicPWM_PIN6_highState = motor[5]<<3;
          atomicPWM_PIN6_lowState = 16383-atomicPWM_PIN6_highState;      
        #else
          atomicPWM_PIN5_highState = ((motor[4]-1000)<<4)+320;
          atomicPWM_PIN5_lowState = 15743-atomicPWM_PIN5_highState;
          atomicPWM_PIN6_highState = ((motor[5]-1000)<<4)+320;
          atomicPWM_PIN6_lowState = 15743-atomicPWM_PIN6_highState;        
        #endif
      #else
        OCR1C = motor[4]<<3; //  pin 11
        TC4H = motor[5]>>8; OCR4A = (motor[5]&0xFF); //  pin 13    
      #endif
    #endif
    #if (NUMBER_MOTOR > 6)
      #if !defined(HWPWM6)
        atomicPWM_PINA2_highState = ((motor[6]-1000)<<4)+320;
        atomicPWM_PINA2_lowState = 15743-atomicPWM_PINA2_highState;
        atomicPWM_PIN12_highState = ((motor[7]-1000)<<4)+320;
        atomicPWM_PIN12_lowState = 15743-atomicPWM_PIN12_highState;
      #else
        atomicPWM_PINA2_highState = ((motor[6]-1000)>>2)+5;
        atomicPWM_PINA2_lowState = 245-atomicPWM_PINA2_highState;
        atomicPWM_PIN12_highState = ((motor[7]-1000)>>2)+5;
        atomicPWM_PIN12_lowState = 245-atomicPWM_PIN12_highState;     
      #endif
    #endif
  #endif
  
/********  Specific PWM Timers & Registers for the atmega328P (Promini)   ************/
  #if defined(PROMINI)
    #if (NUMBER_MOTOR > 0)
      #ifndef EXT_MOTOR_RANGE 
        OCR1A = motor[0]>>3; //  pin 9
      #else
        OCR1A = ((motor[0]>>2) - 250) + 2;
      #endif
    #endif
    #if (NUMBER_MOTOR > 1)
      #ifndef EXT_MOTOR_RANGE 
        OCR1B = motor[1]>>3; //  pin 10
      #else
        OCR1B = ((motor[1]>>2) - 250) + 2;
      #endif
    #endif
     ////////////////////////////////////////////////////////////////////////////////////////////////////
    #if (NUMBER_MOTOR > 2)
      /*
      #ifndef EXT_MOTOR_RANGE
        OCR2A = motor[2]>>3; //  pin 11
      #else
        OCR2A = ((motor[2]>>2) - 250) + 2;
      #endif
      */
      pin12_high = motor[2]>>3;
      pin12_low = 255-pin12_high;
    #endif
    #if (NUMBER_MOTOR > 3)
     /*
      #ifndef EXT_MOTOR_RANGE
        OCR2B = motor[3]>>3; //  pin 3
      #else
        OCR2B = ((motor[3]>>2) - 250) + 2;
      #endif
      */
      pin13_high = motor[3]>>3;
      pin13_low = 255-pin13_high;   
      
    #endif
     ////////////////////////////////////////////////////////////////////////////////////////////////////
    #if (NUMBER_MOTOR > 4)
      #if (NUMBER_MOTOR == 6) && !defined(SERVO)
        #ifndef EXT_MOTOR_RANGE 
          atomicPWM_PIN6_highState = motor[4]>>3;
          atomicPWM_PIN5_highState = motor[5]>>3;
        #else
          atomicPWM_PIN6_highState = ((motor[4]>>2) - 250) + 2;
          atomicPWM_PIN5_highState = ((motor[5]>>2) - 250) + 2;       
        #endif
        atomicPWM_PIN6_lowState  = 255-atomicPWM_PIN6_highState;
        atomicPWM_PIN5_lowState  = 255-atomicPWM_PIN5_highState; 
      #else //note: EXT_MOTOR_RANGE not possible here
        atomicPWM_PIN6_highState = ((motor[4]-1000)>>2)+5;
        atomicPWM_PIN6_lowState  = 245-atomicPWM_PIN6_highState;
        atomicPWM_PIN5_highState = ((motor[5]-1000)>>2)+5;
        atomicPWM_PIN5_lowState  = 245-atomicPWM_PIN5_highState;
      #endif
    #endif
    #if (NUMBER_MOTOR > 6) //note: EXT_MOTOR_RANGE not possible here
      atomicPWM_PINA2_highState = ((motor[6]-1000)>>2)+5;
      atomicPWM_PINA2_lowState  = 245-atomicPWM_PINA2_highState;
      atomicPWM_PIN12_highState = ((motor[7]-1000)>>2)+5;
      atomicPWM_PIN12_lowState  = 245-atomicPWM_PIN12_highState;
    #endif
  #endif
}

/**************************************************************************************/
/************          Writes the mincommand to all Motors           ******************/
/**************************************************************************************/
void writeAllMotors(int16_t mc) {   // Sends commands to all motors
  for (uint8_t i =0;i<NUMBER_MOTOR;i++)
    motor[i]=mc;
  writeMotors();
}

/**************************************************************************************/
/************        Initialize the PWM Timers and Registers         ******************/
/**************************************************************************************/
void initOutput() {
  
/****************            mark all PWM pins as Output             ******************/
  for(uint8_t i=0;i<NUMBER_MOTOR;i++)
    pinMode(PWM_PIN[i],OUTPUT);
    
/****************  Specific PWM Timers & Registers for the MEGA's    ******************/
  #if defined(MEGA)
    #if (NUMBER_MOTOR > 0)
      // init 16bit timer 3
      TCCR3A |= (1<<WGM31); // phase correct mode
      TCCR3A &= ~(1<<WGM30);
      TCCR3B |= (1<<WGM33);
      TCCR3B &= ~(1<<CS31); // no prescaler
      ICR3   |= 0x3FFF; // TOP to 16383;      
      
      TCCR3A |= _BV(COM3C1); // connect pin 3 to timer 3 channel C
    #endif
    #if (NUMBER_MOTOR > 1)
      TCCR3A |= _BV(COM3A1); // connect pin 5 to timer 3 channel A
    #endif
    #if (NUMBER_MOTOR > 2)
      // init 16bit timer 4
      TCCR4A |= (1<<WGM41); // phase correct mode
      TCCR4A &= ~(1<<WGM40);
      TCCR4B |= (1<<WGM43);
      TCCR4B &= ~(1<<CS41); // no prescaler
      ICR4   |= 0x3FFF; // TOP to 16383;    
      
      TCCR4A |= _BV(COM4A1); // connect pin 6 to timer 4 channel A
    #endif
    #if (NUMBER_MOTOR > 3)
      TCCR3A |= _BV(COM3B1); // connect pin 2 to timer 3 channel B
    #endif
    #if (NUMBER_MOTOR > 4)
      TCCR4A |= _BV(COM4B1); // connect pin 7 to timer 4 channel B
      TCCR4A |= _BV(COM4C1); // connect pin 8 to timer 4 channel C
    #endif
    #if (NUMBER_MOTOR > 6)
      // timer 2 is a 8bit timer so we cant change its range 
      TCCR2A |= _BV(COM2B1); // connect pin 9 to timer 2 channel B
      TCCR2A |= _BV(COM2A1); // connect pin 10 to timer 2 channel A
    #endif
  #endif
  
/******** Specific PWM Timers & Registers for the atmega32u4 (Promicro)   ************/
  #if defined(PROMICRO)
    #if (NUMBER_MOTOR > 0)
      TCCR1A |= (1<<WGM11); // phase correct mode & no prescaler
      TCCR1A &= ~(1<<WGM10);
      TCCR1B &= ~(1<<WGM12) &  ~(1<<CS11) & ~(1<<CS12);
      TCCR1B |= (1<<WGM13) | (1<<CS10); 
      ICR1   |= 0x3FFF; // TOP to 16383;     
      TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A
    #endif
    #if (NUMBER_MOTOR > 1)
      TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B
    #endif
    #if (NUMBER_MOTOR > 2)
      #if !defined(HWPWM6) // timer 4A
        TCCR4E |= (1<<ENHC4); // enhanced pwm mode
        TCCR4B &= ~(1<<CS41); TCCR4B |= (1<<CS42)|(1<<CS40); // prescaler to 16
        TCCR4D |= (1<<WGM40); TC4H = 0x3; OCR4C = 0xFF; // phase and frequency correct mode & top to 1023 but with enhanced pwm mode we have 2047
        TCCR4A |= (1<<COM4A0)|(1<<PWM4A); // connect pin 5 to timer 4 channel A 
      #else // timer 3A
        TCCR3A |= (1<<WGM31); // phase correct mode & no prescaler
        TCCR3A &= ~(1<<WGM30);
        TCCR3B &= ~(1<<WGM32) &  ~(1<<CS31) & ~(1<<CS32);
        TCCR3B |= (1<<WGM33) | (1<<CS30); 
        ICR3   |= 0x3FFF; // TOP to 16383;     
        TCCR3A |= _BV(COM3A1); // connect pin 5 to timer 3 channel A    
      #endif 
    #endif
    #if (NUMBER_MOTOR > 3)
      #if defined(HWPWM6) 
        TCCR4E |= (1<<ENHC4); // enhanced pwm mode
        TCCR4B &= ~(1<<CS41); TCCR4B |= (1<<CS42)|(1<<CS40); // prescaler to 16
        TCCR4D |= (1<<WGM40); TC4H = 0x3; OCR4C = 0xFF; // phase and frequency correct mode & top to 1023 but with enhanced pwm mode we have 2047
      #endif
      TCCR4C |= (1<<COM4D1)|(1<<PWM4D); // connect pin 6 to timer 4 channel D
    #endif
    #if (NUMBER_MOTOR > 4)
      #if defined(HWPWM6) 
        TCCR1A |= _BV(COM1C1); // connect pin 11 to timer 1 channel C
        TCCR4A |= (1<<COM4A1)|(1<<PWM4A); // connect pin 13 to timer 4 channel A 
      #else
        initializeSoftPWM();
      #endif
    #endif
    #if (NUMBER_MOTOR > 6)
      #if defined(HWPWM6) 
        initializeSoftPWM();
      #endif
    #endif
  #endif
  
 /********  Specific PWM Timers & Registers for the atmega328P (Promini)   ************/
  #if defined(PROMINI)
    #if (NUMBER_MOTOR > 0)
      TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A
    #endif
    #if (NUMBER_MOTOR > 1)
      TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B
    #endif
     ////////////////////////////////////////////////////////////////////////////////////////////////////
    #if (NUMBER_MOTOR > 2)
      //TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A
    #endif
    #if (NUMBER_MOTOR > 3)
      //TCCR2A |= _BV(COM2B1); // connect pin 3 to timer 2 channel B
      init_arduIMU_PWM();
    #endif
     ////////////////////////////////////////////////////////////////////////////////////////////////////
    #if (NUMBER_MOTOR > 5)  // PIN 5 & 6 or A0 & A1
      initializeSoftPWM();
      #if defined(A0_A1_PIN_HEX) || (NUMBER_MOTOR > 6)
        pinMode(5,INPUT);pinMode(6,INPUT);     // we reactivate the INPUT affectation for these two PINs
        pinMode(A0,OUTPUT);pinMode(A1,OUTPUT);
      #endif
    #endif
  #endif
  
  writeAllMotors(MINCOMMAND);
  delay(300);
  #if defined(SERVO)
    initializeServo();
  #endif
}





 ////////////////////////////////////////////////////////////////////////////////////////////////////
void init_arduIMU_PWM(){
  // pin 12 & 13 must be set as output but thay are if you changed the Motors order
  
  // lower timer 2's frequency (prescaler to 128)
  TCCR2B = TCCR2B & 0b11111000 | 0x05;
  
  // set timer 2 to normal counting mode
  TCCR2A = 0; // normal counting mode
  
  // note because it is now in normal mode (that is more like fast PWM mode you may need rise the prescaler of timer 2 -> http://arduino.cc/playground/Code/PwmFrequency)
  
  //enable the two interrupts for timer2's comperators a & B
  TIMSK2 |= (1<<OCIE2A); // enable comperatorA's ISR
  TIMSK2 |= (1<<OCIE2B); // enable comperatorB's ISR 
}


// for comperator A // pin 12

ISR(TIMER2_COMPA_vect){
     static uint8_t state = 0;
     if(state==0){
           PORTB |= (1<<4); // pin 12 high
           OCR2A += pin12_high; // wait for the dutytime
           state = 1;
     }else{
           PORTB &= ~(1<<4); // pin 12 low
           OCR2A += pin12_low; // wait for the rest
           state = 0;
     }
}

// for comperator B // pin 13
ISR(TIMER2_COMPB_vect){
     static uint8_t state = 0;
     if(state==0){
           PORTB |= (1<<5); // pin 13 high
           OCR2B += pin13_high; // wait for the dutytime
           state = 1;
     }else{
           PORTB &= ~(1<<5); // pin 13 low
           OCR2B += pin13_low; // wait for the rest
           state = 0;
     }
}
 ////////////////////////////////////////////////////////////////////////////////////////////////////







#if defined(SERVO)
/**************************************************************************************/
/************                Initialize the PWM Servos               ******************/
/**************************************************************************************/
void initializeServo() {
  #if (PRI_SERVO_FROM == 1) || (SEC_SERVO_FROM == 1)
    SERVO_1_PINMODE;
  #endif
  #if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2) 
    SERVO_2_PINMODE;
  #endif
  #if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3) 
    SERVO_3_PINMODE;
  #endif 
  #if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4) 
    SERVO_4_PINMODE;
  #endif 
  #if (PRI_SERVO_FROM <= 5 && PRI_SERVO_TO >= 5) || (SEC_SERVO_FROM <= 5 && SEC_SERVO_TO >= 5) 
    SERVO_5_PINMODE;
  #endif 
  #if (PRI_SERVO_FROM <= 6 && PRI_SERVO_TO >= 6) || (SEC_SERVO_FROM <= 6 && SEC_SERVO_TO >= 6) 
    SERVO_6_PINMODE;
  #endif 
  #if (PRI_SERVO_FROM <= 7 && PRI_SERVO_TO >= 7) || (SEC_SERVO_FROM <= 7 && SEC_SERVO_TO >= 7) 
    SERVO_7_PINMODE;
  #endif 
  #if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8) 
    SERVO_8_PINMODE;
  #endif
  
  #if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6)) // uses timer 0 Comperator A (8 bit)
    TCCR0A = 0; // normal counting mode
    TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
    #define SERVO_ISR TIMER0_COMPA_vect
    #define SERVO_CHANNEL OCR0A
    #define SERVO_1K_US 250
  #endif
  #if (defined(PROMICRO) && !defined(HWPWM6)) // uses timer 3 Comperator A (11 bit)
    TCCR3A &= ~(1<<WGM30) & ~(1<<WGM31); //normal counting & no prescaler
    TCCR3B &= ~(1<<WGM32) & ~(1<<CS31) & ~(1<<CS32) & ~(1<<WGM33);
    TCCR3B |= (1<<CS30);   
    TIMSK3 |= (1<<OCIE3A); // Enable CTC interrupt 
    #define SERVO_ISR TIMER3_COMPA_vect
    #define SERVO_CHANNEL OCR3A
    #define SERVO_1K_US 16000 
  #endif
  #if defined(MEGA) // uses timer 5 Comperator A (11 bit)
    TCCR5A &= ~(1<<WGM50) & ~(1<<WGM51); //normal counting & no prescaler
    TCCR5B &= ~(1<<WGM52) & ~(1<<CS51) & ~(1<<CS52) & ~(1<<WGM53);
    TCCR5B |= (1<<CS50);   
    TIMSK5 |= (1<<OCIE5A); // Enable CTC interrupt  
    #define SERVO_ISR TIMER5_COMPA_vect
    #define SERVO_CHANNEL OCR5A
    #define SERVO_1K_US 16000 
  #endif
}

/**************************************************************************************/
/************              Servo software PWM generation             ******************/
/**************************************************************************************/

// prescaler is set by default to 64 on Timer0
// Duemilanove : 16MHz / 64 => 4 us
// 256 steps = 1 counter cycle = 1024 us

// for servo 2-8
// its almost the same as for servo 1
#define SERVO_PULSE(PIN_HIGH,ACT_STATE,SERVO_NUM,LAST_PIN_LOW) \
  }else if(state == ACT_STATE){                                \
    LAST_PIN_LOW;                                              \
    PIN_HIGH;                                                  \
    SERVO_CHANNEL+=SERVO_1K_US;                                \
    state++;                                                   \
  }else if(state == ACT_STATE+1){                              \
    SERVO_CHANNEL+=atomicServo[SERVO_NUM];                     \
    state++;                                                   \

ISR(SERVO_ISR) {
  static uint8_t state = 0; // indicates the current state of the chain
  if(state == 0){
    SERVO_1_HIGH; // set servo 1's pin high 
    SERVO_CHANNEL+=SERVO_1K_US; // wait 1000us
    state++; // count up the state
  }else if(state==1){
    SERVO_CHANNEL+=atomicServo[SERVO_1_ARR_POS]; // load the servo's value (0-1000us)
    state++; // count up the state
  #if defined(SERVO_2_HIGH)
    SERVO_PULSE(SERVO_2_HIGH,2,SERVO_2_ARR_POS,SERVO_1_LOW); // the same here
  #endif
  #if defined(SERVO_3_HIGH)
    SERVO_PULSE(SERVO_3_HIGH,4,SERVO_3_ARR_POS,SERVO_2_LOW);
  #endif
  #if defined(SERVO_4_HIGH)
    SERVO_PULSE(SERVO_4_HIGH,6,SERVO_4_ARR_POS,SERVO_3_LOW);
  #endif
  #if defined(SERVO_5_HIGH)
    SERVO_PULSE(SERVO_5_HIGH,8,SERVO_5_ARR_POS,SERVO_4_LOW);
  #endif
  #if defined(SERVO_6_HIGH)
    SERVO_PULSE(SERVO_6_HIGH,10,SERVO_6_ARR_POS,SERVO_5_LOW);
  #endif
  #if defined(SERVO_7_HIGH)
    SERVO_PULSE(SERVO_7_HIGH,12,SERVO_7_ARR_POS,SERVO_6_LOW);
  #endif
  #if defined(SERVO_8_HIGH)
    SERVO_PULSE(SERVO_8_HIGH,14,SERVO_8_ARR_POS,SERVO_7_LOW);
  #endif
  }else{
    LAST_LOW;
    #if defined(SERVO_RFR_300HZ)
      #if defined(SERVO_3_HIGH)  // if there are 3 or more servos we dont need to slow it down
        SERVO_CHANNEL+=(SERVO_1K_US>>3); // 0 would be better but it causes bad jitter
        state=0; 
      #else // if there are less then 3 servos we need to slow it to not go over 300Hz (the highest working refresh rate for the digital servos for what i know..)
        SERVO_CHANNEL+=SERVO_1K_US;
        if(state<4){
          state+=2;
        }else{
          state=0;
        }
      #endif
    #endif
    #if defined(SERVO_RFR_160HZ)
      #if defined(SERVO_4_HIGH)  // if there are 4 or more servos we dont need to slow it down
        SERVO_CHANNEL+=(SERVO_1K_US>>3); // 0 would be better but it causes bad jitter
        state=0; 
      #else // if there are less then 4 servos we need to slow it to not go over ~170Hz (the highest working refresh rate for analog servos)
        SERVO_CHANNEL+=SERVO_1K_US;
        if(state<8){
          state+=2;
        }else{
          state=0;
        }
      #endif
    #endif   
    #if defined(SERVO_RFR_50HZ) // to have ~ 50Hz for all servos
      SERVO_CHANNEL+=SERVO_1K_US;
      if(state<30){
        state+=2;
      }else{
        state=0;
      }     
    #endif
  }
} 
#endif

/**************************************************************************************/
/************             Motor software PWM generation              ******************/
/**************************************************************************************/
// SW PWM is only used if there are not enough HW PWM pins (for exampe hexa on a promini)

#if (NUMBER_MOTOR > 4) && (defined(PROMINI) || defined(PROMICRO))

/****************    Pre define the used ISR's and Timerchannels     ******************/
  #if !defined(PROMICRO)
    #define SOFT_PWM_ISR1 TIMER0_COMPB_vect
    #define SOFT_PWM_ISR2 TIMER0_COMPA_vect
    #define SOFT_PWM_CHANNEL1 OCR0B
    #define SOFT_PWM_CHANNEL2 OCR0A 
  #elif !defined(HWPWM6)
    #define SOFT_PWM_ISR1 TIMER3_COMPB_vect
    #define SOFT_PWM_ISR2 TIMER3_COMPC_vect
    #define SOFT_PWM_CHANNEL1 OCR3B
    #define SOFT_PWM_CHANNEL2 OCR3C 
  #else
    #define SOFT_PWM_ISR2 TIMER0_COMPB_vect  
    #define SOFT_PWM_CHANNEL2 OCR0B 
  #endif
  
/****************         Initialize Timers and PWM Channels         ******************/
  void initializeSoftPWM() {
    #if !defined(PROMICRO)
      TCCR0A = 0; // normal counting mode
      #if (NUMBER_MOTOR > 4) && !defined(HWPWM6) 
        TIMSK0 |= (1<<OCIE0B); // Enable CTC interrupt  
      #endif
      #if (NUMBER_MOTOR > 6) || ((NUMBER_MOTOR == 6) && !defined(SERVO))

        TIMSK0 |= (1<<OCIE0A);
      #endif
    #else
      #if !defined(HWPWM6)
        TCCR3A &= ~(1<<WGM30) & ~(1<<WGM31); //normal counting & no prescaler
        TCCR3B &= ~(1<<WGM32) & ~(1<<CS31) & ~(1<<CS32) & ~(1<<WGM33);
        TCCR3B |= (1<<CS30);   
        TIMSK3 |= (1<<OCIE3B); // Enable CTC interrupt  
        #if (NUMBER_MOTOR > 6) || ((NUMBER_MOTOR == 6) && !defined(SERVO))
          TIMSK3 |= (1<<OCIE3C);
        #endif   
      #else
        TCCR0A = 0; // normal counting mode
        TIMSK0 |= (1<<OCIE0B); // Enable CTC interrupt 
      #endif
    #endif
  }
  
/****************               Motor SW PWM ISR's                 ******************/
  // hexa with old but sometimes better SW PWM method
  // for setups without servos
  #if (NUMBER_MOTOR == 6) && (!defined(SERVO) && !defined(HWPWM6))
    ISR(SOFT_PWM_ISR1) { 
      static uint8_t state = 0;
      if(state == 0){
        SOFT_PWM_1_PIN_HIGH;
        SOFT_PWM_CHANNEL1 += atomicPWM_PIN5_highState;
        state = 1;
      }else if(state == 1){
        SOFT_PWM_CHANNEL1 += atomicPWM_PIN5_highState;
        state = 2;
      }else if(state == 2){
        SOFT_PWM_1_PIN_LOW;
        SOFT_PWM_CHANNEL1 += atomicPWM_PIN5_lowState;
        state = 3;  
      }else if(state == 3){
        SOFT_PWM_CHANNEL1 += atomicPWM_PIN5_lowState;
        state = 0;   
      }
    }
    ISR(SOFT_PWM_ISR2) { 
      static uint8_t state = 0;
      if(state == 0){
        SOFT_PWM_2_PIN_HIGH;
        SOFT_PWM_CHANNEL2 += atomicPWM_PIN6_highState;
        state = 1;
      }else if(state == 1){
        SOFT_PWM_CHANNEL2 += atomicPWM_PIN6_highState;
        state = 2;
      }else if(state == 2){
        SOFT_PWM_2_PIN_LOW;
        SOFT_PWM_CHANNEL2 += atomicPWM_PIN6_lowState;
        state = 3;  
      }else if(state == 3){
        SOFT_PWM_CHANNEL2 += atomicPWM_PIN6_lowState;
        state = 0;   
      }
    }
  #else
    #if (NUMBER_MOTOR > 4) && !defined(HWPWM6)
      // HEXA with just OCR0B 
      ISR(SOFT_PWM_ISR1) { 
        static uint8_t state = 0;
        if(state == 0){
          SOFT_PWM_1_PIN_HIGH;
          SOFT_PWM_CHANNEL1 += atomicPWM_PIN5_highState;
          state = 1;
        }else if(state == 1){
          SOFT_PWM_2_PIN_LOW;
          SOFT_PWM_CHANNEL1 += atomicPWM_PIN6_lowState;
          state = 2;
        }else if(state == 2){
          SOFT_PWM_2_PIN_HIGH;
          SOFT_PWM_CHANNEL1 += atomicPWM_PIN6_highState;
          state = 3;  
        }else if(state == 3){
          SOFT_PWM_1_PIN_LOW;
          SOFT_PWM_CHANNEL1 += atomicPWM_PIN5_lowState;
          state = 0;   
        }
      } 
    #endif
    //the same with digital PIN A2 & 12 OCR0A counter for OCTO
    #if (NUMBER_MOTOR > 6)
      ISR(SOFT_PWM_ISR2) {
        static uint8_t state = 0;
        if(state == 0){
          SOFT_PWM_3_PIN_HIGH;
          SOFT_PWM_CHANNEL2 += atomicPWM_PINA2_highState;
          state = 1;
        }else if(state == 1){
          SOFT_PWM_4_PIN_LOW;
          SOFT_PWM_CHANNEL2 += atomicPWM_PIN12_lowState;
          state = 2;
        }else if(state == 2){
          SOFT_PWM_4_PIN_HIGH;
          SOFT_PWM_CHANNEL2 += atomicPWM_PIN12_highState;
          state = 3;  
        }else if(state == 3){
          SOFT_PWM_3_PIN_LOW;
          SOFT_PWM_CHANNEL2 += atomicPWM_PINA2_lowState;
          state = 0;   
        }
      }
    #endif
  #endif
#endif


/**************************************************************************************/
/********** Mixes the Computed stabilize values to the Motors & Servos  ***************/
/**************************************************************************************/
void mixTable() {
  int16_t maxMotor;
  uint8_t i;

  #define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z

  #if NUMBER_MOTOR > 3
    //prevent "yaw jump" during yaw correction
    axisPID[YAW] = constrain(axisPID[YAW],-100-abs(rcCommand[YAW]),+100+abs(rcCommand[YAW]));
  #endif
  /****************                   main Mix Table                ******************/
  #ifdef BI
    motor[0] = PIDMIX(+1, 0, 0); //LEFT
    motor[1] = PIDMIX(-1, 0, 0); //RIGHT        
    servo[4]  = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
    servo[5]  = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
  #endif
  #ifdef TRI
    motor[0] = PIDMIX( 0,+4/3, 0); //REAR
    motor[1] = PIDMIX(-1,-2/3, 0); //RIGHT
    motor[2] = PIDMIX(+1,-2/3, 0); //LEFT
    servo[5] = constrain(conf.tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
  #endif
  #ifdef QUADP
    motor[0] = PIDMIX( 0,+1,-1); //REAR
    motor[1] = PIDMIX(-1, 0,+1); //RIGHT
    motor[2] = PIDMIX(+1, 0,+1); //LEFT
    motor[3] = PIDMIX( 0,-1,-1); //FRONT
  #endif
  #ifdef QUADX
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L
    motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
  #endif
  #ifdef Y4
    motor[0] = PIDMIX(+0,+1,-1);   //REAR_1 CW
    motor[1] = PIDMIX(-1,-1, 0); //FRONT_R CCW
    motor[2] = PIDMIX(+0,+1,+1);   //REAR_2 CCW
    motor[3] = PIDMIX(+1,-1, 0); //FRONT_L CW
  #endif
  #ifdef Y6
    motor[0] = PIDMIX(+0,+4/3,+1); //REAR
    motor[1] = PIDMIX(-1,-2/3,-1); //RIGHT
    motor[2] = PIDMIX(+1,-2/3,-1); //LEFT
    motor[3] = PIDMIX(+0,+4/3,-1); //UNDER_REAR
    motor[4] = PIDMIX(-1,-2/3,+1); //UNDER_RIGHT
    motor[5] = PIDMIX(+1,-2/3,+1); //UNDER_LEFT    
  #endif
  #ifdef HEX6
    motor[0] = PIDMIX(-1/2,+1/2,+1); //REAR_R
    motor[1] = PIDMIX(-1/2,-1/2,-1); //FRONT_R
    motor[2] = PIDMIX(+1/2,+1/2,+1); //REAR_L
    motor[3] = PIDMIX(+1/2,-1/2,-1); //FRONT_L
    motor[4] = PIDMIX(+0  ,-1  ,+1); //FRONT
    motor[5] = PIDMIX(+0  ,+1  ,-1); //REAR
  #endif
  #ifdef HEX6X
    motor[0] = PIDMIX(-1/2,+1/2,+1); //REAR_R
    motor[1] = PIDMIX(-1/2,-1/2,+1); //FRONT_R
    motor[2] = PIDMIX(+1/2,+1/2,-1); //REAR_L
    motor[3] = PIDMIX(+1/2,-1/2,-1); //FRONT_L
    motor[4] = PIDMIX(-1  ,+0  ,-1); //RIGHT
    motor[5] = PIDMIX(+1  ,+0  ,+1); //LEFT
  #endif
  #ifdef OCTOX8
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L
    motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
    motor[4] = PIDMIX(-1,+1,+1); //UNDER_REAR_R
    motor[5] = PIDMIX(-1,-1,-1); //UNDER_FRONT_R
    motor[6] = PIDMIX(+1,+1,-1); //UNDER_REAR_L
    motor[7] = PIDMIX(+1,-1,+1); //UNDER_FRONT_L
  #endif
  #ifdef OCTOFLATP
    motor[0] = PIDMIX(+7/10,-7/10,+1); //FRONT_L
    motor[1] = PIDMIX(-7/10,-7/10,+1); //FRONT_R
    motor[2] = PIDMIX(-7/10,+7/10,+1); //REAR_R
    motor[3] = PIDMIX(+7/10,+7/10,+1); //REAR_L
    motor[4] = PIDMIX(+0   ,-1   ,-1); //FRONT
    motor[5] = PIDMIX(-1   ,+0   ,-1); //RIGHT
    motor[6] = PIDMIX(+0   ,+1   ,-1); //REAR
    motor[7] = PIDMIX(+1   ,+0   ,-1); //LEFT 
  #endif
  #ifdef OCTOFLATX
    motor[0] = PIDMIX(+1  ,-1/2,+1); //MIDFRONT_L
    motor[1] = PIDMIX(-1/2,-1  ,+1); //FRONT_R
    motor[2] = PIDMIX(-1  ,+1/2,+1); //MIDREAR_R
    motor[3] = PIDMIX(+1/2,+1  ,+1); //REAR_L
    motor[4] = PIDMIX(+1/2,-1  ,-1); //FRONT_L
    motor[5] = PIDMIX(-1  ,-1/2,-1); //MIDFRONT_R
    motor[6] = PIDMIX(-1/2,+1  ,-1); //REAR_R
    motor[7] = PIDMIX(+1  ,+1/2,-1); //MIDREAR_L 
  #endif
  #ifdef VTAIL4
    motor[0] = PIDMIX(+0,+1, -1/2); //REAR_R 
    motor[1] = PIDMIX(-1, -1, +0);  //FRONT_R 
    motor[2] = PIDMIX(+0,+1, +1/2); //REAR_L 
    motor[3] = PIDMIX(+1, -1, -0);  //FRONT_L
  #endif

  /****************                Cam stabilize Sevos             ******************/
  #if defined(SERVO_TILT)
    #if defined(A0_A1_PIN_HEX) && (NUMBER_MOTOR == 6) && defined(PROMINI)
      #define S_PITCH servo[2]
      #define S_ROLL  servo[3]
    #else
      #define S_PITCH servo[0]
      #define S_ROLL  servo[1]
    #endif
    S_PITCH = TILT_PITCH_MIDDLE + rcData[AUX3]-1500;
    S_ROLL  = TILT_ROLL_MIDDLE  + rcData[AUX4]-1500;
    if (rcOptions[BOXCAMSTAB]) {
      S_PITCH += TILT_PITCH_PROP * angle[PITCH] /16 ;
      S_ROLL  += TILT_ROLL_PROP  * angle[ROLL]  /16 ;
    }
    S_PITCH = constrain(S_PITCH, TILT_PITCH_MIN, TILT_PITCH_MAX);
    S_ROLL  = constrain(S_ROLL , TILT_ROLL_MIN, TILT_ROLL_MAX  );   
  #endif
  
  
 /************************************************************************************************************/ 
 // Bledi Experimentals
 /************************************************************************************************************/ 
   #ifdef SERVO_MIX_TILT 
   // Simple CameraGimbal By Bledy http://youtu.be/zKGr6iR54vM
    if (rcOptions[BOXCAMSTAB]) {
      servo[0] = constrain(TILT_PITCH_MIDDLE - (-TILT_ROLL_PROP) * angle[PITCH] /16 - TILT_ROLL_PROP * angle[ROLL] /16 , TILT_PITCH_MIN, TILT_PITCH_MAX);
      servo[1] = constrain(TILT_ROLL_MIDDLE + (-TILT_ROLL_PROP) * angle[PITCH] /16 - TILT_ROLL_PROP * angle[ROLL] /16 , TILT_ROLL_MIN, TILT_ROLL_MAX);
    } else {
        // to use it with A0_A1_PIN_HEX
      #if defined(A0_A1_PIN_HEX) && (NUMBER_MOTOR == 6) && defined(PROMINI)
        servo[2] = constrain(TILT_PITCH_MIDDLE  + rcData[AUX3]-1500 , TILT_PITCH_MIN, TILT_PITCH_MAX);
        servo[3] = constrain(TILT_ROLL_MIDDLE   + rcData[AUX4]-1500,  TILT_ROLL_MIN, TILT_ROLL_MAX);     
      #else
        servo[0] = constrain(TILT_PITCH_MIDDLE  + rcData[AUX3]-1500 , TILT_PITCH_MIN, TILT_PITCH_MAX);
        servo[1] = constrain(TILT_ROLL_MIDDLE   + rcData[AUX4]-1500,  TILT_ROLL_MIN, TILT_ROLL_MAX);
      #endif
    }
  #endif  
 /************************************************************************************************************/ 
 // End of Bledi Experimentals
 /************************************************************************************************************/ 
  
  #ifdef GIMBAL
    servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP   * angle[ROLL]  /16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
  #endif
  #if defined(FLYING_WING)
    motor[0] = rcCommand[THROTTLE];
    if (passThruMode) {// do not use sensors for correction, simple 2 channel mixing
       servo[0]  = PITCH_DIRECTION_L * (rcData[PITCH]-MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL]-MIDRC);
       servo[1]  = PITCH_DIRECTION_R * (rcData[PITCH]-MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL]-MIDRC);
    } else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
       servo[0]  = PITCH_DIRECTION_L * axisPID[PITCH]        + ROLL_DIRECTION_L * axisPID[ROLL];
       servo[1]  = PITCH_DIRECTION_R * axisPID[PITCH]        + ROLL_DIRECTION_R * axisPID[ROLL];
    }
    servo[0]  = constrain(servo[0] + conf.wing_left_mid , WING_LEFT_MIN,  WING_LEFT_MAX );
    servo[1]  = constrain(servo[1] + conf.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
  #endif
  
  /************************************************************************************************************/
  #if defined(AIRPLANE)
    // Common parts for Plane and Heli
    static int16_t   servoMid[8];                        // Midpoint on servo
    static uint8_t   servoTravel[8] = SERVO_RATES;       // Rates in 0-100% 
    static int8_t    servoReverse[8] = SERVO_DIRECTION ; // Inverted servos
    static int16_t   servoLimit[8][2]; // Holds servoLimit data

    /***************************
     * servo endpoints Airplane. 
     ***************************/
  #define SERVO_MIN 1020           // limit servo travel range must be inside [1020;2000]
  #define SERVO_MAX 2000           // limit servo travel range must be inside [1020;2000]
    for(i=0; i<8; i++){  //  Set rates with 0 - 100%. 
      servoMid[i]     =MIDRC + conf.servoTrim[i];
      servoLimit[i][0]=servoMid[i]-((servoMid[i]-SERVO_MIN)   *(servoTravel[i]*0.01));
      servoLimit[i][1]=servoMid[i]+((SERVO_MAX - servoMid[i]) *(servoTravel[i]*0.01));  
    }

    // servo[7] is programmed with safty features to avoid motorstarts when ardu reset..  
    // All other servos go to center at reset..  Half throttle can be dangerus    
    // Only use servo[7] as motorcontrol if motor is used in the setup            */
    if (!armed){ 
      servo[7] =  MINCOMMAND; // Kill throttle when disarmed
    } else {   
      servo[7] =  rcData[THROTTLE];
    }

    // Flapperon Controll
    int16_t flaps[2]={0,0};    
  #if  defined(FLAP_CHANNEL) && defined(FLAP_EP) && defined(FLAP_INVERT)  
    int8_t flapinv[2] = FLAP_INVERT; 
    static int16_t F_Endpoint[2] = FLAP_EP;
    int16_t flap = (MIDRC- constrain(rcData[FLAP_CHANNEL],F_Endpoint[0],F_Endpoint[1]));
    for(i=0; i<2; i++){flaps[i] = flap * flapinv[i] ;}
  #endif

    if(passThruMode){   // Direct passthru from RX 
      servo[3]  = servoMid[3]+((rcCommand[ROLL] + flaps[0]) *servoReverse[3]);     //   Wing 1
      servo[4]  = servoMid[4]+((rcCommand[ROLL] + flaps[1]) *servoReverse[4]);     //   Wing 2
      servo[5]  = servoMid[5]+(rcCommand[YAW]               *servoReverse[5]);     //   Rudder
      servo[6]  = servoMid[6]+(rcCommand[PITCH]             *servoReverse[6]);     //   Elevator 
    }else{
      // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
      servo[3]  =(servoMid[3] + ((axisPID[ROLL] + flaps[0]) *servoReverse[3]));   //   Wing 1 
      servo[4]  =(servoMid[4] + ((axisPID[ROLL] + flaps[1]) *servoReverse[4]));   //   Wing 2
      servo[5]  =(servoMid[5] + (axisPID[YAW]               *servoReverse[5]));   //   Rudder
      servo[6]  =(servoMid[6] + (axisPID[PITCH]             *servoReverse[6]));   //   Elevator
    } 
    // ServoRates
    for(i=3;i<8;i++){
      servo[i]  = map(servo[i], SERVO_MIN, SERVO_MAX,servoLimit[i][0],servoLimit[i][1]);
      servo[i]  = constrain( servo[i], SERVO_MIN, SERVO_MAX);
    }

  #endif

  /************************************************************************************************************/
  #ifdef HELICOPTER 
  // Common controlls for Helicopters 
    int16_t heliRoll,heliNick;
    int16_t collRange[3] = COLLECTIVE_RANGE;
    static int16_t   collective;   
    static int16_t   servoEndpiont[8][2];
    static int16_t   servoHigh[8] = SERVO_ENDPOINT_HIGH; // HIGHpoint on servo
    static int16_t   servoLow[8]  = SERVO_ENDPOINT_LOW ; // LOWpoint on servo

  /***************************
   * servo settings Heli. 
   ***************************/
    for(i=0; i<8; i++){  //  Set rates using endpoints. 
      servoEndpiont[i][0] = servoLow[i];  //Min
      servoEndpiont[i][1] = servoHigh[i]; //Max   
    }

  // Limit Collective range up/down    
    int16_t collect = rcData[COLLECTIVE_PITCH]-collRange[1];
    if   (collect>0) { 
      collective = collect * (collRange[2]*0.01); 
    } else{
      collective = collect * (collRange[0]*0.01); 
    } 

    if(passThruMode){ // Use Rcdata Without sensors
      heliRoll=  rcCommand[ROLL] ;
      heliNick=  rcCommand[PITCH];
    } else{ // Assisted modes
      heliRoll= axisPID[ROLL];
      heliNick= axisPID[PITCH];
    }  

  // Limit Maximum Rates for Heli
    int16_t cRange[2] = CONTROLL_RANGE;
    heliRoll*=cRange[0]*0.01;
    heliNick*=cRange[1]*0.01;
   
  #define HeliXPIDMIX(Z,Y,X) collRange[1]+collective*Z + heliNick*Y +  heliRoll*X

  // Yaw is common for Heli 90 & 120
    uint16_t yawControll =  YAW_CENTER + (axisPID[YAW]*YAW_DIRECTION) + conf.servoTrim[5];

  /* Throttle & YAW
  ********************
  Handeled in common functions for Heli */
    if (!armed){ 
      servo[7] = 900; // Kill throttle when disarmed
      if (YAWMOTOR){servo[5] =  MINCOMMAND;} else {servo[5] =  yawControll; } // Kill YAWMOTOR when disarmed
    }else {   
      servo[7]  = rcData[THROTTLE]; //   50hz ESC or servo
      if (YAWMOTOR && rcData[THROTTLE] < MINTHROTTLE){servo[5] =  MINCOMMAND;}
      else{ servo[5] =  yawControll; }     // YawSero    
    }


  //              ( Collective, Pitch/Nick, Roll ) Change sign to invert
  /************************************************************************************************************/
  #ifdef HELI_120_CCPM 
    static int8_t nickMix[3] =SERVO_NICK;
    static int8_t leftMix[3] =SERVO_LEFT;
    static int8_t rightMix[3]=SERVO_RIGHT;

    servo[3]  =  HeliXPIDMIX( (nickMix[0]*0.1) , nickMix[1]*0.1, nickMix[2]*0.1) +conf.servoTrim[3] ;   //    NICK  servo
    servo[4]  =  HeliXPIDMIX( (leftMix[0]*0.1) , leftMix[1]*0.1, leftMix[2]*0.1) +conf.servoTrim[4] ;   //    LEFT servo
    servo[6]  =  HeliXPIDMIX( (rightMix[0]*0.1),rightMix[1]*0.1,rightMix[2]*0.1) +conf.servoTrim[6] ;   //    RIGHT  servo

  #endif

  /************************************************************************************************************/
  #ifdef HELI_90_DEG   
    static int8_t servoDir[3]=SERVO_DIRECTIONS;   
    servo[3]  = HeliXPIDMIX( +0, servoDir[1], -0)+conf.servoTrim[3] ;      //     NICK  servo
    servo[4]  = HeliXPIDMIX( +0, +0, servoDir[2])+conf.servoTrim[4] ;      //     ROLL servo
    servo[6]  = HeliXPIDMIX( servoDir[0], +0, +0)+conf.servoTrim[6] ;      //     COLLECTIVE  servo
  #endif    

    for(i=3;i<8;i++){
      servo[i]  = constrain( servo[i], servoEndpiont[i][0], servoEndpiont[i][1] ); 
    }

  #endif
  
// End of PatrikE Experimentals
/************************************************************************************************************/
 
  /****************                    Cam trigger Sevo                ******************/
  #if defined(CAMTRIG)
    static uint8_t camCycle = 0;
    static uint8_t camState = 0;
    static uint32_t camTime = 0;
    if (camCycle==1) {
      if (camState == 0) {
        servo[2] = CAM_SERVO_HIGH;
        camState = 1;
        camTime = millis();
      } else if (camState == 1) {
       if ( (millis() - camTime) > CAM_TIME_HIGH ) {
         servo[2] = CAM_SERVO_LOW;
         camState = 2;
         camTime = millis();
       }
      } else { //camState ==2
       if ( (millis() - camTime) > CAM_TIME_LOW ) {
         camState = 0;
         camCycle = 0;
       }
      }
    }
    if (rcOptions[BOXCAMTRIG]) camCycle=1;
  #endif
  
  /****************                Filter the Motors values                ******************/
  maxMotor=motor[0];
  for(i=1;i< NUMBER_MOTOR;i++)
    if (motor[i]>maxMotor) maxMotor=motor[i];
  for (i = 0; i < NUMBER_MOTOR; i++) {
    if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
      motor[i] -= maxMotor - MAXTHROTTLE;
    motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);    
    if ((rcData[THROTTLE]) < MINCHECK)
      #ifndef MOTOR_STOP
        motor[i] = MINTHROTTLE;
      #else
        motor[i] = MINCOMMAND;
      #endif
    if (armed == 0)
      motor[i] = MINCOMMAND;
  }
  /****************                      Powermeter Log                    ******************/
  #if (LOG_VALUES == 2) || defined(POWERMETER_SOFT)
    uint32_t amp;
    /* true cubic function; when divided by vbat_max=126 (12.6V) for 3 cell battery this gives maximum value of ~ 500 */

    static uint16_t amperes[64] =   {   0,  2,  6, 15, 30, 52, 82,123,
                                     175,240,320,415,528,659,811,984,
                                     1181,1402,1648,1923,2226,2559,2924,3322,
                                     3755,4224,4730,5276,5861,6489,7160,7875,
                                     8637 ,9446 ,10304,11213,12173,13187,14256,15381,
                                     16564,17805,19108,20472,21900,23392,24951,26578,
                                     28274,30041,31879,33792,35779,37843,39984,42205,
                                     44507,46890,49358,51910,54549,57276,60093,63000};
  
    if (vbat) { // by all means - must avoid division by zero 
      for (i =0;i<NUMBER_MOTOR;i++) {
        amp = amperes[ ((motor[i] - 1000)>>4) ] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp
       #if (LOG_VALUES == 2)
           pMeter[i]+= amp; // sum up over time the mapped ESC input 
        #endif
        #if defined(POWERMETER_SOFT)
           pMeter[PMOTOR_SUM]+= amp; // total sum over all motors
        #endif
      }
    }
  #endif
}


i had to lower the frequency because soft PWM always works like fast PWM (dubble speed)

Code: Select all

void init_arduIMU_PWM(){
  // pin 12 & 13 must be set as output but thay are if you changed the Motors order
  
  // lower timer 2's frequency (prescaler to 128)
  TCCR2B = TCCR2B & 0b11111000 | 0x05;


you also need to disable pin 13 as status LED
in def.h

Code: Select all

/**************************   atmega328P (Promini)  ************************************/
#if defined(PROMINI)
  #if !defined(MONGOOSE1_0)
    #define LEDPIN_PINMODE             //pinMode (13, OUTPUT);
    #define LEDPIN_TOGGLE              //PINB |= 1<<5;     //switch LEDPIN state (digital PIN 13)
    #define LEDPIN_OFF                 //PORTB &= ~(1<<5);
    #define LEDPIN_ON                  //PORTB |= (1<<5);


regards felix

Re: ArduIMU V2 (flat) and Multiwii 6DOF

Posted: Mon Jun 11, 2012 7:57 pm
by LenzGr
Hi Felix,

ronco wrote:sorry for the late replay :(

No worries, this is not urgent - I am grateful for your reply!
i chacked it again and there was some mistakes ;)

here is a full working output :D

Thanks a lot for that, much appreciated. Let me try to trim it down to the relevant parts, I hope I caught all the parts you modified. A simple diff against the original Output.pde file would have been somewhat easier :)

Code: Select all

/**************************************************************************************/
/***************                  Motor Pin order                  ********************/
/**************************************************************************************/
// since we are uing the PWM generation in a direct way, the pin order is just to inizialie the right pins 
// its not possible to change a PWM output pin just by changing the order
#if defined(PROMINI)
  ////////////////////////////////////////////////////////////////////////////////////////////////////
  uint8_t PWM_PIN[8] = {9,10,12,13,6,5,A2,12};   //for a quad+: rear,right,left,front
  ////////////////////////////////////////////////////////////////////////////////////////////////////


I currently define the pin order as follows - does this matter? I assume it's relevant as the writeMotors() needs to be adjusted along the same lines? Also note that I can't use pin 9, but have to use 10-13 instead.

Code: Select all

uint8_t PWM_PIN[4] = {13,10,11,12};   //ArduIMU only has pins for four motors. For a quad+: rear,right,left,front


Code: Select all

 ////////////////////////////////////////////////////////////////////////////////////////////////////
// to 8bit variables to precalculate the dutytimes (ist better then doing it inside of a ISR)
volatile uint8_t pin12_high;
volatile uint8_t pin12_low;

volatile uint8_t pin13_high;
volatile uint8_t pin13_low;
 ////////////////////////////////////////////////////////////////////////////////////////////////////

/**************************************************************************************/
/************  Writes the Motors values to the PWM compare register  ******************/
/**************************************************************************************/
void writeMotors() { // [1000;2000] => [125;250]

[CUT]
  
/********  Specific PWM Timers & Registers for the atmega328P (Promini)   ************/
  #if defined(PROMINI)
    #if (NUMBER_MOTOR > 0)
      #ifndef EXT_MOTOR_RANGE 
        OCR1A = motor[0]>>3; //  pin 9
      #else
        OCR1A = ((motor[0]>>2) - 250) + 2;
      #endif
    #endif
    #if (NUMBER_MOTOR > 1)
      #ifndef EXT_MOTOR_RANGE 
        OCR1B = motor[1]>>3; //  pin 10
      #else
        OCR1B = ((motor[1]>>2) - 250) + 2;
      #endif
    #endif
     ////////////////////////////////////////////////////////////////////////////////////////////////////
    #if (NUMBER_MOTOR > 2)
      /*
      #ifndef EXT_MOTOR_RANGE
        OCR2A = motor[2]>>3; //  pin 11
      #else
        OCR2A = ((motor[2]>>2) - 250) + 2;
      #endif
      */
      pin12_high = motor[2]>>3;
      pin12_low = 255-pin12_high;
    #endif
    #if (NUMBER_MOTOR > 3)
     /*
      #ifndef EXT_MOTOR_RANGE
        OCR2B = motor[3]>>3; //  pin 3
      #else
        OCR2B = ((motor[3]>>2) - 250) + 2;
      #endif
      */
      pin13_high = motor[3]>>3;
      pin13_low = 255-pin13_high;   
      
    #endif

[CUT] 

}

/**************************************************************************************/
/************        Initialize the PWM Timers and Registers         ******************/
/**************************************************************************************/
void initOutput() {
  
/****************            mark all PWM pins as Output             ******************/
  for(uint8_t i=0;i<NUMBER_MOTOR;i++)
    pinMode(PWM_PIN[i],OUTPUT);
    
[CUT]
  
 /********  Specific PWM Timers & Registers for the atmega328P (Promini)   ************/
  #if defined(PROMINI)
    #if (NUMBER_MOTOR > 0)
      TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A
    #endif
    #if (NUMBER_MOTOR > 1)
      TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B
    #endif
     ////////////////////////////////////////////////////////////////////////////////////////////////////
    #if (NUMBER_MOTOR > 2)
      //TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A
    #endif
    #if (NUMBER_MOTOR > 3)
      //TCCR2A |= _BV(COM2B1); // connect pin 3 to timer 2 channel B
      init_arduIMU_PWM();
    #endif

[CUT]

void init_arduIMU_PWM(){
  // pin 12 & 13 must be set as output but thay are if you changed the Motors order
  
  // lower timer 2's frequency (prescaler to 128)
  TCCR2B = TCCR2B & 0b11111000 | 0x05;
  
  // set timer 2 to normal counting mode
  TCCR2A = 0; // normal counting mode
  
  // note because it is now in normal mode (that is more like fast PWM mode you may need rise the prescaler of timer 2 -> http://arduino.cc/playground/Code/PwmFrequency)
  
  //enable the two interrupts for timer2's comperators a & B
  TIMSK2 |= (1<<OCIE2A); // enable comperatorA's ISR
  TIMSK2 |= (1<<OCIE2B); // enable comperatorB's ISR 
}

// for comperator A // pin 12

ISR(TIMER2_COMPA_vect){
     static uint8_t state = 0;
     if(state==0){
           PORTB |= (1<<4); // pin 12 high
           OCR2A += pin12_high; // wait for the dutytime
           state = 1;
     }else{
           PORTB &= ~(1<<4); // pin 12 low
           OCR2A += pin12_low; // wait for the rest
           state = 0;
     }
}

// for comperator B // pin 13
ISR(TIMER2_COMPB_vect){
     static uint8_t state = 0;
     if(state==0){
           PORTB |= (1<<5); // pin 13 high
           OCR2B += pin13_high; // wait for the dutytime
           state = 1;
     }else{
           PORTB &= ~(1<<5); // pin 13 low
           OCR2B += pin13_low; // wait for the rest
           state = 0;
     }
}
  
[CUT]


i had to lower the frequency because soft PWM always works like fast PWM (dubble speed)

Code: Select all

void init_arduIMU_PWM(){
  // pin 12 & 13 must be set as output but thay are if you changed the Motors order
  
  // lower timer 2's frequency (prescaler to 128)
  TCCR2B = TCCR2B & 0b11111000 | 0x05;


you also need to disable pin 13 as status LED
in def.h

Yes, I already did that in def.h:

Code: Select all

#if defined(ARDUIMUv2)
  #define LEDPIN_PINMODE             pinMode (6, OUTPUT);
  #define LEDPIN_TOGGLE              PIND ^= B01000000;     //switch Blue LEDPIN state (digital PIN 6)
  #define LEDPIN_OFF                 PORTD &= B01000000;
  #define LEDPIN_ON                  PORTD |= B01000000;
  #define BUZZERPIN_PINMODE          pinMode (5, OUTPUT); // Use Red LED instead of buzzer
  #define BUZZERPIN_ON               PORTD |= B00100000;
  #define BUZZERPIN_OFF              PORTD &= B00100000;
  #define POWERPIN_PINMODE           ; // No need to turn on WM+/Nunchuck
  #define POWERPIN_ON                ;
  #define POWERPIN_OFF               ;
  #define STABLEPIN_PINMODE          pinMode (7, OUTPUT); // Yellow LED
  #define STABLEPIN_ON               PORTD |= B10000000;
  #define STABLEPIN_OFF              PORTD &= B10000000;
  #define HMC5843  // Optional I2C Magnetometer
  #define ADCACC
  #define ADCGYRO
  #define ACC_ORIENTATION(X, Y, Z)  {accADC[ROLL]  =  -Y; accADC[PITCH]  = X; accADC[YAW]  = Z;}
  #define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] =  X; gyroADC[PITCH] = Y; gyroADC[YAW] = -Z;}
  #define MAG_ORIENTATION(X, Y, Z)  {magADC[ROLL]  =  -X;  magADC[PITCH] =  -Y; magADC[YAW]  = -Z;}
#endif

Thanks again for your help. I see what you're getting to. As far as I understand it, you use Timer two and assign comparator A to pin 12 and comparator B to pin 13. But looking at the above, I seem to lose the connection of pin 11 to timer 2 channel A and pin 9 seems to remain connected to Timer 1, comparator A, even though I can't use it. Any idea on how to solve this? Would it be possible to use Timer 1 for pin 10 and 11, and Timer 2 for pin 12 and 13 (as you already suggested)?
All times are UTC+01:00
Page 1 of 1

Powered by phpBB® Forum Software © phpBB Limited