WebSVN - FlightCtrl - Diff - Rev 2136 and 2141 - /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 uint16_t RC_buffer[MAX_CHANNELS];
volatile uint8_t inBfrPnt = 0;
 
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;
}
 
/*
 * 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_CHANNELS) {
        RC_buffer[inBfrPnt++] = signal;
  }
}
 
/********************************************************************/
/*         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_CHANNELS; 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) - 128 + channelMap.HWTrim);
        if (abs(signal - PPM_in[channel]) < TIME(0.05)) {
                // With 7 channels and 50 frames/sec, weget 350 channel values/sec.
          if (RCQuality < 200)
            RCQuality += 2;
        }
        PPM_in[channel] = signal;
      }
    }
  }
}
 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
 
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)) {
          int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);
          if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
                lastRCCommand = COMMAND_GYROCAL;
          else
                lastRCCommand = COMMAND_RCCAL;
        } 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) {
  RC_process();
 
  PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR)   - rcTrim.trim[CH_ELEVATOR];
  PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS)   - rcTrim.trim[CH_AILERONS];
  PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)     - rcTrim.trim[CH_RUDDER];
  PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);  // no trim on throttle!
 
  debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
  debugOut.analog[21] = PRYT[CONTROL_AILERONS];
  debugOut.analog[22] = PRYT[CONTROL_RUDDER];
  debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
}
 
/*
 * Get other channel value
 */
int16_t RC_getVariable(uint8_t varNum) {
  if (varNum < 4) {
    // 0th variable is 5th channel (1-based) etc.
    int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset;
 
    return result;
  }
  /*
   * Let's just say:
   * The RC variable i is hardwired to channel i, i>=4
   */
  return (PPM_in[varNum] / 6) + channelMap.variableOffset;
}
 
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) {
  rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
  rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
  rcTrim.trim[CH_RUDDER]   = RCChannel(CH_RUDDER);
  rcTrim.trim[CH_THROTTLE] = 0;
}
 
int16_t RC_getZeroThrottle(void) {
        return TIME (0.95f);
}
 
void RC_setZeroTrim(void) {
  for (uint8_t i=0; i<MAX_CHANNELS; i++) {
    rcTrim.trim[i] = 0;
  }
}
 

Rev 2136	Rev 2141
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 uint16_t RC_buffer[MAX_CHANNELS];	13	volatile uint16_t RC_buffer[MAX_CHANNELS];
14	volatile uint8_t inBfrPnt = 0;	14	volatile uint8_t inBfrPnt = 0;
15		15
16	volatile uint8_t RCQuality;	16	volatile uint8_t RCQuality;
17		17
18	uint8_t lastRCCommand = COMMAND_NONE;	18	uint8_t lastRCCommand = COMMAND_NONE;
19	uint8_t lastFlightMode = FLIGHT_MODE_NONE;	19	uint8_t lastFlightMode = FLIGHT_MODE_NONE;
20		20
21	#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))	21	#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))
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	DDRB &= ~(1<<0);	33	DDRB &= ~(1<<0);
34	PORTB \|= (1<<PORTB0);	34	PORTB \|= (1<<PORTB0);
35		35
36	// Timer/Counter1 Control Register A, B, C	36	// Timer/Counter1 Control Register A, B, C
37	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)	37	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
38	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)	38	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
39	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)	39	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
40	// Enable input capture noise cancler (bit: ICNC1=1)	40	// Enable input capture noise cancler (bit: ICNC1=1)
41	// Trigger on positive edge of the input capture pin (bit: ICES1=1),	41	// Trigger on positive edge of the input capture pin (bit: ICES1=1),
42	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s	42	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
43	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.	43	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
44	TCCR1A &= ~((1 << COM1A1) \| (1 << COM1A0) \| (1 << COM1B1) \| (1 << COM1B0) \| (1 << WGM11) \| (1 << WGM10));	44	TCCR1A &= ~((1 << COM1A1) \| (1 << COM1A0) \| (1 << COM1B1) \| (1 << COM1B0) \| (1 << WGM11) \| (1 << WGM10));
45	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));	45	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));
46	TCCR1B \|= (1 << CS11) \| (1 << ICES1) \| (1 << ICNC1);	46	TCCR1B \|= (1 << CS11) \| (1 << ICES1) \| (1 << ICNC1);
47	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));	47	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));
48		48
49	// Timer/Counter1 Interrupt Mask Register	49	// Timer/Counter1 Interrupt Mask Register
50	// Enable Input Capture Interrupt (bit: ICIE1=1)	50	// Enable Input Capture Interrupt (bit: ICIE1=1)
51	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)	51	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
52	// Enable Overflow Interrupt (bit: TOIE1=0)	52	// Enable Overflow Interrupt (bit: TOIE1=0)
53	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));	53	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));
54	TIMSK1 \|= (1<<ICIE1);	54	TIMSK1 \|= (1<<ICIE1);
55		55
56	RCQuality = 0;	56	RCQuality = 0;
57		57
58	SREG = sreg;	58	SREG = sreg;
59	}	59	}
60		60
61	/*	61	/*
62	* This new and much faster interrupt handler should reduce servo jolts.	62	* This new and much faster interrupt handler should reduce servo jolts.
63	*/	63	*/
64	ISR(TIMER1_CAPT_vect) {	64	ISR(TIMER1_CAPT_vect) {
65	static uint16_t oldICR1 = 0;	65	static uint16_t oldICR1 = 0;
66	uint16_t signal = (uint16_t)ICR1 - oldICR1;	66	uint16_t signal = (uint16_t)ICR1 - oldICR1;
67	oldICR1 = ICR1;	67	oldICR1 = ICR1;
68	//sync gap? (3.5 ms < signal < 25.6 ms)	68	//sync gap? (3.5 ms < signal < 25.6 ms)
69	if (signal > TIME(3.5)) {	69	if (signal > TIME(3.5)) {
70	inBfrPnt = 0;	70	inBfrPnt = 0;
71	} else if (inBfrPnt<MAX_CHANNELS) {	71	} else if (inBfrPnt<MAX_CHANNELS) {
72	RC_buffer[inBfrPnt++] = signal;	72	RC_buffer[inBfrPnt++] = signal;
73	}	73	}
74	}	74	}
75		75
76	/********************************************************************/	76	/********************************************************************/
77	/* Every time a positive edge is detected at PD6 */	77	/* Every time a positive edge is detected at PD6 */
78	/********************************************************************/	78	/********************************************************************/
79	/* t-Frame	79	/* t-Frame
80	<----------------------------------------------------------------------->	80	<----------------------------------------------------------------------->
81	____ ______ _____ ________ ______ sync gap ____	81	____ ______ _____ ________ ______ sync gap ____
82	\| \| \| \| \| \| \| \| \| \| \|	82	\| \| \| \| \| \| \| \| \| \| \|
83	\| \| \| \| \| \| \| \| \| \| \|	83	\| \| \| \| \| \| \| \| \| \| \|
84	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	84	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
85	<-----><-------><------><----------- <------> <---	85	<-----><-------><------><----------- <------> <---
86	t0 t1 t2 t4 tn t0	86	t0 t1 t2 t4 tn t0
87		87
88	The PPM-Frame length is 22.5 ms.	88	The PPM-Frame length is 22.5 ms.
89	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.	89	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
90	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.	90	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
91	The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.	91	The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.
92	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.	92	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
93	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.	93	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
94	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is	94	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
95	the syncronization gap.	95	the syncronization gap.
96	*/	96	*/
97	void RC_process(void) {	97	void RC_process(void) {
98	if (RCQuality) RCQuality--;	98	if (RCQuality) RCQuality--;
99	for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {	99	for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
100	uint16_t signal = RC_buffer[channel];	100	uint16_t signal = RC_buffer[channel];
101	if (signal != 0) {	101	if (signal != 0) {
102	RC_buffer[channel] = 0; // reset to flag value already used.	102	RC_buffer[channel] = 0; // reset to flag value already used.
103	if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {	103	if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
104	signal -= (TIME(1.5) - 128 + channelMap.HWTrim);	104	signal -= (TIME(1.5) - 128 + channelMap.HWTrim);
105	if (abs(signal - PPM_in[channel]) < TIME(0.05)) {	105	if (abs(signal - PPM_in[channel]) < TIME(0.05)) {
106	// With 7 channels and 50 frames/sec, we get 350 channel values/sec.	106	// With 7 channels and 50 frames/sec, weget 350 channel values/sec.
107	if (RCQuality < 200)	107	if (RCQuality < 200)
108	RCQuality += 2;	108	RCQuality += 2;
109	}	109	}
110	PPM_in[channel] = signal;	110	PPM_in[channel] = signal;
111	}	111	}
112	}	112	}
113	}	113	}
114	}	114	}
115		115
116	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]	116	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
117		117
118	uint8_t getControlModeSwitch(void) {	118	uint8_t getControlModeSwitch(void) {
119	int16_t channel = RCChannel(CH_MODESWITCH);	119	int16_t channel = RCChannel(CH_MODESWITCH);
120	uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);	120	uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
121	return flightMode;	121	return flightMode;
122	}	122	}
123		123
124	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.	124	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
125	// Maybe simply: Very very low throttle.	125	// Maybe simply: Very very low throttle.
126	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).	126	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
127	// mode switched: CHMOD	127	// mode switched: CHMOD
128		128
129	uint8_t RC_getCommand(void) {	129	uint8_t RC_getCommand(void) {
130	uint8_t flightMode = getControlModeSwitch();	130	uint8_t flightMode = getControlModeSwitch();
131		131
132	if (lastFlightMode != flightMode) {	132	if (lastFlightMode != flightMode) {
133	lastFlightMode = flightMode;	133	lastFlightMode = flightMode;
134	lastRCCommand = COMMAND_CHMOD;	134	lastRCCommand = COMMAND_CHMOD;
135	return lastRCCommand;	135	return lastRCCommand;
136	}	136	}
137		137
138	int16_t channel = RCChannel(CH_THROTTLE);	138	int16_t channel = RCChannel(CH_THROTTLE);
139		139
140	if (channel <= -TIME(0.55)) {	140	if (channel <= -TIME(0.55)) {
141	int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);	141	int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);
142	if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.	142	if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
143	lastRCCommand = COMMAND_GYROCAL;	143	lastRCCommand = COMMAND_GYROCAL;
144	else	144	else
145	lastRCCommand = COMMAND_RCCAL;	145	lastRCCommand = COMMAND_RCCAL;
146	} else {	146	} else {
147	lastRCCommand = COMMAND_NONE;	147	lastRCCommand = COMMAND_NONE;
148	}	148	}
149	return lastRCCommand;	149	return lastRCCommand;
150	}	150	}
151		151
152	uint8_t RC_getArgument(void) {	152	uint8_t RC_getArgument(void) {
153	return lastFlightMode;	153	return lastFlightMode;
154	}	154	}
155		155
156	/*	156	/*
157	* Get Pitch, Roll, Throttle, Yaw values	157	* Get Pitch, Roll, Throttle, Yaw values
158	*/	158	*/
159	void RC_periodicTaskAndPRYT(int16_t* PRYT) {	159	void RC_periodicTaskAndPRYT(int16_t* PRYT) {
160	RC_process();	160	RC_process();
161		161
162	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];	162	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];
163	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];	163	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];
164	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];	164	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];
165	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!	165	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!
166		166
167	debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];	167	debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
168	debugOut.analog[21] = PRYT[CONTROL_AILERONS];	168	debugOut.analog[21] = PRYT[CONTROL_AILERONS];
169	debugOut.analog[22] = PRYT[CONTROL_RUDDER];	169	debugOut.analog[22] = PRYT[CONTROL_RUDDER];
170	debugOut.analog[23] = PRYT[CONTROL_THROTTLE];	170	debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
171	}	171	}
172		172
173	/*	173	/*
174	* Get other channel value	174	* Get other channel value
175	*/	175	*/
176	int16_t RC_getVariable(uint8_t varNum) {	176	int16_t RC_getVariable(uint8_t varNum) {
177	if (varNum < 4) {	177	if (varNum < 4) {
178	// 0th variable is 5th channel (1-based) etc.	178	// 0th variable is 5th channel (1-based) etc.
179	int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset;	179	int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset;
180	if (varNum<2) debugOut.analog[18+varNum] = result;	-
181	return result;	180	return result;
182	}	181	}
183	/*	182	/*
184	* Let's just say:	183	* Let's just say:
185	* The RC variable i is hardwired to channel i, i>=4	184	* The RC variable i is hardwired to channel i, i>=4
186	*/	185	*/
187	return (PPM_in[varNum] >> 3) + channelMap.variableOffset;	186	return (PPM_in[varNum] / 6) + channelMap.variableOffset;
188	}	187	}
189		188
190	uint8_t RC_getSignalQuality(void) {	189	uint8_t RC_getSignalQuality(void) {
191	if (RCQuality >= 160)	190	if (RCQuality >= 160)
192	return SIGNAL_GOOD;	191	return SIGNAL_GOOD;
193	if (RCQuality >= 140)	192	if (RCQuality >= 140)
194	return SIGNAL_OK;	193	return SIGNAL_OK;
195	if (RCQuality >= 120)	194	if (RCQuality >= 120)
196	return SIGNAL_BAD;	195	return SIGNAL_BAD;
197	return SIGNAL_LOST;	196	return SIGNAL_LOST;
198	}	197	}
199		198
200	void RC_calibrate(void) {	199	void RC_calibrate(void) {
201	rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);	200	rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
202	rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);	201	rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
203	rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);	202	rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);
204	rcTrim.trim[CH_THROTTLE] = 0;	203	rcTrim.trim[CH_THROTTLE] = 0;
205	}	204	}
206		205
207	int16_t RC_getZeroThrottle(void) {	206	int16_t RC_getZeroThrottle(void) {
208	return TIME (1.0f);	207	return TIME (0.95f);
209	}	208	}
210		209
211	void RC_setZeroTrim(void) {	210	void RC_setZeroTrim(void) {
212	for (uint8_t i=0; i<MAX_CHANNELS; i++) {	211	for (uint8_t i=0; i<MAX_CHANNELS; i++) {
213	rcTrim.trim[i] = 0;	212	rcTrim.trim[i] = 0;
214	}	213	}
215	}	214	}
216		215

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c @ 2111 - Rev 2136 → 2141