Subversion Repositories FlightCtrl

Rev

Rev 1796 | Rev 1868 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1796 Rev 1821
Line 103... Line 103...
103 
  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
 103 
        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
104 
  // Enable input capture noise cancler (bit: ICNC1=1)
 104 
        // Enable input capture noise cancler (bit: ICNC1=1)
105 
  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
 105 
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
106 
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
 106 
        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
107 
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
 107 
        // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
108 
  TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
 108 
        TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0)
- 
 
 109 
                        | (1 << WGM11) | (1 << WGM10));
109 
  TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
 110 
        TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
110 
  TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
 111 
        TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
111 
  TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
 112 
        TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
Line 112... Line 113...
112 
 
 113 
 
Line 142... Line 143...
142 
                                 The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
 143 
 The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
143 
                                 The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
 144 
 The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
144 
                                 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
 145 
 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
145 
                                 the syncronization gap.
 146 
 the syncronization gap.
146 
*/
 147 
 */
- 
 
 148 
ISR(TIMER1_CAPT_vect)
147 
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
 149 
{ // typical rate of 1 ms to 2 ms
148 
  int16_t signal = 0, tmp;
 150 
        int16_t signal = 0, tmp;
149 
  static int16_t index;
 151 
        static int16_t index;
150 
  static uint16_t oldICR1 = 0;
 152 
        static uint16_t oldICR1 = 0;
Line 151... Line 153...
151 
 
 153 
 
Line 174... Line 176...
174 
      if((signal > 250) && (signal < 687)) {
 176 
                        if ((signal > 250) && (signal < 687)) {
175 
        // shift signal to zero symmetric range  -154 to 159
 177 
                                // shift signal to zero symmetric range  -154 to 159
176 
        signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
 178 
                                signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
177 
        // check for stable signal
 179 
                                // check for stable signal
178 
        if(abs(signal - PPM_in[index]) < 6) {
 180 
                                if (abs(signal - PPM_in[index]) < 6) {
179 
          if(RC_Quality < 200) RC_Quality +=10;
 181 
                                        if (RC_Quality < 200)
- 
 
 182 
                                                RC_Quality += 10;
- 
 
 183 
                                        else
180 
          else RC_Quality = 200;
 184 
                                                RC_Quality = 200;
181 
        }
 185 
                                }
182 
        // If signal is the same as before +/- 1, just keep it there.
 186 
                                // If signal is the same as before +/- 1, just keep it there.
183 
        if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) {
 187 
                                if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
184 
          // In addition, if the signal is very close to 0, just set it to 0.
 188 
                                        // In addition, if the signal is very close to 0, just set it to 0.
185 
          if (signal >=-1 && signal <= 1) {
 189 
                                        if (signal >= -1 && signal <= 1) {
186 
            tmp = 0;
 190 
                                                tmp = 0;
187 
          } else {
 191 
                                        } else {
188 
            tmp = PPM_in[index];
 192 
                                                tmp = PPM_in[index];
189 
          }
 193 
                                        }
190 
        }
 - 
 
191 
        else
 194 
                                } else
192 
          tmp = signal;
 195 
                                        tmp = signal;
193 
        // calculate signal difference on good signal level
 196 
                                // calculate signal difference on good signal level
194 
        if(RC_Quality >= 195)  
 197 
                                if (RC_Quality >= 195)
195 
          PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
 198 
                                        PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
- 
 
 199 
                                else
196 
        else PPM_diff[index] = 0;
 200 
                                        PPM_diff[index] = 0;
197 
        PPM_in[index] = tmp; // update channel value
 201 
                                PPM_in[index] = tmp; // update channel value
198 
      }
 202 
                        }
199 
      index++; // next channel
 203 
                        index++; // next channel
200 
      // demux sum signal for channels 5 to 7 to J3, J4, J5
 204 
                        // demux sum signal for channels 5 to 7 to J3, J4, J5
201 
      // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
 205 
                        // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
Line 243... Line 247...
243 
void RC_update() {
 247 
void RC_update() {
244 
  int16_t tmp1, tmp2;
 248 
        int16_t tmp1, tmp2;
245 
  if(RC_Quality) {
 249 
        if (RC_Quality) {
246 
    RC_Quality--;
 250 
                RC_Quality--;
247 
    if (NewPpmData-- == 0) {
 251 
                if (NewPpmData-- == 0) {
248 
      RC_PRTY[CONTROL_PITCH]    = RCChannel(CH_PITCH) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
 252 
                        RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP
- 
 
 253 
                                        + RCDiff(CH_PITCH) * staticParams.StickD;
249 
      RC_PRTY[CONTROL_ROLL]     = RCChannel(CH_ROLL)  * staticParams.StickP + RCDiff(CH_ROLL)  * staticParams.StickD;
 254 
                        RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
- 
 
 255 
                                        + RCDiff(CH_ROLL) * staticParams.StickD;
250 
      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE)                    + RCDiff(CH_THROTTLE) * dynamicParams.UserParams[3] + 120;
 256 
                        RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
- 
 
 257 
                                        * dynamicParams.UserParams[3] + 120;
- 
 
 258 
                        if (RC_PRTY[CONTROL_THROTTLE] < 0)
251 
      if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
 259 
                                RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
252 
      tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
 260 
                        tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
253 
      // exponential stick sensitivity in yawing rate
 261 
                        // exponential stick sensitivity in yawing rate
254 
      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
 262 
                        tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1))
- 
 
 263 
                                        / 512L; // expo  y = ax + bx^2
255 
      tmp2 += (staticParams.StickYawP * tmp1) / 4;
 264 
                        tmp2 += (staticParams.StickYawP * tmp1) / 4;
256 
      RC_PRTY[CONTROL_YAW] = tmp2;
 265 
                        RC_PRTY[CONTROL_YAW] = tmp2;
257 
    }
 266 
                }
258 
    uint8_t command = RC_getStickCommand();
 267 
                uint8_t command = RC_getStickCommand();
259 
    if (lastRCCommand == command) {
 268 
                if (lastRCCommand == command) {
260 
      // Keep timer from overrunning.
 269 
                        // Keep timer from overrunning.
261 
      if (commandTimer < COMMAND_TIMER) commandTimer++;
 270 
                        if (commandTimer < COMMAND_TIMER)
- 
 
 271 
                                commandTimer++;
262 
    } else {
 272 
                } else {
263 
      // There was a change.
 273 
                        // There was a change.
264 
      lastRCCommand = command;
 274 
                        lastRCCommand = command;
265 
      commandTimer = 0;
 275 
                        commandTimer = 0;
266 
    }
 276 
                }
267 
  } else { // Bad signal
 277 
        } else { // Bad signal
268 
    RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
 278 
                RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
- 
 
 279 
                                = RC_PRTY[CONTROL_YAW] = 0;
269 
  }
 280 
        }
270 
}
 281 
}
Line 271... Line 282...
271 
 
 282 
 
272 
/*
 283 
/*
Line 381... Line 392...
381 
}
 392 
}
Line 382... Line 393...
382 
 
 393 
 
383 
uint8_t RC_getLooping(uint8_t looping) {
 394 
uint8_t RC_getLooping(uint8_t looping) {
Line 384... Line 395...
384 
  //  static uint8_t looping = 0;
 395 
        //  static uint8_t looping = 0;
- 
 
 396 
 
385 
 
 397 
        if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
- 
 
 398 
                        & CFG_LOOP_LEFT) {
386 
  if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) {
 399 
                looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
387 
    looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
 400 
        } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
388 
  } else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 401 
                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
Line 389... Line 402...
389 
    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
 402 
                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
- 
 
 403 
        }
390 
  }
 404 
 
- 
 
 405 
        if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
391 
 
 406 
                        && staticParams.BitConfig & CFG_LOOP_RIGHT) {
392 
  if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) {
 407 
                looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
393 
    looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
 408 
        } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
Line 394... Line 409...
394 
  } else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 409 
                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
- 
 
 410 
                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
395 
    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
 411 
        }
- 
 
 412 
 
396 
  }
 413 
        if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
397 
 
 414 
                        && staticParams.BitConfig & CFG_LOOP_UP) {
398 
  if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) {
 415 
                looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
Line 399... Line 416...
399 
    looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
 416 
        } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
- 
 
 417 
                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
400 
  } else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 418 
                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
- 
 
 419 
        }
401 
    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
 420 
 
402 
  }
 421 
        if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
403 
 
 422 
                        && staticParams.BitConfig & CFG_LOOP_DOWN) {
Line 404... Line 423...
404 
  if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) {
 423 
                looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
405 
    looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
 424 
        } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
Line 406... Line 425...
406 
  } else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 425 
                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
407 
    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
 426 
                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
408 
  }
 427 
        }
409 
 
 428 
 
- 
 
 429 
        return looping;
410 
  return looping;
 430 
}
411 
}
 431 
 
412 
 
 432 
uint8_t RC_testCompassCalState(void) {
413 
uint8_t RC_testCompassCalState(void) {
 433 
        static uint8_t stick = 1;
414 
  static uint8_t stick = 1;
 434 
        // if pitch is centered or top set stick to zero