WebSVN - FlightCtrl - Diff - Rev 1775 and 1821 - /branches/dongfang_FC

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Copyright (c) 04.2007 Holger Buss
 // + Nur für den privaten Gebrauch
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 // + Verkauf von Luftbildaufnahmen, usw.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 // + eindeutig als Ursprung verlinkt werden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 // + Benutzung auf eigene Gefahr
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 // + mit unserer Zustimmung zulässig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 // + this list of conditions and the following disclaimer.
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 // +     from this software without specific prior written permission.
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 // +     for non-commercial use (directly or indirectly)
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 // +     with our written permission
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 // +     clearly linked as origin
 // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include "eeprom.h"
 #include "uart0.h"
 #include "rc.h"
 #include "attitude.h"
-volatile int16_t        ServoPitchValue = 0;
+volatile int16_t ServoPitchValue = 0;
-volatile int16_t        ServoRollValue = 0;
+volatile int16_t ServoRollValue = 0;
-volatile uint8_t        ServoActive = 0;
+volatile uint8_t ServoActive = 0;
 #define HEF4017R_ON     PORTC |=  (1<<PORTC6)
 #define HEF4017R_OFF    PORTC &= ~(1<<PORTC6)
 /*****************************************************
  *              Initialize Timer 2
  *****************************************************/
 // The timer 2 is used to generate the PWM at PD7 (J7)
 // to control a camera servo for pitch compensation.
 void timer2_init(void) {
-  uint8_t sreg = SREG;
+        uint8_t sreg = SREG;
-  // disable all interrupts before reconfiguration
+        // disable all interrupts before reconfiguration
-  cli();
+        cli();
-  // set PD7 as output of the PWM for pitch servo
+        // set PD7 as output of the PWM for pitch servo
-  DDRD  |= (1<<DDD7);
+        DDRD |= (1 << DDD7);
-  PORTD &= ~(1<<PORTD7);        // set PD7 to low
+        PORTD &= ~(1 << PORTD7); // set PD7 to low
-  DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
+        DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017)
-  //PORTC &= ~(1<<PORTC6);      // set PC6 to low
+        //PORTC &= ~(1<<PORTC6);        // set PC6 to low
-  HEF4017R_ON; // enable reset
+        HEF4017R_ON; // enable reset
-  // Timer/Counter 2 Control Register A
+        // Timer/Counter 2 Control Register A
-  // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
+        // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
-  // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
+        // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
-  // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
+        // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
-  TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
+        TCCR2A &= ~((1 << COM2A1) | (1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0));
-  TCCR2A |= (1<<WGM21)|(1<<WGM20);
+        TCCR2A |= (1 << WGM21) | (1 << WGM20);
-  // Timer/Counter 2 Control Register B
+        // Timer/Counter 2 Control Register B
-  // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
+        // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
-  // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
+        // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
-  // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
+        // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
-  // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
+        // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
-  TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22));
+        TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS22));
-  TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22);
+        TCCR2B |= (1 << CS21) | (1 << CS20) | (1 << WGM22);
-  // Initialize the Timer/Counter 2 Register
+        // Initialize the Timer/Counter 2 Register
-  TCNT2 = 0;
+        TCNT2 = 0;
-  // Initialize the Output Compare Register A used for PWM generation on port PD7.
+        // Initialize the Output Compare Register A used for PWM generation on port PD7.
-  OCR2A = 255;
+        OCR2A = 255;
-  TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
+        TCCR2A |= (1 << COM2A1); // set or clear at compare match depends on value of COM2A0
-  // Timer/Counter 2 Interrupt Mask Register
+        // Timer/Counter 2 Interrupt Mask Register
-  // Enable timer output compare match A Interrupt only
+        // Enable timer output compare match A Interrupt only
-  TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
+        TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
-  TIMSK2 |= (1<<OCIE2A);
+        TIMSK2 |= (1 << OCIE2A);
-  SREG = sreg;
+        SREG = sreg;
+}
 void Servo_On(void) {
-  ServoActive = 1;
+        ServoActive = 1;
+}
 void Servo_Off(void) {
-  ServoActive = 0;
+        ServoActive = 0;
-  HEF4017R_ON; // enable reset
+        HEF4017R_ON; // enable reset
+}
 /*****************************************************
  * Control Servo Position
  *****************************************************/
-ISR(TIMER2_COMPA_vect) {
+ISR(TIMER2_COMPA_vect)
+{
-  // frame len 22.5 ms = 14063 * 1.6 us
+        // frame len 22.5 ms = 14063 * 1.6 us
-  // stop pulse: 0.3 ms = 188 * 1.6 us
+        // stop pulse: 0.3 ms = 188 * 1.6 us
-  // min servo pulse: 0.6 ms =  375 * 1.6 us
+        // min servo pulse: 0.6 ms =  375 * 1.6 us
-  // max servo pulse: 2.4 ms = 1500 * 1.6 us
+        // max servo pulse: 2.4 ms = 1500 * 1.6 us
-  // resolution: 1500 - 375 = 1125 steps
+        // resolution: 1500 - 375 = 1125 steps
 #define PPM_STOPPULSE 188
 #define PPM_FRAMELEN (1757 * .ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel
 #define MINSERVOPULSE 375
 #define MAXSERVOPULSE 1500
 #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
 #if defined(USE_NON_4017_SERVO_OUTPUTS) || defined(USE_4017_SERVO_OUTPUTS)
-  static uint8_t  isGeneratingPulse = 0;
+        static uint8_t isGeneratingPulse = 0;
-  static uint16_t remainingPulseLength = 0;
+        static uint16_t remainingPulseLength = 0;
-  static uint16_t ServoFrameTime = 0;
+        static uint16_t ServoFrameTime = 0;
-  static uint8_t  ServoIndex = 0;
+        static uint8_t ServoIndex = 0;
 #define MULTIPLIER 4
-  static int16_t ServoPitchOffset = (255 / 2) * MULTIPLIER; // initial value near center position
+        static int16_t ServoPitchOffset = (255 / 2) * MULTIPLIER; // initial value near center position
-  static int16_t ServoRollOffset = (255 / 2) * MULTIPLIER; // initial value near center position
+        static int16_t ServoRollOffset = (255 / 2) * MULTIPLIER; // initial value near center position
 #endif
 #ifdef USE_NON_4017_SERVO_OUTPUTS
-  //---------------------------
+        //---------------------------
-  // Pitch servo state machine
+        // Pitch servo state machine
-  //---------------------------
+        //---------------------------
-  if (!isGeneratingPulse) {    // pulse output complete on _next_ interrupt
+        if (!isGeneratingPulse) { // pulse output complete on _next_ interrupt
-    if(TCCR2A & (1<<COM2A0)) { // we are still outputting a high pulse
+                if(TCCR2A & (1<<COM2A0)) { // we are still outputting a high pulse
-      TCCR2A &= ~(1<<COM2A0);  // make a low pulse on _next_ interrupt, and now
+                        TCCR2A &= ~(1<<COM2A0); // make a low pulse on _next_ interrupt, and now
-      remainingPulseLength = MINSERVOPULSE + SERVORANGE / 2; // center position ~ 1.5ms
+                        remainingPulseLength = MINSERVOPULSE + SERVORANGE / 2; // center position ~ 1.5ms
-      ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl) / 4; // lowpass offset
+                        ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl) / 4; // lowpass offset
-      if(staticParams.ServoPitchCompInvert & 0x01) {
+                        if(staticParams.ServoPitchCompInvert & 0x01) {
-        // inverting movement of servo
+                                // inverting movement of servo
-        // todo: function.
+                                // todo: function.
-        ServoPitchValue = ServoPitchOffset + (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L));
+                                ServoPitchValue = ServoPitchOffset + (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L));
-      } else {
+                        } else {
-        // todo: function.
+                                // todo: function.
-        // non inverting movement of servo
+                                // non inverting movement of servo
-        ServoPitchValue = ServoPitchOffset - (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L));
+                                ServoPitchValue = ServoPitchOffset - (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L));
-      }
+                        }
-      // limit servo value to its parameter range definition
+                        // limit servo value to its parameter range definition
-      if(ServoPitchValue < (int16_t)staticParams.ServoPitchMin) {
+                        if(ServoPitchValue < (int16_t)staticParams.ServoPitchMin) {
-        ServoPitchValue = (int16_t)staticParams.ServoPitchMin;
+                                ServoPitchValue = (int16_t)staticParams.ServoPitchMin;
-      } else if(ServoPitchValue > (int16_t)staticParams.ServoPitchMax) {
+                        } else if(ServoPitchValue > (int16_t)staticParams.ServoPitchMax) {
-        ServoPitchValue = (int16_t)staticParams.ServoPitchMax;
+                                ServoPitchValue = (int16_t)staticParams.ServoPitchMax;
-      }
+                        }
-      remainingPulseLength = (ServoPitchValue - 256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position
+                        remainingPulseLength = (ServoPitchValue - 256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position
-      // range servo pulse width
+                        // range servo pulse width
-      if(remainingPulseLength > MAXSERVOPULSE ) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit
+                        if(remainingPulseLength > MAXSERVOPULSE ) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit
-      else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit
+                        else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit
-      // accumulate time for correct update rate
+                        // accumulate time for correct update rate
-      ServoFrameTime = remainingPulseLength;
+                        ServoFrameTime = remainingPulseLength;
-    } else { // we had a high pulse
+                } else { // we had a high pulse
-      TCCR2A |= (1<<COM2A0); // make a low pulse
+                        TCCR2A |= (1<<COM2A0); // make a low pulse
-      remainingPulseLength = PPM_FRAMELEN - ServoFrameTime;
+                        remainingPulseLength = PPM_FRAMELEN - ServoFrameTime;
-    }
+                }
-    // set pulse output active
+                // set pulse output active
-    isGeneratingPulse = 1;
+                isGeneratingPulse = 1;
-  } // EOF Pitch servo state machine
+        } // EOF Pitch servo state machine
 #elseif defined(USE_4017_SERVOS)
-  //-----------------------------------------------------
+        //-----------------------------------------------------
-  // PPM state machine, onboard demultiplexed by HEF4017
+        // PPM state machine, onboard demultiplexed by HEF4017
-  //-----------------------------------------------------
+        //-----------------------------------------------------
-  if(!isGeneratingPulse) { // pulse output complete
+        if(!isGeneratingPulse) { // pulse output complete
-    if(TCCR2A & (1<<COM2A0)) { // we had a low pulse
+                if(TCCR2A & (1<<COM2A0)) { // we had a low pulse
-    TCCR2A &= ~(1<<COM2A0);// make a high pulse
+                        TCCR2A &= ~(1<<COM2A0);// make a high pulse
-    if(ServoIndex == 0) { // if we are at the sync gap
+                        if(ServoIndex == 0) { // if we are at the sync gap
-      remainingPulseLength = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
+                                remainingPulseLength = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
-      ServoFrameTime = 0; // reset servo frame time
+                                ServoFrameTime = 0; // reset servo frame time
-      HEF4017R_ON; // enable HEF4017 reset
+                                HEF4017R_ON; // enable HEF4017 reset
-    } else { // servo channels
+                        } else { // servo channels
-      remainingPulseLength  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
+                                remainingPulseLength = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
-      switch(ServoIndex) { // map servo channels
+                                switch(ServoIndex) { // map servo channels
-      case 1: // Pitch Compensation Servo
+                                        case 1: // Pitch Compensation Servo
-        ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl * MULTIPLIER) / 4; // lowpass offset
+                                        ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl * MULTIPLIER) / 4; // lowpass offset
-        ServoPitchValue = ServoPitchOffset; // offset (Range from 0 to 255 * 3 = 765)
+                                        ServoPitchValue = ServoPitchOffset; // offset (Range from 0 to 255 * 3 = 765)
-        if(staticParams.ServoPitchCompInvert & 0x01) {
+                                        if(staticParams.ServoPitchCompInvert & 0x01) {
-          // inverting movement of servo
+                                                // inverting movement of servo
-          ServoPitchValue += (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) );
+                                                ServoPitchValue += (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) );
-        } else {        // non inverting movement of servo
+                                        } else { // non inverting movement of servo
-          ServoPitchValue -= (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) );
+                                                ServoPitchValue -= (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) );
-        }
+                                        }
-        // limit servo value to its parameter range definition
+                                        // limit servo value to its parameter range definition
-        if(ServoPitchValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) {
+                                        if(ServoPitchValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) {
-          ServoPitchValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER;
+                                                ServoPitchValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER;
-        } else if(ServoPitchValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) {
+                                        } else if(ServoPitchValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) {
+                                                ServoPitchValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER;
+                                        }
+                                        remainingPulseLength += ServoPitchValue - (256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position
+                                        ServoPitchValue /= MULTIPLIER;
+                                        break;
+                                        case 2: // Roll Compensation Servo
+                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t)80 * MULTIPLIER) / 4; // lowpass offset
+                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
+                                        //if(staticParams.ServoRollCompInvert & 0x01)
+                                        { // inverting movement of servo
+                                                ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLIER * (integralGyroRoll / 128L ) ) / (256L) );
+                                        }
+                                        /*                                                      else
+                                         {      // non inverting movement of servo
+                                         ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLIER * (IntegralGyroRoll / 128L ) ) / (256L) );
+                                         }
+                                         */// limit servo value to its parameter range definition
+                                        if(ServoRollValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) {
+                                                ServoRollValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER;
+                                        } else if(ServoRollValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) {
+                                                ServoRollValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER;
+                                        }
+                                        remainingPulseLength += ServoRollValue - (256 / 2) * MULTIPLIER; // shift ServoRollValue to center position
+                                        ServoRollValue /= MULTIPLIER;
+                                        break;
+                                        default: // other servo channels
+                                        remainingPulseLength += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
+                                        break;
+                                }
+                                // range servo pulse width
+                                if(remainingPulseLength > MAXSERVOPULSE) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit
+                                else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit
+                                // substract stop pulse width
+                                remainingPulseLength -= PPM_STOPPULSE;
+                                // accumulate time for correct sync gap
+                                ServoFrameTime += remainingPulseLength;
+                        }
+                } else { // we had a high pulse
+                        TCCR2A |= (1<<COM2A0); // make a low pulse
+                        // set pulsewidth to stop pulse width
+                        remainingPulseLength = PPM_STOPPULSE;
+                        // accumulate time for correct sync gap
+                        ServoFrameTime += remainingPulseLength;
+                        if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset
+                        ServoIndex++; // change to next servo channel
+                        if(ServoIndex > staticParams.ServoRefresh) ServoIndex = 0; // reset to the sync gap
+                }
+                // set pulse output active
-          ServoPitchValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER;
+                isGeneratingPulse = 1;
-        }
-        remainingPulseLength += ServoPitchValue - (256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position
-        ServoPitchValue /= MULTIPLIER;
-        break;
-      case 2: // Roll Compensation Servo
-        ServoRollOffset = (ServoRollOffset * 3 + (int16_t)80 * MULTIPLIER) / 4; // lowpass offset
-        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
-        //if(staticParams.ServoRollCompInvert & 0x01)
-        {       // inverting movement of servo
-          ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLIER * (integralGyroRoll / 128L ) ) / (256L) );
-        }
-        /*                                                      else
-                                                                {       // non inverting movement of servo
-                                                                ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLIER * (IntegralGyroRoll / 128L ) ) / (256L) );
-                                                                }
-        */                                                      // limit servo value to its parameter range definition
-        if(ServoRollValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) {
-          ServoRollValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER;
-        } else if(ServoRollValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) {
-          ServoRollValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER;
-        }
-        remainingPulseLength += ServoRollValue - (256 / 2) * MULTIPLIER; // shift ServoRollValue to center position
-        ServoRollValue /= MULTIPLIER;
-        break;
-      default: // other servo channels
-        remainingPulseLength += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
-        break;
-      }
-      // range servo pulse width
-      if(remainingPulseLength > MAXSERVOPULSE) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit
-      else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit
-      // substract stop pulse width
-      remainingPulseLength -= PPM_STOPPULSE;
-      // accumulate time for correct sync gap
-      ServoFrameTime += remainingPulseLength;
-    }
-  } else { // we had a high pulse
-    TCCR2A |= (1<<COM2A0); // make a low pulse
-    // set pulsewidth to stop pulse width
-    remainingPulseLength = PPM_STOPPULSE;
-    // accumulate time for correct sync gap
-    ServoFrameTime += remainingPulseLength;
-    if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset
-    ServoIndex++; // change to next servo channel
-    if(ServoIndex > staticParams.ServoRefresh) ServoIndex = 0; // reset to the sync gap
-  }
-  // set pulse output active
-  isGeneratingPulse = 1;
+        }
 #endif
-/*
+        /*
- * Cases:
+         * Cases:
- * 1) 255 + 128 <= remainingPulseLength --> delta = 255
+         * 1) 255 + 128 <= remainingPulseLength --> delta = 255
- * 2) 255 <= remainingPulseLength < 255 + 128 --> delta = 255 - 128
+         * 2) 255 <= remainingPulseLength < 255 + 128 --> delta = 255 - 128
- *    this is to avoid a too short delta on the last cycle, which would cause
+         *    this is to avoid a too short delta on the last cycle, which would cause
- *    an interupt-on-interrupt condition and the loss of the last interrupt.
+         *    an interupt-on-interrupt condition and the loss of the last interrupt.
- * 3) remainingPulseLength < 255 --> delta = remainingPulseLength
+         * 3) remainingPulseLength < 255 --> delta = remainingPulseLength
- */
+         */
 #if defined(USE_NON_4017_SERVO_OUTPUTS) || defined(USE_4017_SERVO_OUTPUTS)
-  uint8_t delta;
+        uint8_t delta;
-  if (remainingPulseLength >= (255 + 128)) {
+        if (remainingPulseLength >= (255 + 128)) {
-    delta = 255;
+                delta = 255;
-  } else if (remainingPulseLength >= 255) {
+        } else if (remainingPulseLength >= 255) {
-    delta = 255- 128;
+                delta = 255- 128;
-  } else {
+        } else {
-    delta = remainingPulseLength;
+                delta = remainingPulseLength;
-    isGeneratingPulse = 0; // trigger to stop pulse
+                isGeneratingPulse = 0; // trigger to stop pulse
-  }
+        }
-  OCR2A = delta;
+        OCR2A = delta;
-  remainingPulseLength -= delta;
+        remainingPulseLength -= delta;
 #endif
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/timer2.c @ 2097 - Rev 1775 → 1821