Subversion Repositories FlightCtrl

Rev

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

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