Subversion Repositories FlightCtrl

Rev

Rev 2124 | Rev 2136 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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