WebSVN - FlightCtrl - Diff - Rev 2109 and 2110 - /branches/dongfang_FC_fixedwing/arduino


#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 uint8_t RCQuality;
 
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t lastFlightMode = FLIGHT_MODE_NONE;
 
#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))
 
/***************************************************************
 *  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
  DDRB &= ~(1<<0);
  PORTB |= (1<<PORTB0);
 
  // 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 << 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, 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.5 ms < signal < 25.6 ms)
  if (signal > TIME(3.5)) {
 
    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.2 ms)
      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
        // shift signal to zero symmetric range  -154 to 159
        //signal -= 3750; // theoretical value
        signal -= (TIME(1.5) + RC_TRIM); // best value with my Futaba in zero trim.
        // check for stable signal
        if (abs(signal - PPM_in[index]) < TIME(0.05)) {
          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;
        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 COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
uint8_t getControlModeSwitch(void) {
        int16_t channel = RCChannel(CH_MODESWITCH);
        uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
        return flightMode;
}
 
// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
// Maybe simply: Very very low throttle.
// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
// mode switched: CHMOD
 
uint8_t RC_getCommand(void) {
        uint8_t flightMode = getControlModeSwitch();
 
        if (lastFlightMode != flightMode) {
                lastFlightMode = flightMode;
                lastRCCommand = COMMAND_CHMOD;
                return lastRCCommand;
        }
 
        int16_t channel = RCChannel(CH_THROTTLE);
 
        if (channel <= -TIME(0.55)) {
          lastRCCommand = COMMAND_GYROCAL;
          debugOut.analog[17] = 1;
        } else {
          lastRCCommand = COMMAND_NONE;
          debugOut.analog[17] = 0;
        }
        return lastRCCommand;
}
 
uint8_t RC_getArgument(void) {
        return lastFlightMode;
}
 
/*
 * Get Pitch, Roll, Throttle, Yaw values
 */
void RC_periodicTaskAndPRYT(int16_t* PRYT) {
  if (RCQuality) {
    RCQuality--;
 
    debugOut.analog[20] = RCChannel(CH_ELEVATOR);
    debugOut.analog[21] = RCChannel(CH_AILERONS);
    debugOut.analog[22] = RCChannel(CH_RUDDER);
    debugOut.analog[23] = RCChannel(CH_THROTTLE);
 
    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR) / RC_SCALING;
    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS) / RC_SCALING;
    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)   / RC_SCALING;
    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE) / RC_SCALING;
  } // 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) >> 2) + VARIABLE_OFFSET;
  /*
   * Let's just say:
   * The RC variable i is hardwired to channel i, i>=4
   */
  return (PPM_in[varNum] >> 2) + VARIABLE_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.
}
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c @ 2111 - Rev 2109 → 2110

Rev 2109		Rev 2110
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 uint8_t RCQuality;	13	volatile uint8_t RCQuality;
14		14
15	uint8_t lastRCCommand = COMMAND_NONE;	15	uint8_t lastRCCommand = COMMAND_NONE;
16	uint8_t lastFlightMode = FLIGHT_MODE_NONE;	16	uint8_t lastFlightMode = FLIGHT_MODE_NONE;
17		17
18	#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))	18	#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))
19		19
20	/***************************************************************	20	/***************************************************************
21	* 16bit timer 1 is used to decode the PPM-Signal	21	* 16bit timer 1 is used to decode the PPM-Signal
22	***************************************************************/	22	***************************************************************/
23	void RC_Init(void) {	23	void RC_Init(void) {
24	uint8_t sreg = SREG;	24	uint8_t sreg = SREG;
25		25
26	// disable all interrupts before reconfiguration	26	// disable all interrupts before reconfiguration
27	cli();	27	cli();
28		28
29	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1	29	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
30	DDRB &= ~(1<<0);	30	DDRB &= ~(1<<0);
31	PORTB \|= (1<<PORTB0);	31	PORTB \|= (1<<PORTB0);
32		32
33	// Timer/Counter1 Control Register A, B, C	33	// Timer/Counter1 Control Register A, B, C
34	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)	34	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
35	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)	35	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
36	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)	36	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
37	// Enable input capture noise cancler (bit: ICNC1=1)	37	// Enable input capture noise cancler (bit: ICNC1=1)
38	// Trigger on positive edge of the input capture pin (bit: ICES1=1),	38	// Trigger on positive edge of the input capture pin (bit: ICES1=1),
39	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s	39	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
40	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.	40	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
41	TCCR1A &= ~((1 << COM1A1) \| (1 << COM1A0) \| (1 << COM1B1) \| (1 << COM1B0) \| (1 << WGM11) \| (1 << WGM10));	41	TCCR1A &= ~((1 << COM1A1) \| (1 << COM1A0) \| (1 << COM1B1) \| (1 << COM1B0) \| (1 << WGM11) \| (1 << WGM10));
42	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));	42	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));
43	TCCR1B \|= (1 << CS11) \| (1 << ICES1) \| (1 << ICNC1);	43	TCCR1B \|= (1 << CS11) \| (1 << ICES1) \| (1 << ICNC1);
44	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));	44	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));
45		45
46	// Timer/Counter1 Interrupt Mask Register	46	// Timer/Counter1 Interrupt Mask Register
47	// Enable Input Capture Interrupt (bit: ICIE1=1)	47	// Enable Input Capture Interrupt (bit: ICIE1=1)
48	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)	48	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
49	// Enable Overflow Interrupt (bit: TOIE1=0)	49	// Enable Overflow Interrupt (bit: TOIE1=0)
50	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));	50	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));
51	TIMSK1 \|= (1<<ICIE1);	51	TIMSK1 \|= (1<<ICIE1);
52		52
53	RCQuality = 0;	53	RCQuality = 0;
54		54
55	SREG = sreg;	55	SREG = sreg;
56	}	56	}
57		57
58	/********************************************************************/	58	/********************************************************************/
59	/* Every time a positive edge is detected at PD6 */	59	/* Every time a positive edge is detected at PD6 */
60	/********************************************************************/	60	/********************************************************************/
61	/* t-Frame	61	/* t-Frame
62	<----------------------------------------------------------------------->	62	<----------------------------------------------------------------------->
63	____ ______ _____ ________ ______ sync gap ____	63	____ ______ _____ ________ ______ sync gap ____
64	\| \| \| \| \| \| \| \| \| \| \|	64	\| \| \| \| \| \| \| \| \| \| \|
65	\| \| \| \| \| \| \| \| \| \| \|	65	\| \| \| \| \| \| \| \| \| \| \|
66	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	66	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
67	<-----><-------><------><----------- <------> <---	67	<-----><-------><------><----------- <------> <---
68	t0 t1 t2 t4 tn t0	68	t0 t1 t2 t4 tn t0
69		69
70	The PPM-Frame length is 22.5 ms.	70	The PPM-Frame length is 22.5 ms.
71	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.	71	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
72	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.	72	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
73	The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.	73	The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.
74	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.	74	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
75	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.	75	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
76	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is	76	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
77	the syncronization gap.	77	the syncronization gap.
78	*/	78	*/
79	ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms	79	ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
80	int16_t signal, tmp;	80	int16_t signal, tmp;
81	static int16_t index;	81	static int16_t index;
82	static uint16_t oldICR1 = 0;	82	static uint16_t oldICR1 = 0;
83		83
84	// 16bit Input Capture Register ICR1 contains the timer value TCNT1	84	// 16bit Input Capture Register ICR1 contains the timer value TCNT1
85	// at the time the edge was detected	85	// at the time the edge was detected
86		86
87	// calculate the time delay to the previous event time which is stored in oldICR1	87	// calculate the time delay to the previous event time which is stored in oldICR1
88	// calculatiing the difference of the two uint16_t and converting the result to an int16_t	88	// calculatiing the difference of the two uint16_t and converting the result to an int16_t
89	// implicit handles a timer overflow 65535 -> 0 the right way.	89	// implicit handles a timer overflow 65535 -> 0 the right way.
90	signal = (uint16_t) ICR1 - oldICR1;	90	signal = (uint16_t) ICR1 - oldICR1;
91	oldICR1 = ICR1;	91	oldICR1 = ICR1;
92		92
93	//sync gap? (3.5 ms < signal < 25.6 ms)	93	//sync gap? (3.5 ms < signal < 25.6 ms)
94	if (signal > TIME(3.5)) {	94	if (signal > TIME(3.5)) {
95	// never happens...	-
96	index = 0;	95	index = 0;
97	} else { // within the PPM frame	96	} else { // within the PPM frame
98	if (index < MAX_CHANNELS) { // PPM24 supports 12 channels	97	if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
99	// check for valid signal length (0.8 ms < signal < 2.2 ms)	98	// check for valid signal length (0.8 ms < signal < 2.2 ms)
100	if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {	99	if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
101	// shift signal to zero symmetric range -154 to 159	100	// shift signal to zero symmetric range -154 to 159
102	//signal -= 3750; // theoretical value	101	//signal -= 3750; // theoretical value
103	signal -= (TIME(1.5) + RC_TRIM); // best value with my Futaba in zero trim.	102	signal -= (TIME(1.5) + RC_TRIM); // best value with my Futaba in zero trim.
104	// check for stable signal	103	// check for stable signal
105	if (abs(signal - PPM_in[index]) < TIME(0.05)) {	104	if (abs(signal - PPM_in[index]) < TIME(0.05)) {
106	if (RCQuality < 200)	105	if (RCQuality < 200)
107	RCQuality += 10;	106	RCQuality += 10;
108	else	107	else
109	RCQuality = 200;	108	RCQuality = 200;
110	}	109	}
111	// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.	110	// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
112	// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {	111	// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
113	// In addition, if the signal is very close to 0, just set it to 0.	112	// In addition, if the signal is very close to 0, just set it to 0.
114	if (signal >= -1 && signal <= 1) {	113	if (signal >= -1 && signal <= 1) {
115	tmp = 0;	114	tmp = 0;
116	//} else {	115	//} else {
117	// tmp = PPM_in[index];	116	// tmp = PPM_in[index];
118	// }	117	// }
119	} else	118	} else
120	tmp = signal;	119	tmp = signal;
121	PPM_in[index] = tmp; // update channel value	120	PPM_in[index] = tmp; // update channel value
122	}	121	}
123	index++; // next channel	122	index++; // next channel
124	// demux sum signal for channels 5 to 7 to J3, J4, J5	123	// demux sum signal for channels 5 to 7 to J3, J4, J5
125	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the	124	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
126	// channels are usually available at the receiver anyway.	125	// channels are usually available at the receiver anyway.
127	// if(index == 5) J3HIGH; else J3LOW;	126	// if(index == 5) J3HIGH; else J3LOW;
128	// if(index == 6) J4HIGH; else J4LOW;	127	// if(index == 6) J4HIGH; else J4LOW;
129	// if(CPUType != ATMEGA644P) // not used as TXD1	128	// if(CPUType != ATMEGA644P) // not used as TXD1
130	// {	129	// {
131	// if(index == 7) J5HIGH; else J5LOW;	130	// if(index == 7) J5HIGH; else J5LOW;
132	// }	131	// }
133	}	132	}
134	}	133	}
135	}	134	}
136		135
137	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]	136	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
138	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE	137	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
139	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW	138	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
140		139
141	uint8_t getControlModeSwitch(void) {	140	uint8_t getControlModeSwitch(void) {
142	int16_t channel = RCChannel(CH_MODESWITCH);	141	int16_t channel = RCChannel(CH_MODESWITCH);
143	uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);	142	uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
144	return flightMode;	143	return flightMode;
145	}	144	}
146		145
147	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.	146	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
148	// Maybe simply: Very very low throttle.	147	// Maybe simply: Very very low throttle.
149	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).	148	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
150	// mode switched: CHMOD	149	// mode switched: CHMOD
151		150
152	uint8_t RC_getCommand(void) {	151	uint8_t RC_getCommand(void) {
153	uint8_t flightMode = getControlModeSwitch();	152	uint8_t flightMode = getControlModeSwitch();
154		153
155	if (lastFlightMode != flightMode) {	154	if (lastFlightMode != flightMode) {
156	lastFlightMode = flightMode;	155	lastFlightMode = flightMode;
157	lastRCCommand = COMMAND_CHMOD;	156	lastRCCommand = COMMAND_CHMOD;
158	return lastRCCommand;	157	return lastRCCommand;
159	}	158	}
160		159
161	int16_t channel = RCChannel(CH_THROTTLE);	160	int16_t channel = RCChannel(CH_THROTTLE);
162		161
163	if (channel <= -TIME(0.55)) {	162	if (channel <= -TIME(0.55)) {
164	lastRCCommand = COMMAND_GYROCAL;	163	lastRCCommand = COMMAND_GYROCAL;
165	debugOut.analog[17] = 1;	164	debugOut.analog[17] = 1;
166	} else {	165	} else {
167	lastRCCommand = COMMAND_NONE;	166	lastRCCommand = COMMAND_NONE;
168	debugOut.analog[17] = 0;	167	debugOut.analog[17] = 0;
169	}	168	}
170	return lastRCCommand;	169	return lastRCCommand;
171	}	170	}
172		171
173	uint8_t RC_getArgument(void) {	172	uint8_t RC_getArgument(void) {
174	return lastFlightMode;	173	return lastFlightMode;
175	}	174	}
176		175
177	/*	176	/*
178	* Get Pitch, Roll, Throttle, Yaw values	177	* Get Pitch, Roll, Throttle, Yaw values
179	*/	178	*/
180	void RC_periodicTaskAndPRYT(int16_t* PRYT) {	179	void RC_periodicTaskAndPRYT(int16_t* PRYT) {
181	if (RCQuality) {	180	if (RCQuality) {
182	RCQuality--;	181	RCQuality--;
183		182
184	debugOut.analog[20] = RCChannel(CH_ELEVATOR);	183	debugOut.analog[20] = RCChannel(CH_ELEVATOR);
185	debugOut.analog[21] = RCChannel(CH_AILERONS);	184	debugOut.analog[21] = RCChannel(CH_AILERONS);
186	debugOut.analog[22] = RCChannel(CH_RUDDER);	185	debugOut.analog[22] = RCChannel(CH_RUDDER);
187	debugOut.analog[23] = RCChannel(CH_THROTTLE);	186	debugOut.analog[23] = RCChannel(CH_THROTTLE);
188		187
189	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) / RC_SCALING;	188	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) / RC_SCALING;
190	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) / RC_SCALING;	189	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) / RC_SCALING;
191	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) / RC_SCALING;	190	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) / RC_SCALING;
192	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) / RC_SCALING;	191	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) / RC_SCALING;
193	} // if RCQuality is no good, we just do nothing.	192	} // if RCQuality is no good, we just do nothing.
194	}	193	}
195		194
196	/*	195	/*
197	* Get other channel value	196	* Get other channel value
198	*/	197	*/
199	int16_t RC_getVariable(uint8_t varNum) {	198	int16_t RC_getVariable(uint8_t varNum) {
200	if (varNum < 4)	199	if (varNum < 4)
201	// 0th variable is 5th channel (1-based) etc.	200	// 0th variable is 5th channel (1-based) etc.
202	return (RCChannel(varNum + CH_POTS) >> 2) + VARIABLE_OFFSET;	201	return (RCChannel(varNum + CH_POTS) >> 2) + VARIABLE_OFFSET;
203	/*	202	/*
204	* Let's just say:	203	* Let's just say:
205	* The RC variable i is hardwired to channel i, i>=4	204	* The RC variable i is hardwired to channel i, i>=4
206	*/	205	*/
207	return (PPM_in[varNum] >> 2) + VARIABLE_OFFSET;	206	return (PPM_in[varNum] >> 2) + VARIABLE_OFFSET;
208	}	207	}
209		208
210	uint8_t RC_getSignalQuality(void) {	209	uint8_t RC_getSignalQuality(void) {
211	if (RCQuality >= 160)	210	if (RCQuality >= 160)
212	return SIGNAL_GOOD;	211	return SIGNAL_GOOD;
213	if (RCQuality >= 140)	212	if (RCQuality >= 140)
214	return SIGNAL_OK;	213	return SIGNAL_OK;
215	if (RCQuality >= 120)	214	if (RCQuality >= 120)
216	return SIGNAL_BAD;	215	return SIGNAL_BAD;
217	return SIGNAL_LOST;	216	return SIGNAL_LOST;
218	}	217	}
219		218
220	/*	219	/*
221	* To should fired only when the right stick is in the center position.	220	* To should fired only when the right stick is in the center position.
222	* This will cause the value of pitch and roll stick to be adjusted	221	* This will cause the value of pitch and roll stick to be adjusted
223	* to zero (not just to near zero, as per the assumption in rc.c	222	* to zero (not just to near zero, as per the assumption in rc.c
224	* about the rc signal. I had values about 50..70 with a Futaba	223	* about the rc signal. I had values about 50..70 with a Futaba
225	* R617 receiver.) This calibration is not strictly necessary, but	224	* R617 receiver.) This calibration is not strictly necessary, but
226	* for control logic that depends on the exact (non)center position	225	* for control logic that depends on the exact (non)center position
227	* of a stick, it may be useful.	226	* of a stick, it may be useful.
228	*/	227	*/
229	void RC_calibrate(void) {	228	void RC_calibrate(void) {
230	// Do nothing.	229	// Do nothing.
231	}	230	}
232		231