WebSVN - FlightCtrl - Diff - Rev 2110 and 2115 - /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;
 
#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
  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 << 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;
        } 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) >> 3) + VARIABLE_OFFSET;
  /*
   * Let's just say:
   * The RC variable i is hardwired to channel i, i>=4
   */
  return (PPM_in[varNum] >> 3) + 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;
}
 
 
 
 
 
 
 
 
 
 
void RC_calibrate(void) {
  // Do nothing.
}
 
int16_t RC_getZeroThrottle() {
        return TIME (-0.5);
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/rc.c - Rev 2110 → 2115