WebSVN - FlightCtrl - Diff - Rev 2102 and 2103 - /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 commandTimer = 0;
 
uint8_t lastFlightMode = FLIGHT_MODE_NONE;
 
/***************************************************************
 *  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 << CS10) | (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 = 0, 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.52 ms < signal < 25.6 ms)
  if ((signal > 1100) && (signal < 8000)) {
    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.1984 ms)
      // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
      if ((signal > 250) && (signal < 687)) {
        // shift signal to zero symmetric range  -154 to 159
        signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
        // check for stable signal
        if (abs(signal - PPM_in[index]) < 6) {
          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_THRESHOLD 85
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
#define RC_SCALING 4
 
uint8_t getControlModeSwitch(void) {
        int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
        uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
                (channel > 256*2/3 ? 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 <= -140) { // <= 900 us
                if (commandTimer == COMMAND_TIMER) {
                        lastRCCommand = COMMAND_GYROCAL;
                }
                if (commandTimer <= COMMAND_TIMER) {
                        commandTimer++;
                }
        } else {
          commandTimer = 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) {
  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;
 
    uint8_t command = COMMAND_NONE; //RC_getStickCommand();
    if (lastRCCommand == command) {
      // Keep timer from overrunning.
      if (commandTimer < COMMAND_TIMER)
        commandTimer++;
    } else {
      // There was a change.
      lastRCCommand = command;
      commandTimer = 0;
    }
  } // 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) + POT_OFFSET;
  /*
   * Let's just say:
   * The RC variable i is hardwired to channel i, i>=4
   */
  return PPM_in[varNum] + POT_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;
}
 
/*
 * To should fired only when the right stick is in the center position.
 * This will cause the value of pitch and roll stick to be adjusted
 * to zero (not just to near zero, as per the assumption in rc.c
 * about the rc signal. I had values about 50..70 with a Futaba
 * R617 receiver.) This calibration is not strictly necessary, but
 * for control logic that depends on the exact (non)center position
 * of a stick, it may be useful.
 */
void RC_calibrate(void) {
  // Do nothing.
}
 

Rev 2102	Rev 2103
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 commandTimer = 0;	16	uint8_t commandTimer = 0;
17		17
18	uint8_t lastFlightMode = FLIGHT_MODE_NONE;	18	uint8_t lastFlightMode = FLIGHT_MODE_NONE;
19		19
20	/***************************************************************	20	/***************************************************************
21	* 16bit timer 1 is used to decode the PPM-Signal	21	* 16bit timer 1 is used to decode the PPM-Signal
22	***************************************************************/	22	***************************************************************/
23	void RC_Init(void) {	23	void RC_Init(void) {
24	uint8_t sreg = SREG;	24	uint8_t sreg = SREG;
25		25
26	// disable all interrupts before reconfiguration	26	// disable all interrupts before reconfiguration
27	cli();	27	cli();
28		28
29	// 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
30	DDRD &= ~(1<<6);	30	DDRD &= ~(1<<6);
31	PORTD \|= (1<<PORTD6);	31	PORTD \|= (1<<PORTD6);
32		32
33	// 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)
34	// set as output	34	// set as output
35	DDRD \|= (1<<DDD5) \| (1<<DDD4) \| (1<<DDD3);	35	DDRD \|= (1<<DDD5) \| (1<<DDD4) \| (1<<DDD3);
36	// low level	36	// low level
37	PORTD &= ~((1<<PORTD5) \| (1<<PORTD4) \| (1<<PORTD3));	37	PORTD &= ~((1<<PORTD5) \| (1<<PORTD4) \| (1<<PORTD3));
38		38
39	// PD3 can't be used if 2nd UART is activated	39	// PD3 can't be used if 2nd UART is activated
40	// because TXD1 is at that port	40	// because TXD1 is at that port
41	if (CPUType != ATMEGA644P) {	41	if (CPUType != ATMEGA644P) {
42	DDRD \|= (1<<PORTD3);	42	DDRD \|= (1<<PORTD3);
43	PORTD &= ~(1<<PORTD3);	43	PORTD &= ~(1<<PORTD3);
44	}	44	}
45		45
46	// Timer/Counter1 Control Register A, B, C	46	// Timer/Counter1 Control Register A, B, C
47		47
48	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)	48	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
49	// 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)
50	// 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)
51	// Enable input capture noise cancler (bit: ICNC1=1)	51	// Enable input capture noise cancler (bit: ICNC1=1)
52	// 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),
53	// 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
54	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.	54	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
55	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));
56	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));	56	TCCR1B &= ~((1 << WGM13) \| (1 << WGM12) \| (1 << CS12));
57	TCCR1B \|= (1 << CS11) \| (1 << CS10) \| (1 << ICES1) \| (1 << ICNC1);	57	TCCR1B \|= (1 << CS11) \| (1 << CS10) \| (1 << ICES1) \| (1 << ICNC1);
58	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));	58	TCCR1C &= ~((1 << FOC1A) \| (1 << FOC1B));
59		59
60	// Timer/Counter1 Interrupt Mask Register	60	// Timer/Counter1 Interrupt Mask Register
61	// Enable Input Capture Interrupt (bit: ICIE1=1)	61	// Enable Input Capture Interrupt (bit: ICIE1=1)
62	// 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)
63	// Enable Overflow Interrupt (bit: TOIE1=0)	63	// Enable Overflow Interrupt (bit: TOIE1=0)
64	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));	64	TIMSK1 &= ~((1<<OCIE1B) \| (1<<OCIE1A) \| (1<<TOIE1));
65	TIMSK1 \|= (1<<ICIE1);	65	TIMSK1 \|= (1<<ICIE1);
66		66
67	RCQuality = 0;	67	RCQuality = 0;
68		68
69	SREG = sreg;	69	SREG = sreg;
70	}	70	}
71		71
72	/********************************************************************/	72	/********************************************************************/
73	/* Every time a positive edge is detected at PD6 */	73	/* Every time a positive edge is detected at PD6 */
74	/********************************************************************/	74	/********************************************************************/
75	/* t-Frame	75	/* t-Frame
76	<----------------------------------------------------------------------->	76	<----------------------------------------------------------------------->
77	____ ______ _____ ________ ______ sync gap ____	77	____ ______ _____ ________ ______ sync gap ____
78	\| \| \| \| \| \| \| \| \| \| \|	78	\| \| \| \| \| \| \| \| \| \| \|
79	\| \| \| \| \| \| \| \| \| \| \|	79	\| \| \| \| \| \| \| \| \| \| \|
80	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	80	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
81	<-----><-------><------><----------- <------> <---	81	<-----><-------><------><----------- <------> <---
82	t0 t1 t2 t4 tn t0	82	t0 t1 t2 t4 tn t0
83		83
84	The PPM-Frame length is 22.5 ms.	84	The PPM-Frame length is 22.5 ms.
85	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.
86	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.
87	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.
88	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.
89	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.
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 remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
91	the syncronization gap.	91	the syncronization gap.
92	*/	92	*/
93	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
94	int16_t signal = 0, tmp;	94	int16_t signal = 0, tmp;
95	static int16_t index;	95	static int16_t index;
96	static uint16_t oldICR1 = 0;	96	static uint16_t oldICR1 = 0;
97		97
98	// 16bit Input Capture Register ICR1 contains the timer value TCNT1	98	// 16bit Input Capture Register ICR1 contains the timer value TCNT1
99	// at the time the edge was detected	99	// at the time the edge was detected
100		100
101	// 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
102	// 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
103	// implicit handles a timer overflow 65535 -> 0 the right way.	103	// implicit handles a timer overflow 65535 -> 0 the right way.
104	signal = (uint16_t) ICR1 - oldICR1;	104	signal = (uint16_t) ICR1 - oldICR1;
105	oldICR1 = ICR1;	105	oldICR1 = ICR1;
106		106
107	//sync gap? (3.52 ms < signal < 25.6 ms)	107	//sync gap? (3.52 ms < signal < 25.6 ms)
108	if ((signal > 1100) && (signal < 8000)) {	108	if ((signal > 1100) && (signal < 8000)) {
109	index = 0;	109	index = 0;
110	} else { // within the PPM frame	110	} else { // within the PPM frame
111	if (index < MAX_CHANNELS) { // PPM24 supports 12 channels	111	if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
112	// check for valid signal length (0.8 ms < signal < 2.1984 ms)	112	// check for valid signal length (0.8 ms < signal < 2.1984 ms)
113	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625	113	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
114	if ((signal > 250) && (signal < 687)) {	114	if ((signal > 250) && (signal < 687)) {
115	// shift signal to zero symmetric range -154 to 159	115	// shift signal to zero symmetric range -154 to 159
116	signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)	116	signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
117	// check for stable signal	117	// check for stable signal
118	if (abs(signal - PPM_in[index]) < 6) {	118	if (abs(signal - PPM_in[index]) < 6) {
119	if (RCQuality < 200)	119	if (RCQuality < 200)
120	RCQuality += 10;	120	RCQuality += 10;
121	else	121	else
122	RCQuality = 200;	122	RCQuality = 200;
123	}	123	}
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 is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
125	// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {	125	// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
126	// 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.
127	if (signal >= -1 && signal <= 1) {	127	if (signal >= -1 && signal <= 1) {
128	tmp = 0;	128	tmp = 0;
129	//} else {	129	//} else {
130	// tmp = PPM_in[index];	130	// tmp = PPM_in[index];
131	// }	131	// }
132	} else	132	} else
133	tmp = signal;	133	tmp = signal;
134	PPM_in[index] = tmp; // update channel value	134	PPM_in[index] = tmp; // update channel value
135	}	135	}
136	index++; // next channel	136	index++; // next channel
137	// 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
138	// 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
139	// channels are usually available at the receiver anyway.	139	// channels are usually available at the receiver anyway.
140	// if(index == 5) J3HIGH; else J3LOW;	140	// if(index == 5) J3HIGH; else J3LOW;
141	// if(index == 6) J4HIGH; else J4LOW;	141	// if(index == 6) J4HIGH; else J4LOW;
142	// if(CPUType != ATMEGA644P) // not used as TXD1	142	// if(CPUType != ATMEGA644P) // not used as TXD1
143	// {	143	// {
144	// if(index == 7) J5HIGH; else J5LOW;	144	// if(index == 7) J5HIGH; else J5LOW;
145	// }	145	// }
146	}	146	}
147	}	147	}
148	}	148	}
149		149
150	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]	150	#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
151	#define COMMAND_THRESHOLD 85	151	#define COMMAND_THRESHOLD 85
152	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE	152	#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
153	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW	153	#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
154		154
155	#define RC_SCALING 4	155	#define RC_SCALING 4
156		156
157	uint8_t getControlModeSwitch() {	157	uint8_t getControlModeSwitch(void) {
158	int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;	158	int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
159	uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :	159	uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
160	(channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);	160	(channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
161	return flightMode;	161	return flightMode;
162	}	162	}
163		163
164	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.	164	// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
165	// Maybe simply: Very very low throttle.	165	// Maybe simply: Very very low throttle.
166	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).	166	// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
167	// mode switched: CHMOD	167	// mode switched: CHMOD
168		168
169	uint8_t RC_getCommand(void) {	169	uint8_t RC_getCommand(void) {
170	uint8_t flightMode = getControlModeSwitch();	170	uint8_t flightMode = getControlModeSwitch();
171		171
172	if (lastFlightMode != flightMode) {	172	if (lastFlightMode != flightMode) {
173	lastFlightMode = flightMode;	173	lastFlightMode = flightMode;
174	lastRCCommand = COMMAND_CHMOD;	174	lastRCCommand = COMMAND_CHMOD;
175	return lastRCCommand;	175	return lastRCCommand;
176	}	176	}
177		177
178	int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;	178	int16_t channel = RCChannel(CH_THROTTLE);
179	if (channel <= -32) { // <= 900 us	179	if (channel <= -140) { // <= 900 us
180	if (commandTimer == COMMAND_TIMER) {	180	if (commandTimer == COMMAND_TIMER) {
181	lastRCCommand = COMMAND_GYROCAL;	181	lastRCCommand = COMMAND_GYROCAL;
182	}	182	}
183	if (commandTimer <= COMMAND_TIMER) {	183	if (commandTimer <= COMMAND_TIMER) {
184	commandTimer++;	184	commandTimer++;
185	}	185	}
-		186	} else {
186	} else commandTimer = 0;	187	commandTimer = 0;
-		188	lastRCCommand = COMMAND_NONE;
-		189	}
187	return lastRCCommand;	190	return lastRCCommand;
188	}	191	}
189		192
190	uint8_t RC_getArgument(void) {	193	uint8_t RC_getArgument(void) {
191	return lastFlightMode;	194	return lastFlightMode;
192	}	195	}
193		196
194	/*	197	/*
195	* Get Pitch, Roll, Throttle, Yaw values	198	* Get Pitch, Roll, Throttle, Yaw values
196	*/	199	*/
197	void RC_periodicTaskAndPRTY(int16_t* PRTY) {	200	void RC_periodicTaskAndPRYT(int16_t* PRYT) {
198	if (RCQuality) {	201	if (RCQuality) {
199	RCQuality--;	202	RCQuality--;
200	PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING;	-
201	PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING;	-
202	PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING;	-
203	PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING;	-
204		203
-		204	debugOut.analog[20] = RCChannel(CH_ELEVATOR);
-		205	debugOut.analog[21] = RCChannel(CH_AILERONS);
205	debugOut.analog[16] = PRTY[CONTROL_ELEVATOR];	206	debugOut.analog[22] = RCChannel(CH_RUDDER);
-		207	debugOut.analog[23] = RCChannel(CH_THROTTLE);
-		208
-		209	PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING;
-		210	PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING;
-		211	PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING;
206	debugOut.analog[17] = PRTY[CONTROL_THROTTLE];	212	PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING;
207		213
208	uint8_t command = COMMAND_NONE; //RC_getStickCommand();	214	uint8_t command = COMMAND_NONE; //RC_getStickCommand();
209	if (lastRCCommand == command) {	215	if (lastRCCommand == command) {
210	// Keep timer from overrunning.	216	// Keep timer from overrunning.
211	if (commandTimer < COMMAND_TIMER)	217	if (commandTimer < COMMAND_TIMER)
212	commandTimer++;	218	commandTimer++;
213	} else {	219	} else {
214	// There was a change.	220	// There was a change.
215	lastRCCommand = command;	221	lastRCCommand = command;
216	commandTimer = 0;	222	commandTimer = 0;
217	}	223	}
218	} // if RCQuality is no good, we just do nothing.	224	} // if RCQuality is no good, we just do nothing.
219	}	225	}
220		226
221	/*	227	/*
222	* Get other channel value	228	* Get other channel value
223	*/	229	*/
224	int16_t RC_getVariable(uint8_t varNum) {	230	int16_t RC_getVariable(uint8_t varNum) {
225	if (varNum < 4)	231	if (varNum < 4)
226	// 0th variable is 5th channel (1-based) etc.	232	// 0th variable is 5th channel (1-based) etc.
227	return RCChannel(varNum + CH_POTS) + POT_OFFSET;	233	return RCChannel(varNum + CH_POTS) + POT_OFFSET;
228	/*	234	/*
229	* Let's just say:	235	* Let's just say:
230	* The RC variable i is hardwired to channel i, i>=4	236	* The RC variable i is hardwired to channel i, i>=4
231	*/	237	*/
232	return PPM_in[varNum] + POT_OFFSET;	238	return PPM_in[varNum] + POT_OFFSET;
233	}	239	}
234		240
235	uint8_t RC_getSignalQuality(void) {	241	uint8_t RC_getSignalQuality(void) {
236	if (RCQuality >= 160)	242	if (RCQuality >= 160)
237	return SIGNAL_GOOD;	243	return SIGNAL_GOOD;
238	if (RCQuality >= 140)	244	if (RCQuality >= 140)
239	return SIGNAL_OK;	245	return SIGNAL_OK;
240	if (RCQuality >= 120)	246	if (RCQuality >= 120)
241	return SIGNAL_BAD;	247	return SIGNAL_BAD;
242	return SIGNAL_LOST;	248	return SIGNAL_LOST;
243	}	249	}
244		250
245	/*	251	/*
246	* To should fired only when the right stick is in the center position.	252	* To should fired only when the right stick is in the center position.
247	* This will cause the value of pitch and roll stick to be adjusted	253	* This will cause the value of pitch and roll stick to be adjusted
248	* to zero (not just to near zero, as per the assumption in rc.c	254	* to zero (not just to near zero, as per the assumption in rc.c
249	* about the rc signal. I had values about 50..70 with a Futaba	255	* about the rc signal. I had values about 50..70 with a Futaba
250	* R617 receiver.) This calibration is not strictly necessary, but	256	* R617 receiver.) This calibration is not strictly necessary, but
251	* for control logic that depends on the exact (non)center position	257	* for control logic that depends on the exact (non)center position
252	* of a stick, it may be useful.	258	* of a stick, it may be useful.
253	*/	259	*/
254	void RC_calibrate(void) {	260	void RC_calibrate(void) {
255	// Do nothing.	261	// Do nothing.
256	}	262	}
257		263

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/rc.c - Rev 2102 → 2103