WebSVN - FlightCtrl - Diff - Rev 2135 and 2136 - /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 = 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
  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 2135	Rev 2136
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	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		75
76	RCQuality = 0;	76	RCQuality = 0;
77		77
78	SREG = sreg;	78	SREG = sreg;
79	}	79	}
80		80
81	/*	81	/*
82	* This new and much faster interrupt handler should reduce servo jolts.	82	* This new and much faster interrupt handler should reduce servo jolts.
83	*/	83	*/
84	ISR(TIMER1_CAPT_vect) {	84	ISR(TIMER1_CAPT_vect) {
85	static uint16_t oldICR1 = 0;	85	static uint16_t oldICR1 = 0;
86	uint16_t signal = (uint16_t)ICR1 - oldICR1;	86	uint16_t signal = (uint16_t)ICR1 - oldICR1;
87	oldICR1 = ICR1;	87	oldICR1 = ICR1;
88	//sync gap? (3.5 ms < signal < 25.6 ms)	88	//sync gap? (3.5 ms < signal < 25.6 ms)
89	if (signal > TIME(3.5)) {	89	if (signal > TIME(3.5)) {
90	inBfrPnt = 0;	90	inBfrPnt = 0;
91	} else if (inBfrPnt<MAX_CHANNELS) {	91	} else if (inBfrPnt<MAX_CHANNELS) {
92	RC_buffer[inBfrPnt++] = signal;	92	RC_buffer[inBfrPnt++] = signal;
93	}	93	}
94	}	94	}
95		95
96	/********************************************************************/	96	/********************************************************************/
97	/* Every time a positive edge is detected at PD6 */	97	/* Every time a positive edge is detected at PD6 */
98	/********************************************************************/	98	/********************************************************************/
99	/* t-Frame	99	/* t-Frame
100	<----------------------------------------------------------------------->	100	<----------------------------------------------------------------------->
101	____ ______ _____ ________ ______ sync gap ____	101	____ ______ _____ ________ ______ sync gap ____
102	\| \| \| \| \| \| \| \| \| \| \|	102	\| \| \| \| \| \| \| \| \| \| \|
103	\| \| \| \| \| \| \| \| \| \| \|	103	\| \| \| \| \| \| \| \| \| \| \|
104	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	104	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
105	<-----><-------><------><----------- <------> <---	105	<-----><-------><------><----------- <------> <---
106	t0 t1 t2 t4 tn t0	106	t0 t1 t2 t4 tn t0
107		107
108	The PPM-Frame length is 22.5 ms.	108	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.	109	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.	110	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.	111	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.	112	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.	113	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	114	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.	115	the syncronization gap.
116	*/	116	*/
117	void RC_process(void) {	117	void RC_process(void) {
118	if (RCQuality) RCQuality--;	118	if (RCQuality) RCQuality--;
119	for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {	119	for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
120	uint16_t signal = RC_buffer[channel];	120	uint16_t signal = RC_buffer[channel];
121	if (signal != 0) {	121	if (signal != 0) {
122	RC_buffer[channel] = 0; // reset to flag value already used.	122	RC_buffer[channel] = 0; // reset to flag value already used.
123	if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {	123	if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
124	signal -= (TIME(1.5) - 128 + channelMap.HWTrim);	124	signal -= (TIME(1.5) - 128 + channelMap.HWTrim);
125	if (abs(signal - PPM_in[channel]) < TIME(0.05)) {	125	if (abs(signal - PPM_in[channel]) < TIME(0.05)) {
126	// With 7 channels and 50 frames/sec, we get 350 channel values/sec.	126	// With 7 channels and 50 frames/sec, we get 350 channel values/sec.
127	if (RCQuality < 200)	127	if (RCQuality < 200)
128	RCQuality += 2;	128	RCQuality += 2;
129	}	129	}
130	PPM_in[channel] = signal;	130	PPM_in[channel] = signal;
131	}	131	}
132	}	132	}
133	}	133	}
134	}	134	}
135		135
136	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]	136	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
137		137
138	uint8_t getControlModeSwitch(void) {	138	uint8_t getControlModeSwitch(void) {
139	int16_t channel = RCChannel(CH_MODESWITCH);	139	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);	140	uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
141	return flightMode;	141	return flightMode;
142	}	142	}
143		143
144	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.	144	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
145	// Maybe simply: Very very low throttle.	145	// Maybe simply: Very very low throttle.
146	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).	146	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
147	// mode switched: CHMOD	147	// mode switched: CHMOD
148		148
149	uint8_t RC_getCommand(void) {	149	uint8_t RC_getCommand(void) {
150	uint8_t flightMode = getControlModeSwitch();	150	uint8_t flightMode = getControlModeSwitch();
151		151
152	if (lastFlightMode != flightMode) {	152	if (lastFlightMode != flightMode) {
153	lastFlightMode = flightMode;	153	lastFlightMode = flightMode;
154	lastRCCommand = COMMAND_CHMOD;	154	lastRCCommand = COMMAND_CHMOD;
155	return lastRCCommand;	155	return lastRCCommand;
156	}	156	}
157		157
158	int16_t channel = RCChannel(CH_THROTTLE);	158	int16_t channel = RCChannel(CH_THROTTLE);
159		159
160	if (channel <= -TIME(0.55)) {	160	if (channel <= -TIME(0.55)) {
161	int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);	161	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.	162	if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
163	lastRCCommand = COMMAND_GYROCAL;	163	lastRCCommand = COMMAND_GYROCAL;
164	else	164	else
165	lastRCCommand = COMMAND_RCCAL;	165	lastRCCommand = COMMAND_RCCAL;
166	} else {	166	} else {
167	lastRCCommand = COMMAND_NONE;	167	lastRCCommand = COMMAND_NONE;
168	}	168	}
169	return lastRCCommand;	169	return lastRCCommand;
170	}	170	}
171		171
172	uint8_t RC_getArgument(void) {	172	uint8_t RC_getArgument(void) {
173	return lastFlightMode;	173	return lastFlightMode;
174	}	174	}
175		175
176	/*	176	/*
177	* Get Pitch, Roll, Throttle, Yaw values	177	* Get Pitch, Roll, Throttle, Yaw values
178	*/	178	*/
179	void RC_periodicTaskAndPRYT(int16_t* PRYT) {	179	void RC_periodicTaskAndPRYT(int16_t* PRYT) {
180	RC_process();	180	RC_process();
181		181
182	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];	182	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];
183	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];	183	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];
184	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];	184	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];
185	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!	185	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!
186		186
187	debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];	187	debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
188	debugOut.analog[21] = PRYT[CONTROL_AILERONS];	188	debugOut.analog[21] = PRYT[CONTROL_AILERONS];
189	debugOut.analog[22] = PRYT[CONTROL_RUDDER];	189	debugOut.analog[22] = PRYT[CONTROL_RUDDER];
190	debugOut.analog[23] = PRYT[CONTROL_THROTTLE];	190	debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
191	}	191	}
192		192
193	/*	193	/*
194	* Get other channel value	194	* Get other channel value
195	*/	195	*/
196	int16_t RC_getVariable(uint8_t varNum) {	196	int16_t RC_getVariable(uint8_t varNum) {
197	if (varNum < 4)	197	if (varNum < 4)
198	// 0th variable is 5th channel (1-based) etc.	198	// 0th variable is 5th channel (1-based) etc.
199	return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset;	199	return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset;
200	/*	200	/*
201	* Let's just say:	201	* Let's just say:
202	* The RC variable i is hardwired to channel i, i>=4	202	* The RC variable i is hardwired to channel i, i>=4
203	*/	203	*/
204	return (PPM_in[varNum] >> 3) + channelMap.variableOffset;	204	return (PPM_in[varNum] >> 3) + channelMap.variableOffset;
205	}	205	}
206		206
207	uint8_t RC_getSignalQuality(void) {	207	uint8_t RC_getSignalQuality(void) {
208	if (RCQuality >= 160)	208	if (RCQuality >= 160)
209	return SIGNAL_GOOD;	209	return SIGNAL_GOOD;
210	if (RCQuality >= 140)	210	if (RCQuality >= 140)
211	return SIGNAL_OK;	211	return SIGNAL_OK;
212	if (RCQuality >= 120)	212	if (RCQuality >= 120)
213	return SIGNAL_BAD;	213	return SIGNAL_BAD;
214	return SIGNAL_LOST;	214	return SIGNAL_LOST;
215	}	215	}
216		216
217	void RC_calibrate(void) {	217	void RC_calibrate(void) {
218	rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);	218	rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
219	rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);	219	rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
220	rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);	220	rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);
221	rcTrim.trim[CH_THROTTLE] = 0;	221	rcTrim.trim[CH_THROTTLE] = 0;
222	}	222	}
223		223
224	int16_t RC_getZeroThrottle(void) {	224	int16_t RC_getZeroThrottle(void) {
225	return TIME (-0.5);	225	return TIME(1.0f);
226	}	226	}
227		227
228	void RC_setZeroTrim(void) {	228	void RC_setZeroTrim(void) {
229	for (uint8_t i=0; i<MAX_CHANNELS; i++) {	229	for (uint8_t i=0; i<MAX_CHANNELS; i++) {
230	rcTrim.trim[i] = 0;	230	rcTrim.trim[i] = 0;
231	}	231	}
232	}	232	}
233		233

Subversion Repositories FlightCtrl

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