WebSVN - FlightCtrl - Diff - Rev 1775 and 1796 - /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 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;
 
  // disable all interrupts before reconfiguration
  cli();
 
  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
  DDRD &= ~(1<<DDD6);
  PORTD |= (1<<PORTD6);
 
  // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
  // set as output
  DDRD |= (1<<DDD5)| (1<<DDD4) | (1<<DDD3);
  // low level
  PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3));
 
  // PD3 can't be used if 2nd UART is activated
  // because TXD1 is at that port
  if(CPUType != ATMEGA644P) {
    DDRD |= (1<<PORTD3);
    PORTD &= ~(1<<PORTD3);
  }
 
  // Timer/Counter1 Control Register A, B, C
 
  // 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)
  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
  // Enable input capture noise cancler (bit: ICNC1=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
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
  TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
  TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
  TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
  TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
 
  // Timer/Counter1 Interrupt Mask Register
 
  // Enable Input Capture Interrupt (bit: ICIE1=1)
  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
  // Enable Overflow Interrupt (bit: TOIE1=0)
  TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
  TIMSK1 |= (1<<ICIE1);
 
  RC_Quality = 0;
 
  SREG = sreg;
}
 
/********************************************************************/
/*         Every time a positive edge is detected at PD6            */
/********************************************************************/
/*                               t-Frame
                                 <----------------------------------------------------------------------->
                                 ____   ______   _____   ________                ______    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;
  static int16_t index;
  static uint16_t oldICR1 = 0;
 
  // 16bit Input Capture Register ICR1 contains the timer value TCNT1
  // at the time the edge was detected
 
  // 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
  // implicit handles a timer overflow 65535 -> 0 the right way.
  signal = (uint16_t) ICR1 - oldICR1;
  oldICR1 = ICR1;
 
  //sync gap? (3.52 ms < signal < 25.6 ms)
  if((signal > 1100) && (signal < 8000)) {
    // 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.
    if(index >= 4) {
      NewPpmData = 0;  // Null means NewData for the first 4 channels
    }
    // synchronize channel index
    index = 1;
  } else { // within the PPM frame
    if(index < MAX_CHANNELS-1) { // PPM24 supports 12 channels
 
      // 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
      if((signal > 250) && (signal < 687)) {
        // shift signal to zero symmetric range  -154 to 159
        signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
        // check for stable signal
        if(abs(signal - PPM_in[index]) < 6) {
          if(RC_Quality < 200) RC_Quality +=10;
          else RC_Quality = 200;
        }
        // If signal is the same as before +/- 1, just keep it there.
        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.
          if (signal >=-1 && signal <= 1) {
            tmp = 0;
          } else {
            tmp = PPM_in[index];
 
 
 
 
 
 
 
 
 
          }
        }
        else
          tmp = signal;
        // calculate signal difference on good signal level
 
 
        if(RC_Quality >= 195)  
          PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
        else PPM_diff[index] = 0;
 
        PPM_in[index] = tmp; // update channel value
 
      }
      index++; // next channel
      // 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
      // channels are usually available at the receiver anyway.
      // if(index == 5) J3HIGH; else J3LOW;
      // if(index == 6) J4HIGH; else J4LOW;
      // if(CPUType != ATMEGA644P) // not used as TXD1
      //  {
      //    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) {
    // vertical is up
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
      return COMMAND_GYROCAL;
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
      return COMMAND_ACCCAL;
    return COMMAND_NONE;
  } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
    // vertical is down
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
      return COMMAND_STOP;
    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
      return COMMAND_START;
    return COMMAND_NONE;
  }
    // vertical is around center
  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;
  if(RC_Quality) {
    RC_Quality--;
    if (NewPpmData-- == 0) {
      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_THROTTLE] = RCChannel(CH_THROTTLE)                    + RCDiff(CH_THROTTLE) * dynamicParams.UserParams[3] + 120;
      if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
      tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
      // exponential stick sensitivity in yawing rate
      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
      tmp2 += (staticParams.StickYawP * tmp1) / 4;
      RC_PRTY[CONTROL_YAW] = tmp2;
    }
    uint8_t command = RC_getStickCommand();
    if (lastRCCommand == command) {
      // Keep timer from overrunning.
      if (commandTimer < COMMAND_TIMER) commandTimer++;
    } else {
      // There was a change.
      lastRCCommand = command;
      commandTimer = 0;
    }
  } else { // Bad signal
    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;
}
 
/*
 * Get other channel value
 */
int16_t RC_getVariable (uint8_t varNum) {
  if (varNum < 4)
    // 0th variable is 5th channel (1-based) etc.
    return RCChannel(varNum + 4) + POT_OFFSET;
  /*
   * Let's just say:
   * The RC variable 4 is hardwired to channel 5
   * The RC variable 5 is hardwired to channel 6
   * The RC variable 6 is hardwired to channel 7
   * The RC variable 7 is hardwired to channel 8
   * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
   */
  return PPM_in[varNum + 1] + POT_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
  if (RC_Quality >= 160)
    return SIGNAL_GOOD;
  if (RC_Quality >= 140)
    return SIGNAL_OK;
  if (RC_Quality >= 120)
    return SIGNAL_BAD;
  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.
}
 
/*
  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) {
    // Stick has been held long enough; command committed.
    return lastRCCommand;
  }
  // Not yet sure what the command is.
  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) {
    // vertical is up
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
      return 2;
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
      return 4;
    return 3;
  } else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
    // vertical is down
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
      return 8;
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
      return 6;
    return 7;
  } else {
    // vertical is around center
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
      return 1;
    if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
      return 5;
    return 0;
  }
}
 
uint8_t RC_getLooping(uint8_t looping) {
  //  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_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) {
    looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
  } else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
  }
 
  if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) {
    looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
  } else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
  }
 
  return looping;
}
 
uint8_t RC_testCompassCalState(void) {
  static uint8_t stick = 1;
  // if pitch is centered or top set stick to zero
  if(RCChannel(CH_PITCH) > -20) stick = 0;
  // if pitch is down trigger to next cal state
  if((RCChannel(CH_PITCH) < -70) && !stick) {
    stick = 1;
    return 1;
  }
  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
 };
*/
 

Rev 1775	Rev 1796
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + Nur für den privaten Gebrauch	3	// + Nur für den privaten Gebrauch
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	9	// + bzgl. der Nutzungsbedingungen aufzunehmen.
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11	// + Verkauf von Luftbildaufnahmen, usw.	11	// + Verkauf von Luftbildaufnahmen, usw.
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18	// + eindeutig als Ursprung verlinkt werden	18	// + eindeutig als Ursprung verlinkt werden
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21	// + Benutzung auf eigene Gefahr	21	// + Benutzung auf eigene Gefahr
22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25	// + mit unserer Zustimmung zulässig	25	// + mit unserer Zustimmung zulässig
26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30	// + this list of conditions and the following disclaimer.	30	// + this list of conditions and the following disclaimer.
31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32	// + from this software without specific prior written permission.	32	// + from this software without specific prior written permission.
33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34	// + for non-commercial use (directly or indirectly)	34	// + for non-commercial use (directly or indirectly)
35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36	// + with our written permission	36	// + with our written permission
37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38	// + clearly linked as origin	38	// + clearly linked as origin
39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed	39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49	// + POSSIBILITY OF SUCH DAMAGE.	49	// + POSSIBILITY OF SUCH DAMAGE.
50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51	#include <stdlib.h>	51	#include <stdlib.h>
52	#include <avr/io.h>	52	#include <avr/io.h>
53	#include <avr/interrupt.h>	53	#include <avr/interrupt.h>
54		54
55	#include "rc.h"	55	#include "rc.h"
56	#include "uart0.h"	56	#include "uart0.h"
57	#include "controlMixer.h"	57	#include "controlMixer.h"
58	#include "configuration.h"	58	#include "configuration.h"
59	#include "commands.h"	59	#include "commands.h"
60		60
61	// The channel array is 1-based. The 0th entry is not used.	61	// The channel array is 1-based. The 0th entry is not used.
62	volatile int16_t PPM_in[MAX_CHANNELS];	62	volatile int16_t PPM_in[MAX_CHANNELS];
63	volatile int16_t PPM_diff[MAX_CHANNELS];	63	volatile int16_t PPM_diff[MAX_CHANNELS];
64	volatile uint8_t NewPpmData = 1;	64	volatile uint8_t NewPpmData = 1;
65	volatile int16_t RC_Quality = 0;	65	volatile int16_t RC_Quality = 0;
66	int16_t RC_PRTY[4];	66	int16_t RC_PRTY[4];
67	uint8_t lastRCCommand = COMMAND_NONE;	67	uint8_t lastRCCommand = COMMAND_NONE;
68	uint8_t commandTimer = 0;	68	uint8_t commandTimer = 0;
69		69
70	// Useless. Just trim on the R/C instead.	70	// Useless. Just trim on the R/C instead.
71	// int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;	71	// int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
72		72
73	/***************************************************************	73	/***************************************************************
74	* 16bit timer 1 is used to decode the PPM-Signal	74	* 16bit timer 1 is used to decode the PPM-Signal
75	***************************************************************/	75	***************************************************************/
76	void RC_Init (void) {	76	void RC_Init (void) {
77	uint8_t sreg = SREG;	77	uint8_t sreg = SREG;
78		78
79	// disable all interrupts before reconfiguration	79	// disable all interrupts before reconfiguration
80	cli();	80	cli();
81		81
82	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1	82	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
83	DDRD &= ~(1<<DDD6);	83	DDRD &= ~(1<<DDD6);
84	PORTD \|= (1<<PORTD6);	84	PORTD \|= (1<<PORTD6);
85		85
86	// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)	86	// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
87	// set as output	87	// set as output
88	DDRD \|= (1<<DDD5)\| (1<<DDD4) \| (1<<DDD3);	88	DDRD \|= (1<<DDD5)\| (1<<DDD4) \| (1<<DDD3);
89	// low level	89	// low level
90	PORTD &= ~((1<<PORTD5) \| (1<<PORTD4) \| (1<<PORTD3));	90	PORTD &= ~((1<<PORTD5) \| (1<<PORTD4) \| (1<<PORTD3));
91		91
92	// PD3 can't be used if 2nd UART is activated	92	// PD3 can't be used if 2nd UART is activated
93	// because TXD1 is at that port	93	// because TXD1 is at that port
94	if(CPUType != ATMEGA644P) {	94	if(CPUType != ATMEGA644P) {
95	DDRD \|= (1<<PORTD3);	95	DDRD \|= (1<<PORTD3);
96	PORTD &= ~(1<<PORTD3);	96	PORTD &= ~(1<<PORTD3);
97	}	97	}
98		98
99	// Timer/Counter1 Control Register A, B, C	99	// Timer/Counter1 Control Register A, B, C
100		100
101	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)	101	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
102	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)	102	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
103	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)	103	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
104	// Enable input capture noise cancler (bit: ICNC1=1)	104	// Enable input capture noise cancler (bit: ICNC1=1)
105	// Trigger on positive edge of the input capture pin (bit: ICES1=1),	105	// Trigger on positive edge of the input capture pin (bit: ICES1=1),
106	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs	106	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
107	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.	107	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
108	TCCR1A &= ~((1<<COM1A1)\|(1<<COM1A0)\|(1<<COM1B1)\|(1<<COM1B0)\|(1<<WGM11)\|(1<<WGM10));	108	TCCR1A &= ~((1<<COM1A1)\|(1<<COM1A0)\|(1<<COM1B1)\|(1<<COM1B0)\|(1<<WGM11)\|(1<<WGM10));
109	TCCR1B &= ~((1<<WGM13)\|(1<<WGM12)\|(1<<CS12));	109	TCCR1B &= ~((1<<WGM13)\|(1<<WGM12)\|(1<<CS12));
110	TCCR1B \|= (1<<CS11)\|(1<<CS10)\|(1<<ICES1)\|(1<<ICNC1);	110	TCCR1B \|= (1<<CS11)\|(1<<CS10)\|(1<<ICES1)\|(1<<ICNC1);
111	TCCR1C &= ~((1<<FOC1A)\|(1<<FOC1B));	111	TCCR1C &= ~((1<<FOC1A)\|(1<<FOC1B));
112		112
113	// Timer/Counter1 Interrupt Mask Register	113	// Timer/Counter1 Interrupt Mask Register
114		114
115	// Enable Input Capture Interrupt (bit: ICIE1=1)	115	// Enable Input Capture Interrupt (bit: ICIE1=1)
116	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)	116	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
117	// Enable Overflow Interrupt (bit: TOIE1=0)	117	// Enable Overflow Interrupt (bit: TOIE1=0)
118	TIMSK1 &= ~((1<<OCIE1B)\|(1<<OCIE1A)\|(1<<TOIE1));	118	TIMSK1 &= ~((1<<OCIE1B)\|(1<<OCIE1A)\|(1<<TOIE1));
119	TIMSK1 \|= (1<<ICIE1);	119	TIMSK1 \|= (1<<ICIE1);
120		120
121	RC_Quality = 0;	121	RC_Quality = 0;
122		122
123	SREG = sreg;	123	SREG = sreg;
124	}	124	}
125		125
126	/********************************************************************/	126	/********************************************************************/
127	/* Every time a positive edge is detected at PD6 */	127	/* Every time a positive edge is detected at PD6 */
128	/********************************************************************/	128	/********************************************************************/
129	/* t-Frame	129	/* t-Frame
130	<----------------------------------------------------------------------->	130	<----------------------------------------------------------------------->
131	____ ______ _____ ________ ______ sync gap ____	131	____ ______ _____ ________ ______ sync gap ____
132	\| \| \| \| \| \| \| \| \| \| \|	132	\| \| \| \| \| \| \| \| \| \| \|
133	\| \| \| \| \| \| \| \| \| \| \|	133	\| \| \| \| \| \| \| \| \| \| \|
134	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	134	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
135	<-----><-------><------><--------> <------> <---	135	<-----><-------><------><--------> <------> <---
136	t0 t1 t2 t4 tn t0	136	t0 t1 t2 t4 tn t0
137		137
138	The PPM-Frame length is 22.5 ms.	138	The PPM-Frame length is 22.5 ms.
139	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.	139	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
140	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.	140	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
141	The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.	141	The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
142	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.	142	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
143	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.	143	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
144	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is	144	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
145	the syncronization gap.	145	the syncronization gap.
146	*/	146	*/
147	ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms	147	ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
148	int16_t signal = 0, tmp;	148	int16_t signal = 0, tmp;
149	static int16_t index;	149	static int16_t index;
150	static uint16_t oldICR1 = 0;	150	static uint16_t oldICR1 = 0;
151		151
152	// 16bit Input Capture Register ICR1 contains the timer value TCNT1	152	// 16bit Input Capture Register ICR1 contains the timer value TCNT1
153	// at the time the edge was detected	153	// at the time the edge was detected
154		154
155	// calculate the time delay to the previous event time which is stored in oldICR1	155	// calculate the time delay to the previous event time which is stored in oldICR1
156	// calculatiing the difference of the two uint16_t and converting the result to an int16_t	156	// calculatiing the difference of the two uint16_t and converting the result to an int16_t
157	// implicit handles a timer overflow 65535 -> 0 the right way.	157	// implicit handles a timer overflow 65535 -> 0 the right way.
158	signal = (uint16_t) ICR1 - oldICR1;	158	signal = (uint16_t) ICR1 - oldICR1;
159	oldICR1 = ICR1;	159	oldICR1 = ICR1;
160		160
161	//sync gap? (3.52 ms < signal < 25.6 ms)	161	//sync gap? (3.52 ms < signal < 25.6 ms)
162	if((signal > 1100) && (signal < 8000)) {	162	if((signal > 1100) && (signal < 8000)) {
163	// if a sync gap happens and there where at least 4 channels decoded before	163	// if a sync gap happens and there where at least 4 channels decoded before
164	// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.	164	// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
165	if(index >= 4) {	165	if(index >= 4) {
166	NewPpmData = 0; // Null means NewData for the first 4 channels	166	NewPpmData = 0; // Null means NewData for the first 4 channels
167	}	167	}
168	// synchronize channel index	168	// synchronize channel index
169	index = 1;	169	index = 1;
170	} else { // within the PPM frame	170	} else { // within the PPM frame
171	if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels	171	if(index < MAX_CHANNELS-1) { // PPM24 supports 12 channels
172	{	-
173	// check for valid signal length (0.8 ms < signal < 2.1984 ms)	172	// check for valid signal length (0.8 ms < signal < 2.1984 ms)
174	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625	173	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
175	if((signal > 250) && (signal < 687)) {	174	if((signal > 250) && (signal < 687)) {
176	// shift signal to zero symmetric range -154 to 159	175	// shift signal to zero symmetric range -154 to 159
177	signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)	176	signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
178	// check for stable signal	177	// check for stable signal
179	if(abs(signal - PPM_in[index]) < 6) {	178	if(abs(signal - PPM_in[index]) < 6) {
180	if(RC_Quality < 200) RC_Quality +=10;	179	if(RC_Quality < 200) RC_Quality +=10;
181	else RC_Quality = 200;	180	else RC_Quality = 200;
182	}	181	}
183	// If signal is the same as before +/- 1, just keep it there.	182	// If signal is the same as before +/- 1, just keep it there.
184	if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) {	183	if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) {
185	// In addition, if the signal is very close to 0, just set it to 0.	184	// In addition, if the signal is very close to 0, just set it to 0.
186	if (signal >=-1 && signal <= 1) {	185	if (signal >=-1 && signal <= 1) {
187	tmp = 0;	186	tmp = 0;
188	} else {	187	} else {
189	tmp = PPM_in[index];	188	tmp = PPM_in[index];
190	}	-
191	}	-
192	else	-
193	tmp = signal;	-
194	// calculate signal difference on good signal level	-
195	if(RC_Quality >= 195)	-
196	PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction	-
197	else PPM_diff[index] = 0;	-
198	PPM_in[index] = tmp; // update channel value	-
199	}	189	}
-		190	}
-		191	else
200	index++; // next channel	192	tmp = signal;
201	// demux sum signal for channels 5 to 7 to J3, J4, J5	193	// calculate signal difference on good signal level
202	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the	-
203	// channels are usually available at the receiver anyway.	-
204	// if(index == 5) J3HIGH; else J3LOW;	194	if(RC_Quality >= 195)
205	// if(index == 6) J4HIGH; else J4LOW;	195	PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
206	// if(CPUType != ATMEGA644P) // not used as TXD1	196	else PPM_diff[index] = 0;
207	// {	-
208	// if(index == 7) J5HIGH; else J5LOW;	197	PPM_in[index] = tmp; // update channel value
209	// }	-
210	}	198	}
-		199	index++; // next channel
-		200	// demux sum signal for channels 5 to 7 to J3, J4, J5
-		201	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
-		202	// channels are usually available at the receiver anyway.
-		203	// if(index == 5) J3HIGH; else J3LOW;
-		204	// if(index == 6) J4HIGH; else J4LOW;
-		205	// if(CPUType != ATMEGA644P) // not used as TXD1
-		206	// {
-		207	// if(index == 7) J5HIGH; else J5LOW;
-		208	// }
-		209	}
211	}	210	}
212	}	211	}
213		212
214	#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]	213	#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
215	#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]	214	#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
216	#define COMMAND_THRESHOLD 85	215	#define COMMAND_THRESHOLD 85
217	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE	216	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
218	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW	217	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
219		218
220	// Internal.	219	// Internal.
221	uint8_t RC_getStickCommand(void) {	220	uint8_t RC_getStickCommand(void) {
222	if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {	221	if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
223	// vertical is up	222	// vertical is up
224	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)	223	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
225	return COMMAND_GYROCAL;	224	return COMMAND_GYROCAL;
226	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)	225	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
227	return COMMAND_ACCCAL;	226	return COMMAND_ACCCAL;
228	return COMMAND_NONE;	227	return COMMAND_NONE;
229	} else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {	228	} else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
230	// vertical is down	229	// vertical is down
231	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)	230	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
232	return COMMAND_STOP;	231	return COMMAND_STOP;
233	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)	232	if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
234	return COMMAND_START;	233	return COMMAND_START;
235	return COMMAND_NONE;	234	return COMMAND_NONE;
236	}	235	}
237	// vertical is around center	236	// vertical is around center
238	return COMMAND_NONE;	237	return COMMAND_NONE;
239	}	238	}
240		239
241	/*	240	/*
242	* This must be called (as the only thing) for each control loop cycle (488 Hz).	241	* This must be called (as the only thing) for each control loop cycle (488 Hz).
243	*/	242	*/
244	void RC_update() {	243	void RC_update() {
245	int16_t tmp1, tmp2;	244	int16_t tmp1, tmp2;
246	if(RC_Quality) {	245	if(RC_Quality) {
247	RC_Quality--;	246	RC_Quality--;
248	if (NewPpmData-- == 0) {	247	if (NewPpmData-- == 0) {
249	RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch / + RCDiff(CH_PITCH) staticParams.StickD;	248	RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
250	RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll / + RCDiff(CH_ROLL) staticParams.StickD;	249	RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD;
251	RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120;	250	RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * dynamicParams.UserParams[3] + 120;
252	if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.	251	if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
253	tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);	252	tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
254	// exponential stick sensitivity in yawing rate	253	// exponential stick sensitivity in yawing rate
255	tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2	254	tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2
256	tmp2 += (staticParams.StickYawP * tmp1) / 4;	255	tmp2 += (staticParams.StickYawP * tmp1) / 4;
257	RC_PRTY[CONTROL_YAW] = tmp2;	256	RC_PRTY[CONTROL_YAW] = tmp2;
258	}	257	}
259	uint8_t command = RC_getStickCommand();	258	uint8_t command = RC_getStickCommand();
260	if (lastRCCommand == command) {	259	if (lastRCCommand == command) {
261	// Keep timer from overrunning.	260	// Keep timer from overrunning.
262	if (commandTimer < COMMAND_TIMER) commandTimer++;	261	if (commandTimer < COMMAND_TIMER) commandTimer++;
263	} else {	262	} else {
264	// There was a change.	263	// There was a change.
265	lastRCCommand = command;	264	lastRCCommand = command;
266	commandTimer = 0;	265	commandTimer = 0;
267	}	266	}
268	} else { // Bad signal	267	} else { // Bad signal
269	RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;	268	RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
270	}	269	}
271	}	270	}
272		271
273	/*	272	/*
274	* Get Pitch, Roll, Throttle, Yaw values	273	* Get Pitch, Roll, Throttle, Yaw values
275	*/	274	*/
276	int16_t* RC_getPRTY(void) {	275	int16_t* RC_getPRTY(void) {
277	return RC_PRTY;	276	return RC_PRTY;
278	}	277	}
279		278
280	/*	279	/*
281	* Get other channel value	280	* Get other channel value
282	*/	281	*/
283	int16_t RC_getVariable (uint8_t varNum) {	282	int16_t RC_getVariable (uint8_t varNum) {
284	if (varNum < 4)	283	if (varNum < 4)
285	// 0th variable is 5th channel (1-based) etc.	284	// 0th variable is 5th channel (1-based) etc.
286	return RCChannel(varNum + 4) + POT_OFFSET;	285	return RCChannel(varNum + 4) + POT_OFFSET;
287	/*	286	/*
288	* Let's just say:	287	* Let's just say:
289	* The RC variable 4 is hardwired to channel 5	288	* The RC variable 4 is hardwired to channel 5
290	* The RC variable 5 is hardwired to channel 6	289	* The RC variable 5 is hardwired to channel 6
291	* The RC variable 6 is hardwired to channel 7	290	* The RC variable 6 is hardwired to channel 7
292	* The RC variable 7 is hardwired to channel 8	291	* The RC variable 7 is hardwired to channel 8
293	* Alternatively, one could bind them to channel (4 + varNum) - or whatever...	292	* Alternatively, one could bind them to channel (4 + varNum) - or whatever...
294	*/	293	*/
295	return PPM_in[varNum + 1] + POT_OFFSET;	294	return PPM_in[varNum + 1] + POT_OFFSET;
296	}	295	}
297		296
298	uint8_t RC_getSignalQuality(void) {	297	uint8_t RC_getSignalQuality(void) {
299	if (RC_Quality >= 160)	298	if (RC_Quality >= 160)
300	return SIGNAL_GOOD;	299	return SIGNAL_GOOD;
301	if (RC_Quality >= 140)	300	if (RC_Quality >= 140)
302	return SIGNAL_OK;	301	return SIGNAL_OK;
303	if (RC_Quality >= 120)	302	if (RC_Quality >= 120)
304	return SIGNAL_BAD;	303	return SIGNAL_BAD;
305	return SIGNAL_LOST;	304	return SIGNAL_LOST;
306	}	305	}
307		306
308	/*	307	/*
309	* To should fired only when the right stick is in the center position.	308	* To should fired only when the right stick is in the center position.
310	* This will cause the value of pitch and roll stick to be adjusted	309	* This will cause the value of pitch and roll stick to be adjusted
311	* to zero (not just to near zero, as per the assumption in rc.c	310	* to zero (not just to near zero, as per the assumption in rc.c
312	* about the rc signal. I had values about 50..70 with a Futaba	311	* about the rc signal. I had values about 50..70 with a Futaba
313	* R617 receiver.) This calibration is not strictly necessary, but	312	* R617 receiver.) This calibration is not strictly necessary, but
314	* for control logic that depends on the exact (non)center position	313	* for control logic that depends on the exact (non)center position
315	* of a stick, it may be useful.	314	* of a stick, it may be useful.
316	*/	315	*/
317	void RC_calibrate(void) {	316	void RC_calibrate(void) {
318	// Do nothing.	317	// Do nothing.
319	}	318	}
320		319
321	/*	320	/*
322	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {	321	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
323	// In HH, it s OK to trim the R/C. The effect should not be conteracted here.	322	// In HH, it s OK to trim the R/C. The effect should not be conteracted here.
324	stickOffsetPitch = stickOffsetRoll = 0;	323	stickOffsetPitch = stickOffsetRoll = 0;
325	} else {	324	} else {
326	stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;	325	stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
327	stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;	326	stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;
328	}	327	}
329	}	328	}
330	*/	329	*/
331		330
332	uint8_t RC_getCommand(void) {	331	uint8_t RC_getCommand(void) {
333	if (commandTimer == COMMAND_TIMER) {	332	if (commandTimer == COMMAND_TIMER) {
334	// Stick has been held long enough; command committed.	333	// Stick has been held long enough; command committed.
335	return lastRCCommand;	334	return lastRCCommand;
336	}	335	}
337	// Not yet sure what the command is.	336	// Not yet sure what the command is.
338	return COMMAND_NONE;	337	return COMMAND_NONE;
339	}	338	}
340		339
341	/*	340	/*
342	* Command arguments on R/C:	341	* Command arguments on R/C:
343	* 2--3--4	342	* 2--3--4
344	* \| \| +	343	* \| \| +
345	* 1 0 5 ^ 0	344	* 1 0 5 ^ 0
346	* \| \| \|	345	* \| \| \|
347	* 8--7--6	346	* 8--7--6
348	*	347	*
349	* + <--	348	* + <--
350	* 0	349	* 0
351	*	350	*
352	* Not in any of these positions: 0	351	* Not in any of these positions: 0
353	*/	352	*/
354		353
355	#define ARGUMENT_THRESHOLD 70	354	#define ARGUMENT_THRESHOLD 70
356	#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH	355	#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
357	#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL	356	#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
358		357
359	uint8_t RC_getArgument(void) {	358	uint8_t RC_getArgument(void) {
360	if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {	359	if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
361	// vertical is up	360	// vertical is up
362	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)	361	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
363	return 2;	362	return 2;
364	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)	363	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
365	return 4;	364	return 4;
366	return 3;	365	return 3;
367	} else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {	366	} else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
368	// vertical is down	367	// vertical is down
369	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)	368	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
370	return 8;	369	return 8;
371	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)	370	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
372	return 6;	371	return 6;
373	return 7;	372	return 7;
374	} else {	373	} else {
375	// vertical is around center	374	// vertical is around center
376	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)	375	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
377	return 1;	376	return 1;
378	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)	377	if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
379	return 5;	378	return 5;
380	return 0;	379	return 0;
381	}	380	}
382	}	381	}
383		382
384	uint8_t RC_getLooping(uint8_t looping) {	383	uint8_t RC_getLooping(uint8_t looping) {
385	// static uint8_t looping = 0;	384	// static uint8_t looping = 0;
386		385
387	if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) {	386	if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) {
388	looping \|= (LOOPING_ROLL_AXIS \| LOOPING_LEFT);	387	looping \|= (LOOPING_ROLL_AXIS \| LOOPING_LEFT);
389	} else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {	388	} else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
390	looping &= (~(LOOPING_ROLL_AXIS \| LOOPING_LEFT));	389	looping &= (~(LOOPING_ROLL_AXIS \| LOOPING_LEFT));
391	}	390	}
392		391
393	if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) {	392	if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) {
394	looping \|= (LOOPING_ROLL_AXIS \| LOOPING_RIGHT);	393	looping \|= (LOOPING_ROLL_AXIS \| LOOPING_RIGHT);
395	} else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {	394	} else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
396	looping &= (~(LOOPING_ROLL_AXIS \| LOOPING_RIGHT));	395	looping &= (~(LOOPING_ROLL_AXIS \| LOOPING_RIGHT));
397	}	396	}
398		397
399	if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) {	398	if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) {
400	looping \|= (LOOPING_PITCH_AXIS \| LOOPING_UP);	399	looping \|= (LOOPING_PITCH_AXIS \| LOOPING_UP);
401	} else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {	400	} else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
402	looping &= (~(LOOPING_PITCH_AXIS \| LOOPING_UP));	401	looping &= (~(LOOPING_PITCH_AXIS \| LOOPING_UP));
403	}	402	}
404		403
405	if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) {	404	if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) {
406	looping \|= (LOOPING_PITCH_AXIS \| LOOPING_DOWN);	405	looping \|= (LOOPING_PITCH_AXIS \| LOOPING_DOWN);
407	} else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {	406	} else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
408	looping &= (~(LOOPING_PITCH_AXIS \| LOOPING_DOWN));	407	looping &= (~(LOOPING_PITCH_AXIS \| LOOPING_DOWN));
409	}	408	}
410		409
411	return looping;	410	return looping;
412	}	411	}
413		412
414	uint8_t RC_testCompassCalState(void) {	413	uint8_t RC_testCompassCalState(void) {
415	static uint8_t stick = 1;	414	static uint8_t stick = 1;
416	// if pitch is centered or top set stick to zero	415	// if pitch is centered or top set stick to zero
417	if(RCChannel(CH_PITCH) > -20) stick = 0;	416	if(RCChannel(CH_PITCH) > -20) stick = 0;
418	// if pitch is down trigger to next cal state	417	// if pitch is down trigger to next cal state
419	if((RCChannel(CH_PITCH) < -70) && !stick) {	418	if((RCChannel(CH_PITCH) < -70) && !stick) {
420	stick = 1;	419	stick = 1;
421	return 1;	420	return 1;
422	}	421	}
423	return 0;	422	return 0;
424	}	423	}
425	/*	424	/*
426	* Abstract controls are not used at the moment.	425	* Abstract controls are not used at the moment.
427	t_control rc_control = {	426	t_control rc_control = {
428	RC_getPitch,	427	RC_getPitch,
429	RC_getRoll,	428	RC_getRoll,
430	RC_getYaw,	429	RC_getYaw,
431	RC_getThrottle,	430	RC_getThrottle,
432	RC_getSignalQuality,	431	RC_getSignalQuality,
433	RC_calibrate	432	RC_calibrate
434	};	433	};
435	*/	434	*/
436		435

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c - Rev 1775 → 1796