WebSVN - FlightCtrl - Diff - Rev 1796 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 <stdlib.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include "rc.h"
 #include "uart0.h"
 #include "controlMixer.h"
 #include "configuration.h"
 #include "commands.h"
 // The channel array is 1-based. The 0th entry is not used.
 volatile int16_t PPM_in[MAX_CHANNELS];
 volatile int16_t PPM_diff[MAX_CHANNELS];
 volatile uint8_t NewPpmData = 1;
 volatile int16_t RC_Quality = 0;
 int16_t RC_PRTY[4];
-uint8_t lastRCCommand  = COMMAND_NONE;
+uint8_t lastRCCommand = COMMAND_NONE;
 uint8_t commandTimer = 0;
 // Useless. Just trim on the R/C instead.
 // int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
 /***************************************************************
  *  16bit timer 1 is used to decode the PPM-Signal
  ***************************************************************/
-void RC_Init (void) {
+void RC_Init(void) {
-  uint8_t sreg = SREG;
+        uint8_t sreg = SREG;
-  // disable all interrupts before reconfiguration
+        // disable all interrupts before reconfiguration
-  cli();
+        cli();
-  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
+        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
-  DDRD &= ~(1<<DDD6);
+        DDRD &= ~(1 << DDD6);
-  PORTD |= (1<<PORTD6);
+        PORTD |= (1 << PORTD6);
-  // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
+        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
-  // set as output
+        // set as output
-  DDRD |= (1<<DDD5)| (1<<DDD4) | (1<<DDD3);
+        DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3);
-  // low level
+        // low level
-  PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3));
+        PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3));
-  // PD3 can't be used if 2nd UART is activated
+        // PD3 can't be used if 2nd UART is activated
-  // because TXD1 is at that port
+        // because TXD1 is at that port
-  if(CPUType != ATMEGA644P) {
+        if (CPUType != ATMEGA644P) {
-    DDRD |= (1<<PORTD3);
+                DDRD |= (1 << PORTD3);
-    PORTD &= ~(1<<PORTD3);
+                PORTD &= ~(1 << PORTD3);
-  }
+        }
-  // Timer/Counter1 Control Register A, B, C
+        // Timer/Counter1 Control Register A, B, C
-  // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
+        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
-  // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
+        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
-  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
+        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
-  // Enable input capture noise cancler (bit: ICNC1=1)
+        // Enable input capture noise cancler (bit: ICNC1=1)
-  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
+        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
-  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
+        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
+        // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
-  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
+        TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0)
-  TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
+                        | (1 << WGM11) | (1 << WGM10));
-  TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
+        TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
-  TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
+        TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
-  TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
+        TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
-  // Timer/Counter1 Interrupt Mask Register
+        // Timer/Counter1 Interrupt Mask Register
-  // Enable Input Capture Interrupt (bit: ICIE1=1)
+        // Enable Input Capture Interrupt (bit: ICIE1=1)
-  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
+        // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
-  // Enable Overflow Interrupt (bit: TOIE1=0)
+        // Enable Overflow Interrupt (bit: TOIE1=0)
-  TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
+        TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1));
-  TIMSK1 |= (1<<ICIE1);
+        TIMSK1 |= (1 << ICIE1);
-  RC_Quality = 0;
+        RC_Quality = 0;
-  SREG = sreg;
+        SREG = sreg;
+}
 /********************************************************************/
 /*         Every time a positive edge is detected at PD6            */
 /********************************************************************/
 /*                               t-Frame
-                                 <----------------------------------------------------------------------->
+ <----------------------------------------------------------------------->
-                                 ____   ______   _____   ________                ______    sync gap      ____
+ ____   ______   _____   ________                ______    sync gap      ____
-                                 |    | |      | |     | |        |              |      |                |
+ |    | |      | |     | |        |              |      |                |
-                                 |    | |      | |     | |        |              |      |                |
+ |    | |      | |     | |        |              |      |                |
-                              ___|    |_|      |_|     |_|        |_.............|      |________________|
+ ___|    |_|      |_|     |_|        |_.............|      |________________|
-                                 <-----><-------><------><-------->              <------>                <---
+ <-----><-------><------><-------->              <------>                <---
-                                 t0       t1      t2       t4                     tn                     t0
+ t0       t1      t2       t4                     tn                     t0
-                                 The PPM-Frame length is 22.5 ms.
+ The PPM-Frame length is 22.5 ms.
-                                 Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
+ Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
-                                 The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
+ The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
-                                 The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
+ The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
-                                 The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
+ The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
-                                 The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
+ The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
-                                 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
+ The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
-                                 the syncronization gap.
+ the syncronization gap.
-*/
+ */
+ISR(TIMER1_CAPT_vect)
-ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
+{ // typical rate of 1 ms to 2 ms
-  int16_t signal = 0, tmp;
+        int16_t signal = 0, tmp;
-  static int16_t index;
+        static int16_t index;
-  static uint16_t oldICR1 = 0;
+        static uint16_t oldICR1 = 0;
-  // 16bit Input Capture Register ICR1 contains the timer value TCNT1
+        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
-  // at the time the edge was detected
+        // at the time the edge was detected
-  // calculate the time delay to the previous event time which is stored in oldICR1
+        // calculate the time delay to the previous event time which is stored in oldICR1
-  // calculatiing the difference of the two uint16_t and converting the result to an int16_t
+        // calculatiing the difference of the two uint16_t and converting the result to an int16_t
-  // implicit handles a timer overflow 65535 -> 0 the right way.
+        // implicit handles a timer overflow 65535 -> 0 the right way.
-  signal = (uint16_t) ICR1 - oldICR1;
+        signal = (uint16_t) ICR1 - oldICR1;
-  oldICR1 = ICR1;
+        oldICR1 = ICR1;
-  //sync gap? (3.52 ms < signal < 25.6 ms)
+        //sync gap? (3.52 ms < signal < 25.6 ms)
-  if((signal > 1100) && (signal < 8000)) {
+        if ((signal > 1100) && (signal < 8000)) {
-    // if a sync gap happens and there where at least 4 channels decoded before
+                // if a sync gap happens and there where at least 4 channels decoded before
-    // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
+                // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
-    if(index >= 4) {
+                if (index >= 4) {
-      NewPpmData = 0;  // Null means NewData for the first 4 channels
+                        NewPpmData = 0; // Null means NewData for the first 4 channels
-    }
+                }
-    // synchronize channel index
+                // synchronize channel index
-    index = 1;
+                index = 1;
-  } else { // within the PPM frame
+        } else { // within the PPM frame
-    if(index < MAX_CHANNELS-1) { // PPM24 supports 12 channels
+                if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels
-      // check for valid signal length (0.8 ms < signal < 2.1984 ms)
+                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)
-      // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
+                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
-      if((signal > 250) && (signal < 687)) {
+                        if ((signal > 250) && (signal < 687)) {
-        // shift signal to zero symmetric range  -154 to 159
+                                // shift signal to zero symmetric range  -154 to 159
-        signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
+                                signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
-        // check for stable signal
+                                // check for stable signal
-        if(abs(signal - PPM_in[index]) < 6) {
+                                if (abs(signal - PPM_in[index]) < 6) {
-          if(RC_Quality < 200) RC_Quality +=10;
+                                        if (RC_Quality < 200)
+                                                RC_Quality += 10;
+                                        else
-          else RC_Quality = 200;
+                                                RC_Quality = 200;
-        }
+                                }
-        // If signal is the same as before +/- 1, just keep it there.
+                                // If signal is the same as before +/- 1, just keep it there.
-        if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) {
+                                if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
-          // In addition, if the signal is very close to 0, just set it to 0.
+                                        // In addition, if the signal is very close to 0, just set it to 0.
-          if (signal >=-1 && signal <= 1) {
+                                        if (signal >= -1 && signal <= 1) {
-            tmp = 0;
+                                                tmp = 0;
-          } else {
+                                        } else {
-            tmp = PPM_in[index];
+                                                tmp = PPM_in[index];
-          }
+                                        }
-        }
-        else
+                                } else
-          tmp = signal;
+                                        tmp = signal;
-        // calculate signal difference on good signal level
+                                // calculate signal difference on good signal level
-        if(RC_Quality >= 195)
+                                if (RC_Quality >= 195)
-          PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
+                                        PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
+                                else
-        else PPM_diff[index] = 0;
+                                        PPM_diff[index] = 0;
-        PPM_in[index] = tmp; // update channel value
+                                PPM_in[index] = tmp; // update channel value
-      }
+                        }
-      index++; // next channel
+                        index++; // next channel
-      // demux sum signal for channels 5 to 7 to J3, J4, J5
+                        // demux sum signal for channels 5 to 7 to J3, J4, J5
-      // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
+                        // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
-      // channels are usually available at the receiver anyway.
+                        // channels are usually available at the receiver anyway.
-      // if(index == 5) J3HIGH; else J3LOW;
+                        // if(index == 5) J3HIGH; else J3LOW;
-      // if(index == 6) J4HIGH; else J4LOW;
+                        // if(index == 6) J4HIGH; else J4LOW;
-      // if(CPUType != ATMEGA644P) // not used as TXD1
+                        // if(CPUType != ATMEGA644P) // not used as TXD1
-      //  {
+                        //  {
-      //    if(index == 7) J5HIGH; else J5LOW;
+                        //    if(index == 7) J5HIGH; else J5LOW;
-      //  }
+                        //  }
-    }
+                }
-  }
+        }
+}
 #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
 #define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
 #define COMMAND_THRESHOLD 85
 #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
 #define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 // Internal.
 uint8_t RC_getStickCommand(void) {
-  if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
+        if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
-    // vertical is up
+                // vertical is up
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
+                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
-      return COMMAND_GYROCAL;
+                        return COMMAND_GYROCAL;
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
+                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
-      return COMMAND_ACCCAL;
+                        return COMMAND_ACCCAL;
-    return COMMAND_NONE;
+                return COMMAND_NONE;
-  } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
+        } else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
-    // vertical is down
+                // vertical is down
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
+                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
-      return COMMAND_STOP;
+                        return COMMAND_STOP;
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
+                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
-      return COMMAND_START;
+                        return COMMAND_START;
-    return COMMAND_NONE;
+                return COMMAND_NONE;
-  }
+        }
-    // vertical is around center
+        // vertical is around center
-  return COMMAND_NONE;
+        return COMMAND_NONE;
+}
 /*
  * This must be called (as the only thing) for each control loop cycle (488 Hz).
  */
 void RC_update() {
-  int16_t tmp1, tmp2;
+        int16_t tmp1, tmp2;
-  if(RC_Quality) {
+        if (RC_Quality) {
-    RC_Quality--;
+                RC_Quality--;
-    if (NewPpmData-- == 0) {
+                if (NewPpmData-- == 0) {
-      RC_PRTY[CONTROL_PITCH]    = RCChannel(CH_PITCH) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
+                        RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP
+                                        + RCDiff(CH_PITCH) * staticParams.StickD;
-      RC_PRTY[CONTROL_ROLL]     = RCChannel(CH_ROLL)  * staticParams.StickP + RCDiff(CH_ROLL)  * staticParams.StickD;
+                        RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
+                                        + RCDiff(CH_ROLL) * staticParams.StickD;
-      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE)                    + RCDiff(CH_THROTTLE) * dynamicParams.UserParams[3] + 120;
+                        RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
+                                        * dynamicParams.UserParams[3] + 120;
+                        if (RC_PRTY[CONTROL_THROTTLE] < 0)
-      if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
+                                RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
-      tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
+                        tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
-      // exponential stick sensitivity in yawing rate
+                        // exponential stick sensitivity in yawing rate
-      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
+                        tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1))
+                                        / 512L; // expo  y = ax + bx^2
-      tmp2 += (staticParams.StickYawP * tmp1) / 4;
+                        tmp2 += (staticParams.StickYawP * tmp1) / 4;
-      RC_PRTY[CONTROL_YAW] = tmp2;
+                        RC_PRTY[CONTROL_YAW] = tmp2;
-    }
+                }
-    uint8_t command = RC_getStickCommand();
+                uint8_t command = RC_getStickCommand();
-    if (lastRCCommand == command) {
+                if (lastRCCommand == command) {
-      // Keep timer from overrunning.
+                        // Keep timer from overrunning.
-      if (commandTimer < COMMAND_TIMER) commandTimer++;
+                        if (commandTimer < COMMAND_TIMER)
+                                commandTimer++;
-    } else {
+                } else {
-      // There was a change.
+                        // There was a change.
-      lastRCCommand = command;
+                        lastRCCommand = command;
-      commandTimer = 0;
+                        commandTimer = 0;
-    }
+                }
-  } else { // Bad signal
+        } else { // Bad signal
-    RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
+                RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
+                                = RC_PRTY[CONTROL_YAW] = 0;
-  }
+        }
+}
 /*
  * Get Pitch, Roll, Throttle, Yaw values
  */
 int16_t* RC_getPRTY(void) {
-  return RC_PRTY;
+        return RC_PRTY;
+}
 /*
  * Get other channel value
  */
-int16_t RC_getVariable (uint8_t varNum) {
+int16_t RC_getVariable(uint8_t varNum) {
-  if (varNum < 4)
+        if (varNum < 4)
-    // 0th variable is 5th channel (1-based) etc.
+                // 0th variable is 5th channel (1-based) etc.
-    return RCChannel(varNum + 4) + POT_OFFSET;
+                return RCChannel(varNum + 4) + POT_OFFSET;
-  /*
+        /*
-   * Let's just say:
+         * Let's just say:
-   * The RC variable 4 is hardwired to channel 5
+         * The RC variable 4 is hardwired to channel 5
-   * The RC variable 5 is hardwired to channel 6
+         * The RC variable 5 is hardwired to channel 6
-   * The RC variable 6 is hardwired to channel 7
+         * The RC variable 6 is hardwired to channel 7
-   * The RC variable 7 is hardwired to channel 8
+         * The RC variable 7 is hardwired to channel 8
-   * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
+         * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
-   */
+         */
-  return PPM_in[varNum + 1] + POT_OFFSET;
+        return PPM_in[varNum + 1] + POT_OFFSET;
+}
 uint8_t RC_getSignalQuality(void) {
-  if (RC_Quality >= 160)
+        if (RC_Quality >= 160)
-    return SIGNAL_GOOD;
+                return SIGNAL_GOOD;
-  if (RC_Quality >= 140)
+        if (RC_Quality >= 140)
-    return SIGNAL_OK;
+                return SIGNAL_OK;
-  if (RC_Quality >= 120)
+        if (RC_Quality >= 120)
-    return SIGNAL_BAD;
+                return SIGNAL_BAD;
-  return SIGNAL_LOST;
+        return SIGNAL_LOST;
+}
 /*
  * To should fired only when the right stick is in the center position.
  * This will cause the value of pitch and roll stick to be adjusted
  * to zero (not just to near zero, as per the assumption in rc.c
  * about the rc signal. I had values about 50..70 with a Futaba
  * R617 receiver.) This calibration is not strictly necessary, but
  * for control logic that depends on the exact (non)center position
  * of a stick, it may be useful.
  */
 void RC_calibrate(void) {
-  // Do nothing.
+        // Do nothing.
+}
 /*
-  if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
+ if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
-    // In HH, it s OK to trim the R/C. The effect should not be conteracted here.
+ // In HH, it s OK to trim the R/C. The effect should not be conteracted here.
-    stickOffsetPitch = stickOffsetRoll = 0;
+ stickOffsetPitch = stickOffsetRoll = 0;
-  } else {
+ } else {
-    stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
+ stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
-    stickOffsetRoll = RCChannel(CH_ROLL)   * staticParams.StickP;
+ stickOffsetRoll = RCChannel(CH_ROLL)   * staticParams.StickP;
-  }
+ }
-}
+ }
-*/
+ */
 uint8_t RC_getCommand(void) {
-  if (commandTimer == COMMAND_TIMER) {
+        if (commandTimer == COMMAND_TIMER) {
-    // Stick has been held long enough; command committed.
+                // Stick has been held long enough; command committed.
-    return lastRCCommand;
+                return lastRCCommand;
-  }
+        }
-  // Not yet sure what the command is.
+        // Not yet sure what the command is.
-  return COMMAND_NONE;
+        return COMMAND_NONE;
+}
 /*
  * Command arguments on R/C:
  * 2--3--4
  * |     |  +
  * 1  0  5  ^ 0
  * |     |  |
  * 8--7--6
+ *
  * + <--
  *    0
+ *
  * Not in any of these positions: 0
  */
 #define ARGUMENT_THRESHOLD 70
 #define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
 #define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
 uint8_t RC_getArgument(void) {
-  if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
+        if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
-    // vertical is up
+                // vertical is up
-    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
+                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
-      return 2;
+                        return 2;
-    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
+                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
-      return 4;
+                        return 4;
-    return 3;
+                return 3;
-  } else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
+        } else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
-    // vertical is down
+                // vertical is down
-    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
+                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
-      return 8;
+                        return 8;
-    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
+                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
-      return 6;
+                        return 6;
-    return 7;
+                return 7;
-  } else {
+        } else {
-    // vertical is around center
+                // vertical is around center
-    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
+                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
-      return 1;
+                        return 1;
-    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
+                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
-      return 5;
+                        return 5;
-    return 0;
+                return 0;
-  }
+        }
+}
 uint8_t RC_getLooping(uint8_t looping) {
-  //  static uint8_t looping = 0;
+        //  static uint8_t looping = 0;
+        if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
+                        & CFG_LOOP_LEFT) {
+                looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
+        } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
+                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
+        }
+        if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
+                        && staticParams.BitConfig & CFG_LOOP_RIGHT) {
+                looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
+        } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
+                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
+        }
-  if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) {
+        if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
-    looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
-  } else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+                        && staticParams.BitConfig & CFG_LOOP_UP) {
-    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
-  }
-  if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) {
+                looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
-    looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
+        } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
-  } else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
+                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
-  }
+        }
-  if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) {
+        if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
-    looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
-  } else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+                        && staticParams.BitConfig & CFG_LOOP_DOWN) {
-    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
-  }
-  if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) {
+                looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
-    looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
+        } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
-  } else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
+                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
-  }
+        }
-  return looping;
+        return looping;
+}
 uint8_t RC_testCompassCalState(void) {
-  static uint8_t stick = 1;
+        static uint8_t stick = 1;
-  // if pitch is centered or top set stick to zero
+        // if pitch is centered or top set stick to zero
-  if(RCChannel(CH_PITCH) > -20) stick = 0;
+        if (RCChannel(CH_PITCH) > -20)
+                stick = 0;
-  // if pitch is down trigger to next cal state
+        // if pitch is down trigger to next cal state
-  if((RCChannel(CH_PITCH) < -70) && !stick) {
+        if ((RCChannel(CH_PITCH) < -70) && !stick) {
-    stick = 1;
+                stick = 1;
-    return 1;
+                return 1;
-  }
+        }
-  return 0;
+        return 0;
+}
 /*
  * Abstract controls are not used at the moment.
  t_control rc_control = {
  RC_getPitch,
  RC_getRoll,
  RC_getYaw,
  RC_getThrottle,
  RC_getSignalQuality,
  RC_calibrate
  };
-*/
+ */

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c @ 1991 - Rev 1796 → 1821