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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c @ 2758 - Rev 2160 → 2189