WebSVN - FlightCtrl - Diff - Rev 2141 and 2142 - /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, we get 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)) {
          debugOut.analog[17] = 1;
          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 {
          debugOut.analog[17] = 0;
          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 (1.0f);
}
 
void RC_setZeroTrim(void) {
  for (uint8_t i=0; i<MAX_CHANNELS; i++) {
    rcTrim.trim[i] = 0;
  }
}
 

Rev 2141	Rev 2142
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, weget 350 channel values/sec.	106	// With 7 channels and 50 frames/sec, we get 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	debugOut.analog[17] = 1;
141	int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);	142	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.	143	if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
143	lastRCCommand = COMMAND_GYROCAL;	144	lastRCCommand = COMMAND_GYROCAL;
144	else	145	else
145	lastRCCommand = COMMAND_RCCAL;	146	lastRCCommand = COMMAND_RCCAL;
146	} else {	147	} else {
-		148	debugOut.analog[17] = 0;
147	lastRCCommand = COMMAND_NONE;	149	lastRCCommand = COMMAND_NONE;
148	}	150	}
149	return lastRCCommand;	151	return lastRCCommand;
150	}	152	}
151		153
152	uint8_t RC_getArgument(void) {	154	uint8_t RC_getArgument(void) {
153	return lastFlightMode;	155	return lastFlightMode;
154	}	156	}
155		157
156	/*	158	/*
157	* Get Pitch, Roll, Throttle, Yaw values	159	* Get Pitch, Roll, Throttle, Yaw values
158	*/	160	*/
159	void RC_periodicTaskAndPRYT(int16_t* PRYT) {	161	void RC_periodicTaskAndPRYT(int16_t* PRYT) {
160	RC_process();	162	RC_process();
161		163
162	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];	164	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];
163	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];	165	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];
164	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];	166	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];
165	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!	167	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!
166		168
167	debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];	169	debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
168	debugOut.analog[21] = PRYT[CONTROL_AILERONS];	170	debugOut.analog[21] = PRYT[CONTROL_AILERONS];
169	debugOut.analog[22] = PRYT[CONTROL_RUDDER];	171	debugOut.analog[22] = PRYT[CONTROL_RUDDER];
170	debugOut.analog[23] = PRYT[CONTROL_THROTTLE];	172	debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
171	}	173	}
172		174
173	/*	175	/*
174	* Get other channel value	176	* Get other channel value
175	*/	177	*/
176	int16_t RC_getVariable(uint8_t varNum) {	178	int16_t RC_getVariable(uint8_t varNum) {
177	if (varNum < 4) {	179	if (varNum < 4) {
178	// 0th variable is 5th channel (1-based) etc.	180	// 0th variable is 5th channel (1-based) etc.
179	int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset;	181	int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset;
180	return result;	182	return result;
181	}	183	}
182	/*	184	/*
183	* Let's just say:	185	* Let's just say:
184	* The RC variable i is hardwired to channel i, i>=4	186	* The RC variable i is hardwired to channel i, i>=4
185	*/	187	*/
186	return (PPM_in[varNum] / 6) + channelMap.variableOffset;	188	return (PPM_in[varNum] / 6) + channelMap.variableOffset;
187	}	189	}
188		190
189	uint8_t RC_getSignalQuality(void) {	191	uint8_t RC_getSignalQuality(void) {
190	if (RCQuality >= 160)	192	if (RCQuality >= 160)
191	return SIGNAL_GOOD;	193	return SIGNAL_GOOD;
192	if (RCQuality >= 140)	194	if (RCQuality >= 140)
193	return SIGNAL_OK;	195	return SIGNAL_OK;
194	if (RCQuality >= 120)	196	if (RCQuality >= 120)
195	return SIGNAL_BAD;	197	return SIGNAL_BAD;
196	return SIGNAL_LOST;	198	return SIGNAL_LOST;
197	}	199	}
198		200
199	void RC_calibrate(void) {	201	void RC_calibrate(void) {
200	rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);	202	rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
201	rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);	203	rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
202	rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);	204	rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);
203	rcTrim.trim[CH_THROTTLE] = 0;	205	rcTrim.trim[CH_THROTTLE] = 0;
204	}	206	}
205		207
206	int16_t RC_getZeroThrottle(void) {	208	int16_t RC_getZeroThrottle(void) {
207	return TIME (0.95f);	209	return TIME (1.0f);
208	}	210	}
209		211
210	void RC_setZeroTrim(void) {	212	void RC_setZeroTrim(void) {
211	for (uint8_t i=0; i<MAX_CHANNELS; i++) {	213	for (uint8_t i=0; i<MAX_CHANNELS; i++) {
212	rcTrim.trim[i] = 0;	214	rcTrim.trim[i] = 0;
213	}	215	}
214	}	216	}
215		217

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c - Rev 2141 → 2142