WebSVN - FlightCtrl - Diff - Rev 1869 and 1872 - /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 example: 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 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) {
-        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
  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.
  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 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 remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
  the syncronization gap.
  */
 ISR(TIMER1_CAPT_vect)
 { // 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)
+          if (RC_Quality < 200)
-                                                RC_Quality += 10;
+            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
+      RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP
-                                        + RCDiff(CH_PITCH) * staticParams.StickD;
+          + RCDiff(CH_PITCH) * staticParams.StickD;
-                        RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
+      RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
-                                        + RCDiff(CH_ROLL) * staticParams.StickD;
+          + RCDiff(CH_ROLL) * staticParams.StickD;
-                        RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
+      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
-                                        * dynamicParams.UserParams[3] + 120;
+          * 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))
+      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1))
-                                        / 512L; // expo  y = ax + bx^2
+          / 512L; // expo  y = ax + bx^2
-                        tmp2 += (staticParams.StickYawP * tmp1) / 4;
+      tmp2 += (staticParams.StickYawP * tmp1) >> 2;
-                        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)
+      if (commandTimer < COMMAND_TIMER)
-                                commandTimer++;
+        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_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
-                                = RC_PRTY[CONTROL_YAW] = 0;
+        = 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) {
-        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) {
  // In HH, it s OK to trim the R/C. The effect should not be conteracted here.
  stickOffsetPitch = stickOffsetRoll = 0;
  } else {
  stickOffsetPitch = RCChannel(CH_PITCH) * 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
+  if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
-                        & CFG_LOOP_LEFT) {
+      & CFG_LOOP_LEFT) {
-                looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
+    looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
-        } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
+  } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
-                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+      < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
+    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
-        }
+  }
-        if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
+  if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
-                        && staticParams.BitConfig & CFG_LOOP_RIGHT) {
+      && staticParams.BitConfig & CFG_LOOP_RIGHT) {
-                looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
+    looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
-        } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
+  } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
-                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+      > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
+    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
-        }
+  }
-        if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
+  if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
-                        && staticParams.BitConfig & CFG_LOOP_UP) {
+      && staticParams.BitConfig & CFG_LOOP_UP) {
-                looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
+    looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
-        } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
+  } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
-                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
+      < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
+    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
-        }
+  }
-        if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
+  if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
-                        && staticParams.BitConfig & CFG_LOOP_DOWN) {
+      && 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)
+  if (RCChannel(CH_PITCH) > -20)
-                stick = 0;
+    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 - Rev 1869 → 1872