WebSVN - FlightCtrl - Diff - Rev 2071 and 2073 - /branches/dongfang_FC


#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
 
#include "rc.h"
#include "controlMixer.h"
#include "configuration.h"
#include "commands.h"
#include "output.h"
 
// The channel array is 0-based!
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint8_t RCQuality;
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t commandTimer = 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<<6);
  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);
 
  RCQuality = 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 channel 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)) {
    index = 0;
  } else { // within the PPM frame
    if (index < MAX_CHANNELS) { // 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 -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
        // check for stable signal
        if (abs(signal - PPM_in[index]) < 6) {
          if (RCQuality < 200)
            RCQuality += 10;
          else
            RCQuality = 200;
        }
        // If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
        // 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 (RCQuality >= 195)
          PPM_diff[index] = signal - PPM_in[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[channelMap.channels[dimension]]
#define RCDiff(dimension) PPM_diff[channelMap.channels[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;
}
 
/*
 * Get Pitch, Roll, Throttle, Yaw values
 */
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
  int16_t tmp1, tmp2;
  if (RCQuality) {
    RCQuality--;
    PRTY[CONTROL_PITCH]     = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
    PRTY[CONTROL_ROLL]      = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
    int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
    // Negative throttle values are taken as zero.
    if (throttle > 0)
      PRTY[CONTROL_THROTTLE]  = throttle;
    tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
    // exponential stick sensitivity in yawing rate
    tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo  y = ax + bx^2
    tmp2 += (staticParams.stickYawP * tmp1) >> 2;
    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;
    }
  } // if RCQuality is no good, we just do nothing.
}
 
/*
 * 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 + CH_POTS) + POT_OFFSET;
  /*
   * Let's just say:
   * The RC variable i is hardwired to channel i, i>=4
   */
  return PPM_in[varNum] + POT_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
  if (RCQuality >= 160)
    return SIGNAL_GOOD;
  if (RCQuality >= 140)
    return SIGNAL_OK;
  if (RCQuality >= 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;
  }
}
 
#ifdef USE_MK3MAG
/*
 * For each time the stick is pulled, returns true.
 */
uint8_t RC_testCompassCalState(void) {
  static uint8_t stickPulled = 1;
  // if pitch is centered or top set stick to zero
  if (RCChannel(CH_PITCH) > -20)
    stickPulled = 0;
  // if pitch is down trigger to next cal state
  if ((RCChannel(CH_PITCH) < -70) && !stickPulled) {
    stickPulled = 1;
    return 1;
  }
  return 0;
}
#endif
 
/*
 * 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 2071	Rev 2073
1	#include <stdlib.h>	1	#include <stdlib.h>
2	#include <avr/io.h>	2	#include <avr/io.h>
3	#include <avr/interrupt.h>	3	#include <avr/interrupt.h>
4		4
5	#include "rc.h"	5	#include "rc.h"
6	#include "controlMixer.h"	6	#include "controlMixer.h"
7	#include "configuration.h"	7	#include "configuration.h"
8	#include "commands.h"	8	#include "commands.h"
9	#include "output.h"	9	#include "output.h"
10		10
11	// The channel array is 0-based!	11	// The channel array is 0-based!
12	volatile int16_t PPM_in[MAX_CHANNELS];	12	volatile int16_t PPM_in[MAX_CHANNELS];
13	volatile int16_t PPM_diff[MAX_CHANNELS];	13	volatile int16_t PPM_diff[MAX_CHANNELS];
14	volatile uint8_t RCQuality;	14	volatile uint8_t RCQuality;
15	uint8_t lastRCCommand = COMMAND_NONE;	15	uint8_t lastRCCommand = COMMAND_NONE;
16	uint8_t commandTimer = 0;	16	uint8_t commandTimer = 0;
17		17
18	/***************************************************************	18	/***************************************************************
19	* 16bit timer 1 is used to decode the PPM-Signal	19	* 16bit timer 1 is used to decode the PPM-Signal
20	***************************************************************/	20	***************************************************************/
21	void RC_Init(void) {	21	void RC_Init(void) {
22	uint8_t sreg = SREG;	22	uint8_t sreg = SREG;
23		23
24	// disable all interrupts before reconfiguration	24	// disable all interrupts before reconfiguration
25	cli();	25	cli();
26		26
27	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1	27	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
28	DDRD &= ~(1<<6);	28	DDRD &= ~(1<<6);
29	PORTD \|= (1<<PORTD6);	29	PORTD \|= (1<<PORTD6);
30		30
31	// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)	31	// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
32	// set as output	32	// set as output
33	DDRD \|= (1<<DDD5) \| (1<<DDD4) \| (1<<DDD3);	33	DDRD \|= (1<<DDD5) \| (1<<DDD4) \| (1<<DDD3);
34	// low level	34	// low level
35	PORTD &= ~((1<<PORTD5) \| (1<<PORTD4) \| (1<<PORTD3));	35	PORTD &= ~((1<<PORTD5) \| (1<<PORTD4) \| (1<<PORTD3));
36		36
37	// PD3 can't be used if 2nd UART is activated	37	// PD3 can't be used if 2nd UART is activated
38	// because TXD1 is at that port	38	// because TXD1 is at that port
39	if (CPUType != ATMEGA644P) {	39	if (CPUType != ATMEGA644P) {
40	DDRD \|= (1<<PORTD3);	40	DDRD \|= (1<<PORTD3);
41	PORTD &= ~(1<<PORTD3);	41	PORTD &= ~(1<<PORTD3);
42	}	42	}
43		43
44	// Timer/Counter1 Control Register A, B, C	44	// Timer/Counter1 Control Register A, B, C
45		45
46	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)	46	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
47	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)	47	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
48	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)	48	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
49	// Enable input capture noise cancler (bit: ICNC1=1)	49	// Enable input capture noise cancler (bit: ICNC1=1)
50	// Trigger on positive edge of the input capture pin (bit: ICES1=1),	50	// Trigger on positive edge of the input capture pin (bit: ICES1=1),
51	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s	51	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
52	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.	52	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
53	TCCR1A &= ~((1 << COM1A1) \| (1 << COM1A0) \| (1 << COM1B1) \| (1 << COM1B0) \| (1 << WGM11) \| (1 << WGM10));	53	TCCR1A &= ~((1 << COM1A1) \| (1 << COM1A0) \| (1 << COM1B1) \| (1 << COM1B0) \| (1 << WGM11) \| (1 << WGM10));
54	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));	54	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));
55	TCCR1B \|= (1 << CS11) \| (1 << CS10) \| (1 << ICES1) \| (1 << ICNC1);	55	TCCR1B \|= (1 << CS11) \| (1 << CS10) \| (1 << ICES1) \| (1 << ICNC1);
56	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));	56	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));
57		57
58	// Timer/Counter1 Interrupt Mask Register	58	// Timer/Counter1 Interrupt Mask Register
59	// Enable Input Capture Interrupt (bit: ICIE1=1)	59	// Enable Input Capture Interrupt (bit: ICIE1=1)
60	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)	60	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
61	// Enable Overflow Interrupt (bit: TOIE1=0)	61	// Enable Overflow Interrupt (bit: TOIE1=0)
62	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));	62	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));
63	TIMSK1 \|= (1<<ICIE1);	63	TIMSK1 \|= (1<<ICIE1);
64		64
65	RCQuality = 0;	65	RCQuality = 0;
66		66
67	SREG = sreg;	67	SREG = sreg;
68	}	68	}
69		69
70	/********************************************************************/	70	/********************************************************************/
71	/* Every time a positive edge is detected at PD6 */	71	/* Every time a positive edge is detected at PD6 */
72	/********************************************************************/	72	/********************************************************************/
73	/* t-Frame	73	/* t-Frame
74	<----------------------------------------------------------------------->	74	<----------------------------------------------------------------------->
75	____ ______ _____ ________ ______ sync gap ____	75	____ ______ _____ ________ ______ sync gap ____
76	\| \| \| \| \| \| \| \| \| \| \|	76	\| \| \| \| \| \| \| \| \| \| \|
77	\| \| \| \| \| \| \| \| \| \| \|	77	\| \| \| \| \| \| \| \| \| \| \|
78	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	78	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
79	<-----><-------><------><----------- <------> <---	79	<-----><-------><------><----------- <------> <---
80	t0 t1 t2 t4 tn t0	80	t0 t1 t2 t4 tn t0
81		81
82	The PPM-Frame length is 22.5 ms.	82	The PPM-Frame length is 22.5 ms.
83	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.	83	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
84	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.	84	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
85	The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.	85	The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.
86	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.	86	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
87	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.	87	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
88	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is	88	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
89	the syncronization gap.	89	the syncronization gap.
90	*/	90	*/
91	ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms	91	ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
92	int16_t signal = 0, tmp;	92	int16_t signal = 0, tmp;
93	static int16_t index;	93	static int16_t index;
94	static uint16_t oldICR1 = 0;	94	static uint16_t oldICR1 = 0;
95		95
96	// 16bit Input Capture Register ICR1 contains the timer value TCNT1	96	// 16bit Input Capture Register ICR1 contains the timer value TCNT1
97	// at the time the edge was detected	97	// at the time the edge was detected
98		98
99	// calculate the time delay to the previous event time which is stored in oldICR1	99	// calculate the time delay to the previous event time which is stored in oldICR1
100	// calculatiing the difference of the two uint16_t and converting the result to an int16_t	100	// calculatiing the difference of the two uint16_t and converting the result to an int16_t
101	// implicit handles a timer overflow 65535 -> 0 the right way.	101	// implicit handles a timer overflow 65535 -> 0 the right way.
102	signal = (uint16_t) ICR1 - oldICR1;	102	signal = (uint16_t) ICR1 - oldICR1;
103	oldICR1 = ICR1;	103	oldICR1 = ICR1;
104		104
105	//sync gap? (3.52 ms < signal < 25.6 ms)	105	//sync gap? (3.52 ms < signal < 25.6 ms)
106	if ((signal > 1100) && (signal < 8000)) {	106	if ((signal > 1100) && (signal < 8000)) {
107	index = 0;	107	index = 0;
108	} else { // within the PPM frame	108	} else { // within the PPM frame
109	if (index < MAX_CHANNELS) { // PPM24 supports 12 channels	109	if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
110	// check for valid signal length (0.8 ms < signal < 2.1984 ms)	110	// check for valid signal length (0.8 ms < signal < 2.1984 ms)
111	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625	111	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
112	if ((signal > 250) && (signal < 687)) {	112	if ((signal > 250) && (signal < 687)) {
113	// shift signal to zero symmetric range -154 to 159	113	// shift signal to zero symmetric range -154 to 159
114	signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)	114	signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
115	// check for stable signal	115	// check for stable signal
116	if (abs(signal - PPM_in[index]) < 6) {	116	if (abs(signal - PPM_in[index]) < 6) {
117	if (RCQuality < 200)	117	if (RCQuality < 200)
118	RCQuality += 10;	118	RCQuality += 10;
119	else	119	else
120	RCQuality = 200;	120	RCQuality = 200;
121	}	121	}
122	// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.	122	// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
123	// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {	123	// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
124	// In addition, if the signal is very close to 0, just set it to 0.	124	// In addition, if the signal is very close to 0, just set it to 0.
125	if (signal >= -1 && signal <= 1) {	125	if (signal >= -1 && signal <= 1) {
126	tmp = 0;	126	tmp = 0;
127	//} else {	127	//} else {
128	// tmp = PPM_in[index];	128	// tmp = PPM_in[index];
129	// }	129	// }
130	} else	130	} else
131	tmp = signal;	131	tmp = signal;
132	// calculate signal difference on good signal level	132	// calculate signal difference on good signal level
133	if (RCQuality >= 195)	133	if (RCQuality >= 195)
134	PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction	134	PPM_diff[index] = signal - PPM_in[index]; //((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
135	else	135	else
136	PPM_diff[index] = 0;	136	PPM_diff[index] = 0;
137	PPM_in[index] = tmp; // update channel value	137	PPM_in[index] = tmp; // update channel value
138	}	138	}
139	index++; // next channel	139	index++; // next channel
140	// demux sum signal for channels 5 to 7 to J3, J4, J5	140	// demux sum signal for channels 5 to 7 to J3, J4, J5
141	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the	141	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
142	// channels are usually available at the receiver anyway.	142	// channels are usually available at the receiver anyway.
143	// if(index == 5) J3HIGH; else J3LOW;	143	// if(index == 5) J3HIGH; else J3LOW;
144	// if(index == 6) J4HIGH; else J4LOW;	144	// if(index == 6) J4HIGH; else J4LOW;
145	// if(CPUType != ATMEGA644P) // not used as TXD1	145	// if(CPUType != ATMEGA644P) // not used as TXD1
146	// {	146	// {
147	// if(index == 7) J5HIGH; else J5LOW;	147	// if(index == 7) J5HIGH; else J5LOW;
148	// }	148	// }
149	}	149	}
150	}	150	}
151	}	151	}
152		152
153	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]	153	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
154	#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]	154	#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
155	#define COMMAND_THRESHOLD 85	155	#define COMMAND_THRESHOLD 85
156	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE	156	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
157	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW	157	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
158		158
159	// Internal.	159	// Internal.
160	uint8_t RC_getStickCommand(void) {	160	uint8_t RC_getStickCommand(void) {
161	if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {	161	if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
162	// vertical is up	162	// vertical is up
163	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)	163	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
164	return COMMAND_GYROCAL;	164	return COMMAND_GYROCAL;
165	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)	165	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
166	return COMMAND_ACCCAL;	166	return COMMAND_ACCCAL;
167	return COMMAND_NONE;	167	return COMMAND_NONE;
168	} else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {	168	} else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
169	// vertical is down	169	// vertical is down
170	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)	170	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
171	return COMMAND_STOP;	171	return COMMAND_STOP;
172	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)	172	if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
173	return COMMAND_START;	173	return COMMAND_START;
174	return COMMAND_NONE;	174	return COMMAND_NONE;
175	}	175	}
176	// vertical is around center	176	// vertical is around center
177	return COMMAND_NONE;	177	return COMMAND_NONE;
178	}	178	}
179		179
180	/*	180	/*
181	* Get Pitch, Roll, Throttle, Yaw values	181	* Get Pitch, Roll, Throttle, Yaw values
182	*/	182	*/
183	void RC_periodicTaskAndPRTY(int16_t* PRTY) {	183	void RC_periodicTaskAndPRTY(int16_t* PRTY) {
184	int16_t tmp1, tmp2;	184	int16_t tmp1, tmp2;
185	if (RCQuality) {	185	if (RCQuality) {
186	RCQuality--;	186	RCQuality--;
187	PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;	187	PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
188	PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;	188	PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
189	int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;	189	int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
190	// Negative throttle values are taken as zero.	190	// Negative throttle values are taken as zero.
191	if (throttle > 0)	191	if (throttle > 0)
192	PRTY[CONTROL_THROTTLE] = throttle;	192	PRTY[CONTROL_THROTTLE] = throttle;
193	tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);	193	tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
194	// exponential stick sensitivity in yawing rate	194	// exponential stick sensitivity in yawing rate
195	tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2	195	tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2
196	tmp2 += (staticParams.stickYawP * tmp1) >> 2;	196	tmp2 += (staticParams.stickYawP * tmp1) >> 2;
197	PRTY[CONTROL_YAW] = tmp2;	197	PRTY[CONTROL_YAW] = tmp2;
198		198
199	uint8_t command = RC_getStickCommand();	199	uint8_t command = RC_getStickCommand();
200	if (lastRCCommand == command) {	200	if (lastRCCommand == command) {
201	// Keep timer from overrunning.	201	// Keep timer from overrunning.
202	if (commandTimer < COMMAND_TIMER)	202	if (commandTimer < COMMAND_TIMER)
203	commandTimer++;	203	commandTimer++;
204	} else {	204	} else {
205	// There was a change.	205	// There was a change.
206	lastRCCommand = command;	206	lastRCCommand = command;
207	commandTimer = 0;	207	commandTimer = 0;
208	}	208	}
209	} // if RCQuality is no good, we just do nothing.	209	} // if RCQuality is no good, we just do nothing.
210	}	210	}
211		211
212	/*	212	/*
213	* Get other channel value	213	* Get other channel value
214	*/	214	*/
215	int16_t RC_getVariable(uint8_t varNum) {	215	int16_t RC_getVariable(uint8_t varNum) {
216	if (varNum < 4)	216	if (varNum < 4)
217	// 0th variable is 5th channel (1-based) etc.	217	// 0th variable is 5th channel (1-based) etc.
218	return RCChannel(varNum + CH_POTS) + POT_OFFSET;	218	return RCChannel(varNum + CH_POTS) + POT_OFFSET;
219	/*	219	/*
220	* Let's just say:	220	* Let's just say:
221	* The RC variable i is hardwired to channel i, i>=4	221	* The RC variable i is hardwired to channel i, i>=4
222	*/	222	*/
223	return PPM_in[varNum] + POT_OFFSET;	223	return PPM_in[varNum] + POT_OFFSET;
224	}	224	}
225		225
226	uint8_t RC_getSignalQuality(void) {	226	uint8_t RC_getSignalQuality(void) {
227	if (RCQuality >= 160)	227	if (RCQuality >= 160)
228	return SIGNAL_GOOD;	228	return SIGNAL_GOOD;
229	if (RCQuality >= 140)	229	if (RCQuality >= 140)
230	return SIGNAL_OK;	230	return SIGNAL_OK;
231	if (RCQuality >= 120)	231	if (RCQuality >= 120)
232	return SIGNAL_BAD;	232	return SIGNAL_BAD;
233	return SIGNAL_LOST;	233	return SIGNAL_LOST;
234	}	234	}
235		235
236	/*	236	/*
237	* To should fired only when the right stick is in the center position.	237	* To should fired only when the right stick is in the center position.
238	* This will cause the value of pitch and roll stick to be adjusted	238	* This will cause the value of pitch and roll stick to be adjusted
239	* to zero (not just to near zero, as per the assumption in rc.c	239	* to zero (not just to near zero, as per the assumption in rc.c
240	* about the rc signal. I had values about 50..70 with a Futaba	240	* about the rc signal. I had values about 50..70 with a Futaba
241	* R617 receiver.) This calibration is not strictly necessary, but	241	* R617 receiver.) This calibration is not strictly necessary, but
242	* for control logic that depends on the exact (non)center position	242	* for control logic that depends on the exact (non)center position
243	* of a stick, it may be useful.	243	* of a stick, it may be useful.
244	*/	244	*/
245	void RC_calibrate(void) {	245	void RC_calibrate(void) {
246	// Do nothing.	246	// Do nothing.
247	}	247	}
248		248
249	/*	249	/*
250	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {	250	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
251	// In HH, it s OK to trim the R/C. The effect should not be conteracted here.	251	// In HH, it s OK to trim the R/C. The effect should not be conteracted here.
252	stickOffsetPitch = stickOffsetRoll = 0;	252	stickOffsetPitch = stickOffsetRoll = 0;
253	} else {	253	} else {
254	stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;	254	stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
255	stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;	255	stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;
256	}	256	}
257	}	257	}
258	*/	258	*/
259		259
260	uint8_t RC_getCommand(void) {	260	uint8_t RC_getCommand(void) {
261	if (commandTimer == COMMAND_TIMER) {	261	if (commandTimer == COMMAND_TIMER) {
262	// Stick has been held long enough; command committed.	262	// Stick has been held long enough; command committed.
263	return lastRCCommand;	263	return lastRCCommand;
264	}	264	}
265	// Not yet sure what the command is.	265	// Not yet sure what the command is.
266	return COMMAND_NONE;	266	return COMMAND_NONE;
267	}	267	}
268		268
269	/*	269	/*
270	* Command arguments on R/C:	270	* Command arguments on R/C:
271	* 2--3--4	271	* 2--3--4
272	* \| \| +	272	* \| \| +
273	* 1 0 5 ^ 0	273	* 1 0 5 ^ 0
274	* \| \| \|	274	* \| \| \|
275	* 8--7--6	275	* 8--7--6
276	*	276	*
277	* + <--	277	* + <--
278	* 0	278	* 0
279	*	279	*
280	* Not in any of these positions: 0	280	* Not in any of these positions: 0
281	*/	281	*/
282		282
283	#define ARGUMENT_THRESHOLD 70	283	#define ARGUMENT_THRESHOLD 70
284	#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH	284	#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
285	#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL	285	#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
286		286
287	uint8_t RC_getArgument(void) {	287	uint8_t RC_getArgument(void) {
288	if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {	288	if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
289	// vertical is up	289	// vertical is up
290	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)	290	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
291	return 2;	291	return 2;
292	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)	292	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
293	return 4;	293	return 4;
294	return 3;	294	return 3;
295	} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {	295	} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
296	// vertical is down	296	// vertical is down
297	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)	297	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
298	return 8;	298	return 8;
299	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)	299	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
300	return 6;	300	return 6;
301	return 7;	301	return 7;
302	} else {	302	} else {
303	// vertical is around center	303	// vertical is around center
304	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)	304	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
305	return 1;	305	return 1;
306	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)	306	if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
307	return 5;	307	return 5;
308	return 0;	308	return 0;
309	}	309	}
310	}	310	}
311		311
312	#ifdef USE_MK3MAG	312	#ifdef USE_MK3MAG
313	/*	313	/*
314	* For each time the stick is pulled, returns true.	314	* For each time the stick is pulled, returns true.
315	*/	315	*/
316	uint8_t RC_testCompassCalState(void) {	316	uint8_t RC_testCompassCalState(void) {
317	static uint8_t stickPulled = 1;	317	static uint8_t stickPulled = 1;
318	// if pitch is centered or top set stick to zero	318	// if pitch is centered or top set stick to zero
319	if (RCChannel(CH_PITCH) > -20)	319	if (RCChannel(CH_PITCH) > -20)
320	stickPulled = 0;	320	stickPulled = 0;
321	// if pitch is down trigger to next cal state	321	// if pitch is down trigger to next cal state
322	if ((RCChannel(CH_PITCH) < -70) && !stickPulled) {	322	if ((RCChannel(CH_PITCH) < -70) && !stickPulled) {
323	stickPulled = 1;	323	stickPulled = 1;
324	return 1;	324	return 1;
325	}	325	}
326	return 0;	326	return 0;
327	}	327	}
328	#endif	328	#endif
329		329
330	/*	330	/*
331	* Abstract controls are not used at the moment.	331	* Abstract controls are not used at the moment.
332	t_control rc_control = {	332	t_control rc_control = {
333	RC_getPitch,	333	RC_getPitch,
334	RC_getRoll,	334	RC_getRoll,
335	RC_getYaw,	335	RC_getYaw,
336	RC_getThrottle,	336	RC_getThrottle,
337	RC_getSignalQuality,	337	RC_getSignalQuality,
338	RC_calibrate	338	RC_calibrate
339	};	339	};
340	*/	340	*/
341		341

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c - Rev 2071 → 2073