Subversion Repositories FlightCtrl

Rev

Rev 2107 | Rev 2110 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2107 Rev 2109
Line 50... Line 50...
50 
  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
 50 
  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
51 
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
 51 
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
52 
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
 52 
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
53 
  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
 53 
  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
54 
  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
 54 
  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
55 
  TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
 55 
  TCCR1B |= (1 << CS11) | (1 << ICES1) | (1 << ICNC1);
56 
  TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
 56 
  TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
Line 57... Line 57...
57 
 
 57 
 
58 
  // Timer/Counter1 Interrupt Mask Register
 58 
  // Timer/Counter1 Interrupt Mask Register
59 
  // Enable Input Capture Interrupt (bit: ICIE1=1)
 59 
  // Enable Input Capture Interrupt (bit: ICIE1=1)
Line 100... Line 100...
100 
  // calculatiing the difference of the two uint16_t and converting the result to an int16_t
 100 
  // calculatiing the difference of the two uint16_t and converting the result to an int16_t
101 
  // implicit handles a timer overflow 65535 -> 0 the right way.
 101 
  // implicit handles a timer overflow 65535 -> 0 the right way.
102 
  signal = (uint16_t) ICR1 - oldICR1;
 102 
  signal = (uint16_t) ICR1 - oldICR1;
103 
  oldICR1 = ICR1;
 103 
  oldICR1 = ICR1;
Line 104... Line 104...
104 
 
 104 
 
105 
  //sync gap? (3.52 ms < signal < 25.6 ms)
 105 
  //sync gap? (3.5 ms < signal < 25.6 ms)
106 
  if ((signal > 1100) && (signal < 8000)) {
 106 
  if (signal > 8750) {
107 
    index = 0;
 107 
    index = 0;
108 
  } else { // within the PPM frame
 108 
  } else { // within the PPM frame
109 
    if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
 109 
    if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
110 
      // check for valid signal length (0.8 ms < signal < 2.1984 ms)
 - 
 
111 
      // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
 110 
      // check for valid signal length (0.8 ms < signal < 2.2 ms)
112 
      if ((signal > 250) && (signal < 687)) {
 111 
      if ((signal >= 2000) && (signal < 5500)) {
- 
 
 112 
        // shift signal to zero symmetric range  -154 to 159
113 
        // shift signal to zero symmetric range  -154 to 159
 113 
        //signal -= 3750; // theoretical value
114 
        signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
 114 
        signal -= (3750+56); // best value with my Futaba in zero trim.
115 
        // check for stable signal
 115 
        // check for stable signal
116 
        if (abs(signal - PPM_in[index]) < 6) {
 116 
        if (abs(signal - PPM_in[index]) < 50) {
117 
          if (RCQuality < 200)
 117 
          if (RCQuality < 200)
118 
            RCQuality += 10;
 118 
            RCQuality += 10;
119 
          else
 119 
          else
120 
            RCQuality = 200;
 120 
            RCQuality = 200;
Line 144... Line 144...
144 
    }
 144 
    }
145 
  }
 145 
  }
146 
}
 146 
}
Line 147... Line 147...
147 
 
 147 
 
148 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
 - 
 
149 
#define COMMAND_THRESHOLD 85
 148 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
150 
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
 149 
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
Line 151... Line 150...
151 
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 150 
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
Line 152... Line 151...
152 
 
 151 
 
153 
#define RC_SCALING 4
 152 
#define RC_SCALING 2
154 
 
 153 
 
155 
uint8_t getControlModeSwitch(void) {
 - 
 
156 
        int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
 154 
uint8_t getControlModeSwitch(void) {
157 
        uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
 155 
        int16_t channel = RCChannel(CH_MODESWITCH);
Line 158... Line 156...
158 
                (channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
 156 
        uint8_t flightMode = channel < -330 ? FLIGHT_MODE_MANUAL : (channel > 330 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
159 
        return flightMode;
 157 
        return flightMode;
Line 173... Line 171...
173 
                return lastRCCommand;
 171 
                return lastRCCommand;
174 
        }
 172 
        }
Line 175... Line 173...
175 
 
 173 
 
Line 176... Line 174...
176 
        int16_t channel = RCChannel(CH_THROTTLE);
 174 
        int16_t channel = RCChannel(CH_THROTTLE);
177 
 
 175 
 
178 
        if (channel <= -140) { // <= 900 us
 176 
        if (channel <= -1400) {
179 
                lastRCCommand = COMMAND_GYROCAL;
 177 
          lastRCCommand = COMMAND_GYROCAL;
180 
        } else {
 178 
        } else {
181 
          lastRCCommand = COMMAND_NONE;
 179 
          lastRCCommand = COMMAND_NONE;
182 
        }
 180 
        }
Line 197... Line 195...
197 
    debugOut.analog[20] = RCChannel(CH_ELEVATOR);
 195 
    debugOut.analog[20] = RCChannel(CH_ELEVATOR);
198 
    debugOut.analog[21] = RCChannel(CH_AILERONS);
 196 
    debugOut.analog[21] = RCChannel(CH_AILERONS);
199 
    debugOut.analog[22] = RCChannel(CH_RUDDER);
 197 
    debugOut.analog[22] = RCChannel(CH_RUDDER);
200 
    debugOut.analog[23] = RCChannel(CH_THROTTLE);
 198 
    debugOut.analog[23] = RCChannel(CH_THROTTLE);
Line 201... Line 199...
201 
 
 199 
 
202 
    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR) * RC_SCALING;
 200 
    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR) / RC_SCALING;
203 
    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS) * RC_SCALING;
 201 
    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS) / RC_SCALING;
204 
    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)   * RC_SCALING;
 202 
    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)   / RC_SCALING;
205 
    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE) * RC_SCALING;
 203 
    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE) / RC_SCALING;
206 
  } // if RCQuality is no good, we just do nothing.
 204 
  } // if RCQuality is no good, we just do nothing.
Line 207... Line 205...
207 
}
 205 
}
208 
 
 206 
 
209 
/*
 207 
/*
210 
 * Get other channel value
 208 
 * Get other channel value
211 
 */
 209 
 */
212 
int16_t RC_getVariable(uint8_t varNum) {
 210 
int16_t RC_getVariable(uint8_t varNum) {
213 
  if (varNum < 4)
 211 
  if (varNum < 4)
214 
    // 0th variable is 5th channel (1-based) etc.
 212 
    // 0th variable is 5th channel (1-based) etc.
215 
    return RCChannel(varNum + CH_POTS) + POT_OFFSET;
 213 
    return (RCChannel(varNum + CH_POTS) >> 3) + POT_OFFSET;
216 
  /*
 214 
  /*
217 
   * Let's just say:
 215 
   * Let's just say:
218 
   * The RC variable i is hardwired to channel i, i>=4
 216 
   * The RC variable i is hardwired to channel i, i>=4
219 
   */
 217 
   */
Line 220... Line 218...
220 
  return PPM_in[varNum] + POT_OFFSET;
 218 
  return (PPM_in[varNum] >> 3) + POT_OFFSET;
221 
}
 219 
}
222 
 
 220