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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/rc.c @ 2088 - Rev 2136 → 2173