WebSVN - FlightCtrl - Diff - Rev 2104 and 2107 - /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 uint8_t RCQuality;
 
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t lastFlightMode = FLIGHT_MODE_NONE;
 
/***************************************************************
 *  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, 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;
        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_THRESHOLD 85
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
#define RC_SCALING 4
 
uint8_t getControlModeSwitch(void) {
        int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
        uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
                (channel > 256*2/3 ? 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 <= -140) { // <= 900 us
                lastRCCommand = COMMAND_GYROCAL;
        } else {
          lastRCCommand = COMMAND_NONE;
        }
        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) + 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.
}
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/rc.c - Rev 2104 → 2107

Rev 2104		Rev 2107
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	/***************************************************************	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, 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 -= 475; // 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	PPM_in[index] = tmp; // update channel value	132	PPM_in[index] = tmp; // update channel value
133	}	133	}
134	index++; // next channel	134	index++; // next channel
135	// demux sum signal for channels 5 to 7 to J3, J4, J5	135	// demux sum signal for channels 5 to 7 to J3, J4, J5
136	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the	136	// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
137	// channels are usually available at the receiver anyway.	137	// channels are usually available at the receiver anyway.
138	// if(index == 5) J3HIGH; else J3LOW;	138	// if(index == 5) J3HIGH; else J3LOW;
139	// if(index == 6) J4HIGH; else J4LOW;	139	// if(index == 6) J4HIGH; else J4LOW;
140	// if(CPUType != ATMEGA644P) // not used as TXD1	140	// if(CPUType != ATMEGA644P) // not used as TXD1
141	// {	141	// {
142	// if(index == 7) J5HIGH; else J5LOW;	142	// if(index == 7) J5HIGH; else J5LOW;
143	// }	143	// }
144	}	144	}
145	}	145	}
146	}	146	}
147		147
148	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]	148	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
149	#define COMMAND_THRESHOLD 85	149	#define COMMAND_THRESHOLD 85
150	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE	150	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
151	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW	151	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
152		152
153	#define RC_SCALING 4	153	#define RC_SCALING 4
154		154
155	uint8_t getControlModeSwitch(void) {	155	uint8_t getControlModeSwitch(void) {
156	int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;	156	int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
157	uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :	157	uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
158	(channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);	158	(channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
159	return flightMode;	159	return flightMode;
160	}	160	}
161		161
162	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.	162	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
163	// Maybe simply: Very very low throttle.	163	// Maybe simply: Very very low throttle.
164	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).	164	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
165	// mode switched: CHMOD	165	// mode switched: CHMOD
166		166
167	uint8_t RC_getCommand(void) {	167	uint8_t RC_getCommand(void) {
168	uint8_t flightMode = getControlModeSwitch();	168	uint8_t flightMode = getControlModeSwitch();
169		169
170	if (lastFlightMode != flightMode) {	170	if (lastFlightMode != flightMode) {
171	lastFlightMode = flightMode;	171	lastFlightMode = flightMode;
172	lastRCCommand = COMMAND_CHMOD;	172	lastRCCommand = COMMAND_CHMOD;
173	return lastRCCommand;	173	return lastRCCommand;
174	}	174	}
175		175
176	int16_t channel = RCChannel(CH_THROTTLE);	176	int16_t channel = RCChannel(CH_THROTTLE);
177		177
178	if (channel <= -140) { // <= 900 us	178	if (channel <= -140) { // <= 900 us
179	lastRCCommand = COMMAND_GYROCAL;	179	lastRCCommand = COMMAND_GYROCAL;
180	} else {	180	} else {
181	lastRCCommand = COMMAND_NONE;	181	lastRCCommand = COMMAND_NONE;
182	}	182	}
183	return lastRCCommand;	183	return lastRCCommand;
184	}	184	}
185		185
186	uint8_t RC_getArgument(void) {	186	uint8_t RC_getArgument(void) {
187	return lastFlightMode;	187	return lastFlightMode;
188	}	188	}
189		189
190	/*	190	/*
191	* Get Pitch, Roll, Throttle, Yaw values	191	* Get Pitch, Roll, Throttle, Yaw values
192	*/	192	*/
193	void RC_periodicTaskAndPRYT(int16_t* PRYT) {	193	void RC_periodicTaskAndPRYT(int16_t* PRYT) {
194	if (RCQuality) {	194	if (RCQuality) {
195	RCQuality--;	195	RCQuality--;
196		196
197	debugOut.analog[20] = RCChannel(CH_ELEVATOR);	197	debugOut.analog[20] = RCChannel(CH_ELEVATOR);
198	debugOut.analog[21] = RCChannel(CH_AILERONS);	198	debugOut.analog[21] = RCChannel(CH_AILERONS);
199	debugOut.analog[22] = RCChannel(CH_RUDDER);	199	debugOut.analog[22] = RCChannel(CH_RUDDER);
200	debugOut.analog[23] = RCChannel(CH_THROTTLE);	200	debugOut.analog[23] = RCChannel(CH_THROTTLE);
201		201
202	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING;	202	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING;
203	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING;	203	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING;
204	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING;	204	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING;
205	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING;	205	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING;
206	} // if RCQuality is no good, we just do nothing.	206	} // if RCQuality is no good, we just do nothing.
207	}	207	}
208		208
209	/*	209	/*
210	* Get other channel value	210	* Get other channel value
211	*/	211	*/
212	int16_t RC_getVariable(uint8_t varNum) {	212	int16_t RC_getVariable(uint8_t varNum) {
213	if (varNum < 4)	213	if (varNum < 4)
214	// 0th variable is 5th channel (1-based) etc.	214	// 0th variable is 5th channel (1-based) etc.
215	return RCChannel(varNum + CH_POTS) + POT_OFFSET;	215	return RCChannel(varNum + CH_POTS) + POT_OFFSET;
216	/*	216	/*
217	* Let's just say:	217	* Let's just say:
218	* The RC variable i is hardwired to channel i, i>=4	218	* The RC variable i is hardwired to channel i, i>=4
219	*/	219	*/
220	return PPM_in[varNum] + POT_OFFSET;	220	return PPM_in[varNum] + POT_OFFSET;
221	}	221	}
222		222
223	uint8_t RC_getSignalQuality(void) {	223	uint8_t RC_getSignalQuality(void) {
224	if (RCQuality >= 160)	224	if (RCQuality >= 160)
225	return SIGNAL_GOOD;	225	return SIGNAL_GOOD;
226	if (RCQuality >= 140)	226	if (RCQuality >= 140)
227	return SIGNAL_OK;	227	return SIGNAL_OK;
228	if (RCQuality >= 120)	228	if (RCQuality >= 120)
229	return SIGNAL_BAD;	229	return SIGNAL_BAD;
230	return SIGNAL_LOST;	230	return SIGNAL_LOST;
231	}	231	}
232		232
233	/*	233	/*
234	* To should fired only when the right stick is in the center position.	234	* To should fired only when the right stick is in the center position.
235	* This will cause the value of pitch and roll stick to be adjusted	235	* This will cause the value of pitch and roll stick to be adjusted
236	* to zero (not just to near zero, as per the assumption in rc.c	236	* to zero (not just to near zero, as per the assumption in rc.c
237	* about the rc signal. I had values about 50..70 with a Futaba	237	* about the rc signal. I had values about 50..70 with a Futaba
238	* R617 receiver.) This calibration is not strictly necessary, but	238	* R617 receiver.) This calibration is not strictly necessary, but
239	* for control logic that depends on the exact (non)center position	239	* for control logic that depends on the exact (non)center position
240	* of a stick, it may be useful.	240	* of a stick, it may be useful.
241	*/	241	*/
242	void RC_calibrate(void) {	242	void RC_calibrate(void) {
243	// Do nothing.	243	// Do nothing.
244	}	244	}
245		245