Subversion Repositories FlightCtrl

#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;

volatile uint8_t RCQuality;

uint8_t lastRCCommand;
uint8_t lastFlightMode;

#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;
  }
}