Subversion Repositories FlightCtrl

Rev

Rev 2118 | Rev 2122 | 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];
2099 - 13
volatile uint8_t RCQuality;
2102 - 14
 
1910 - 15
uint8_t lastRCCommand = COMMAND_NONE;
2102 - 16
uint8_t lastFlightMode = FLIGHT_MODE_NONE;
17
 
2110 - 18
#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))
2115 - 19
 
1910 - 20
/***************************************************************
21
 *  16bit timer 1 is used to decode the PPM-Signal            
22
 ***************************************************************/
23
void RC_Init(void) {
24
  uint8_t sreg = SREG;
25
 
26
  // disable all interrupts before reconfiguration
27
  cli();
28
 
29
  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
2099 - 30
  DDRD &= ~(1<<6);
31
  PORTD |= (1<<PORTD6);
1910 - 32
 
33
  // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
34
  // set as output
2099 - 35
  DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3);
1910 - 36
  // low level
2099 - 37
  PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3));
1910 - 38
 
39
  // PD3 can't be used if 2nd UART is activated
40
  // because TXD1 is at that port
41
  if (CPUType != ATMEGA644P) {
2099 - 42
    DDRD |= (1<<PORTD3);
43
    PORTD &= ~(1<<PORTD3);
1910 - 44
  }
45
 
46
  // Timer/Counter1 Control Register A, B, C
47
 
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)
2119 - 50
  // Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=0)
1910 - 51
  // Enable input capture noise cancler (bit: ICNC1=1)
52
  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
2099 - 53
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
1910 - 54
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
2119 - 55
  TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
56
  TCCR1B &= ~((1<<WGM13) | (1<<WGM12)  | (1<<CS12));
57
  TCCR1B |= (1<<CS11)    | (1<<ICNC1);
58
  TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
1910 - 59
 
2119 - 60
  if (channelMap.RCPolarity) {
61
    TCCR1B |= (1<<ICES1);
62
  } else {
63
    TCCR1B &= ~(1<<ICES1);
64
  }
65
 
1910 - 66
  // Timer/Counter1 Interrupt Mask Register
67
  // Enable Input Capture Interrupt (bit: ICIE1=1)
68
  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
69
  // Enable Overflow Interrupt (bit: TOIE1=0)
2099 - 70
  TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
71
  TIMSK1 |= (1<<ICIE1);
1910 - 72
 
2099 - 73
  RCQuality = 0;
1910 - 74
 
75
  SREG = sreg;
76
}
77
 
78
/********************************************************************/
79
/*         Every time a positive edge is detected at PD6            */
80
/********************************************************************/
81
/*                               t-Frame
2099 - 82
    <----------------------------------------------------------------------->
83
     ____   ______   _____   ________                ______    sync gap      ____
84
    |    | |      | |     | |        |              |      |                |
85
    |    | |      | |     | |        |              |      |                |
1910 - 86
 ___|    |_|      |_|     |_|        |_.............|      |________________|
2099 - 87
    <-----><-------><------><-----------            <------>                <---
1910 - 88
 t0       t1      t2       t4                     tn                     t0
89
 
90
 The PPM-Frame length is 22.5 ms.
91
 Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
92
 The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
2099 - 93
 The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.
1910 - 94
 The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
95
 The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
96
 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
97
 the syncronization gap.
98
 */
2099 - 99
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
2107 - 100
  int16_t signal, tmp;
1910 - 101
  static int16_t index;
102
  static uint16_t oldICR1 = 0;
103
 
104
  // 16bit Input Capture Register ICR1 contains the timer value TCNT1
105
  // at the time the edge was detected
106
 
107
  // calculate the time delay to the previous event time which is stored in oldICR1
108
  // calculatiing the difference of the two uint16_t and converting the result to an int16_t
109
  // implicit handles a timer overflow 65535 -> 0 the right way.
110
  signal = (uint16_t) ICR1 - oldICR1;
111
  oldICR1 = ICR1;
112
 
2109 - 113
  //sync gap? (3.5 ms < signal < 25.6 ms)
2110 - 114
  if (signal > TIME(3.5)) {
2099 - 115
    index = 0;
1910 - 116
  } else { // within the PPM frame
2099 - 117
    if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
2109 - 118
      // check for valid signal length (0.8 ms < signal < 2.2 ms)
2110 - 119
      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
1910 - 120
        // shift signal to zero symmetric range  -154 to 159
2109 - 121
        //signal -= 3750; // theoretical value
2116 - 122
        signal -= (TIME(1.5) - 128 + channelMap.trim); // best value with my Futaba in zero trim.
2099 - 123
        // check for stable signal
2110 - 124
        if (abs(signal - PPM_in[index]) < TIME(0.05)) {
2099 - 125
          if (RCQuality < 200)
126
            RCQuality += 10;
1910 - 127
          else
2099 - 128
            RCQuality = 200;
1910 - 129
        }
2099 - 130
        // If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
131
        // if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
132
          // In addition, if the signal is very close to 0, just set it to 0.
133
        if (signal >= -1 && signal <= 1) {
134
          tmp = 0;
135
        //} else {
136
        //  tmp = PPM_in[index];
137
        //  }
138
        } else
139
          tmp = signal;
140
        PPM_in[index] = tmp; // update channel value
1910 - 141
      }
142
      index++; // next channel
2099 - 143
      // demux sum signal for channels 5 to 7 to J3, J4, J5
144
      // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
145
      // channels are usually available at the receiver anyway.
146
      // if(index == 5) J3HIGH; else J3LOW;
147
      // if(index == 6) J4HIGH; else J4LOW;
148
      // if(CPUType != ATMEGA644P) // not used as TXD1
149
      //  {
150
      //    if(index == 7) J5HIGH; else J5LOW;
151
      //  }
1910 - 152
    }
153
  }
154
}
155
 
2099 - 156
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
157
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
158
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
1910 - 159
 
2103 - 160
uint8_t getControlModeSwitch(void) {
2109 - 161
        int16_t channel = RCChannel(CH_MODESWITCH);
2110 - 162
        uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
2102 - 163
        return flightMode;
164
}
165
 
166
// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
167
// Maybe simply: Very very low throttle.
168
// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
169
// mode switched: CHMOD
170
 
171
uint8_t RC_getCommand(void) {
172
        uint8_t flightMode = getControlModeSwitch();
173
 
174
        if (lastFlightMode != flightMode) {
175
                lastFlightMode = flightMode;
176
                lastRCCommand = COMMAND_CHMOD;
177
                return lastRCCommand;
178
        }
179
 
2103 - 180
        int16_t channel = RCChannel(CH_THROTTLE);
2104 - 181
 
2110 - 182
        if (channel <= -TIME(0.55)) {
2109 - 183
          lastRCCommand = COMMAND_GYROCAL;
2103 - 184
        } else {
185
          lastRCCommand = COMMAND_NONE;
186
        }
2102 - 187
        return lastRCCommand;
188
}
189
 
190
uint8_t RC_getArgument(void) {
191
        return lastFlightMode;
192
}
193
 
1910 - 194
/*
2099 - 195
 * Get Pitch, Roll, Throttle, Yaw values
1910 - 196
 */
2103 - 197
void RC_periodicTaskAndPRYT(int16_t* PRYT) {
2099 - 198
  if (RCQuality) {
199
    RCQuality--;
200
 
2103 - 201
    debugOut.analog[20] = RCChannel(CH_ELEVATOR);
202
    debugOut.analog[21] = RCChannel(CH_AILERONS);
203
    debugOut.analog[22] = RCChannel(CH_RUDDER);
204
    debugOut.analog[23] = RCChannel(CH_THROTTLE);
2102 - 205
 
2118 - 206
    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR);
207
    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS);
208
    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER);
209
    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);
2099 - 210
  } // if RCQuality is no good, we just do nothing.
1910 - 211
}
212
 
213
/*
214
 * Get other channel value
215
 */
216
int16_t RC_getVariable(uint8_t varNum) {
217
  if (varNum < 4)
218
    // 0th variable is 5th channel (1-based) etc.
2116 - 219
    return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset;
1910 - 220
  /*
221
   * Let's just say:
2099 - 222
   * The RC variable i is hardwired to channel i, i>=4
1910 - 223
   */
2116 - 224
  return (PPM_in[varNum] >> 3) + channelMap.variableOffset;
1910 - 225
}
226
 
227
uint8_t RC_getSignalQuality(void) {
2099 - 228
  if (RCQuality >= 160)
1910 - 229
    return SIGNAL_GOOD;
2099 - 230
  if (RCQuality >= 140)
1910 - 231
    return SIGNAL_OK;
2099 - 232
  if (RCQuality >= 120)
1910 - 233
    return SIGNAL_BAD;
234
  return SIGNAL_LOST;
235
}
236
 
237
void RC_calibrate(void) {
238
  // Do nothing.
239
}
2115 - 240
 
2119 - 241
int16_t RC_getZeroThrottle(void) {
2115 - 242
        return TIME (-0.5);
243
}