Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / Receiver.h @ 9240aaa3

History | View | Annotate | Download (9.79 KB)

1
/*
2
  AeroQuad v2.1 - October 2010
3
  www.AeroQuad.com
4
  Copyright (c) 2010 Ted Carancho.  All rights reserved.
5
  An Open Source Arduino based multicopter.
6
 
7
  This program is free software: you can redistribute it and/or modify 
8
  it under the terms of the GNU General Public License as published by 
9
  the Free Software Foundation, either version 3 of the License, or 
10
  (at your option) any later version. 
11

12
  This program is distributed in the hope that it will be useful, 
13
  but WITHOUT ANY WARRANTY; without even the implied warranty of 
14
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
15
  GNU General Public License for more details. 
16

17
  You should have received a copy of the GNU General Public License 
18
  along with this program. If not, see <http://www.gnu.org/licenses/>. 
19
*/
20

    
21

    
22

    
23
  #define AUX2SCALE_ADR AUXSCALE_ADR
24
  #define AUX2OFFSET_ADR AUXOFFSET_ADR
25
  #define AUX2SMOOTH_ADR AUXSMOOTH_ADR
26

    
27
class  Receiver {
28
public:
29
  int receiverData[7];
30
  int transmitterCommand[7];
31
  int transmitterCommandSmooth[7];
32
  int transmitterZero[3];
33
  int transmitterTrim[3];
34
  // Controls the strength of the commands sent from the transmitter
35
  // xmitFactor ranges from 0.01 - 1.0 (0.01 = weakest, 1.0 - strongest)
36
  float xmitFactor; // Read in from EEPROM
37
  float transmitterSmooth[7];
38
  float mTransmitter[7];
39
  float bTransmitter[7];
40

    
41
  Receiver(void) { 
42
    transmitterCommand[ROLL] = 1500;
43
    transmitterCommand[PITCH] = 1500;
44
    transmitterCommand[YAW] = 1500;
45
    transmitterCommand[THROTTLE] = 1000;
46
    transmitterCommand[MODE] = 1000;
47
    transmitterCommand[AUX] = 1000;
48
    transmitterCommand[AUX2] = 1000;
49
    
50
    for (channel = ROLL; channel < LASTCHANNEL; channel++)
51
      transmitterCommandSmooth[channel] = 0;
52
    for (channel = ROLL; channel < THROTTLE; channel++)
53
      transmitterZero[channel] = 1500;
54
  }
55

    
56
  // ******************************************************************
57
  // The following function calls must be defined in any new subclasses
58
  // ******************************************************************
59
  virtual void initialize(void);
60
  virtual void read(void);
61

    
62
  // **************************************************************
63
  // The following functions are common between all Gyro subclasses
64
  // **************************************************************
65
  
66
  void _initialize(void) {
67
    xmitFactor = readFloat(XMITFACTOR_ADR);
68
    mTransmitter[ROLL] = readFloat(ROLLSCALE_ADR);
69
    bTransmitter[ROLL] = readFloat(ROLLOFFSET_ADR);
70
    mTransmitter[PITCH] = readFloat(PITCHSCALE_ADR);
71
    bTransmitter[PITCH] = readFloat(PITCHOFFSET_ADR);
72
    mTransmitter[YAW] = readFloat(YAWSCALE_ADR);
73
    bTransmitter[YAW] = readFloat(YAWOFFSET_ADR);
74
    mTransmitter[THROTTLE] = readFloat(THROTTLESCALE_ADR);
75
    bTransmitter[THROTTLE] = readFloat(THROTTLEOFFSET_ADR);
76
    mTransmitter[MODE] = readFloat(MODESCALE_ADR);
77
    bTransmitter[MODE] = readFloat(MODEOFFSET_ADR);
78
    mTransmitter[AUX] = readFloat(AUXSCALE_ADR);
79
    bTransmitter[AUX] = readFloat(AUXOFFSET_ADR);
80
    mTransmitter[AUX2] = readFloat(AUX2SCALE_ADR);
81
    bTransmitter[AUX2] = readFloat(AUX2OFFSET_ADR);
82
    
83
    transmitterSmooth[THROTTLE] = readFloat(THROTTLESMOOTH_ADR);
84
    transmitterSmooth[ROLL] = readFloat(ROLLSMOOTH_ADR);
85
    transmitterSmooth[PITCH] = readFloat(PITCHSMOOTH_ADR);
86
    transmitterSmooth[YAW] = readFloat(YAWSMOOTH_ADR);
87
    transmitterSmooth[MODE] = readFloat(MODESMOOTH_ADR);
88
    transmitterSmooth[AUX] = readFloat(AUXSMOOTH_ADR);
89
    transmitterSmooth[AUX2] = readFloat(AUX2SMOOTH_ADR);
90
  }
91
  
92
  const int getRaw(byte channel) {
93
    return receiverData[channel];
94
  }
95
  
96
  const int getData(byte channel) {
97
    // reduce sensitivity of transmitter input by xmitFactor
98
    return transmitterCommand[channel];
99
  }
100
  
101
  const int getTrimData(byte channel) {
102
    return receiverData[channel] - transmitterTrim[channel];
103
  }
104
  
105
  const int getZero(byte channel) {
106
    return transmitterZero[channel];
107
  }
108
  
109
  void setZero(byte channel, int value) {
110
    transmitterZero[channel] = value;
111
  }
112
  
113
  const int getTransmitterTrim(byte channel) {
114
    return transmitterTrim[channel];
115
  }
116
  
117
  void setTransmitterTrim(byte channel, int value) {
118
    transmitterTrim[channel] = value;
119
  }
120
  
121
  const float getSmoothFactor(byte channel) {
122
    return transmitterSmooth[channel];
123
  }
124
  
125
  void setSmoothFactor(byte channel, float value) {
126
    transmitterSmooth[channel] = value;
127
  }
128
  
129
  const float getXmitFactor(void) {
130
    return xmitFactor;
131
  }
132
  
133
  void setXmitFactor(float value) {
134
    xmitFactor = value;
135
  }
136
  
137
  const float getTransmitterSlope(byte channel) {
138
    return mTransmitter[channel];
139
  }
140
  
141
  void setTransmitterSlope(byte channel, float value) {
142
    mTransmitter[channel] = value;
143
  }
144
  
145
  const float getTransmitterOffset(byte channel) {
146
    return bTransmitter[channel];
147
  }
148
  
149
  void setTransmitterOffset(byte channel, float value) {
150
    bTransmitter[channel] = value;
151
  }
152
  
153
  const float getAngle(byte channel) {
154
    // Scale 1000-2000 usecs to -45 to 45 degrees
155
    // m = 0.09, b = -135
156
    // reduce transmitterCommand by xmitFactor to lower sensitivity of transmitter input
157
    return (0.09 * transmitterCommand[channel]) - 135;
158
  }
159
};
160

    
161

    
162

    
163

    
164

    
165

    
166

    
167

    
168

    
169

    
170

    
171

    
172

    
173

    
174

    
175

    
176

    
177

    
178

    
179

    
180

    
181

    
182

    
183

    
184

    
185

    
186

    
187

    
188

    
189

    
190

    
191

    
192

    
193

    
194

    
195
/******************************************************/
196
/*************** AeroQuad Mega PCINT ******************/
197
/******************************************************/
198
#if defined(AeroQuadMega_v2)
199
  volatile uint8_t *port_to_pcmask[] = {
200
    &PCMSK0,
201
    &PCMSK1,
202
    &PCMSK2
203
  };
204
  volatile static uint8_t PCintLast[3];
205
  // Channel data 
206
  typedef struct {
207
    byte edge;
208
    unsigned long riseTime;    
209
    unsigned long fallTime; 
210
    unsigned long lastGoodWidth;
211
  } pinTimingData;  
212
  volatile static pinTimingData pinData[24]; 
213

    
214
  static void MegaPcIntISR() {
215
    uint8_t bit;
216
    uint8_t curr;
217
    uint8_t mask;
218
    uint8_t pin;
219
    uint32_t currentTime;
220
    uint32_t time;
221

    
222
    //curr = PORTK;
223
    curr = *portInputRegister(11);
224
    mask = curr ^ PCintLast[0];
225
    PCintLast[0] = curr;  
226

    
227
    //Serial.println(curr,DEC);
228

    
229
    // mask is pins that have changed. screen out non pcint pins.
230
    if ((mask &= PCMSK2) == 0) {
231
      return;
232
    }
233

    
234
    currentTime = micros();
235

    
236
    // mask is pcint pins that have changed.
237
    for (uint8_t i=0; i < 8; i++) {
238
      bit = 0x01 << i;
239
      if (bit & mask) {
240
        pin = i;
241
        // for each pin changed, record time of change
242
        if (bit & PCintLast[0]) {
243
         time = currentTime - pinData[pin].fallTime;
244
          pinData[pin].riseTime = currentTime;
245
          if ((time >= MINOFFWIDTH) && (time <= MAXOFFWIDTH))
246
            pinData[pin].edge = RISING_EDGE;
247
          else
248
            pinData[pin].edge = FALLING_EDGE; // invalid rising edge detected
249
        }
250
        else {
251
          time = currentTime - pinData[pin].riseTime;
252
          pinData[pin].fallTime = currentTime;
253
          if ((time >= MINONWIDTH) && (time <= MAXONWIDTH) && (pinData[pin].edge == RISING_EDGE)) {
254
            pinData[pin].lastGoodWidth = time;
255
            //Serial.println(pinData[4].lastGoodWidth);
256
            pinData[pin].edge = FALLING_EDGE;
257
          } 
258
        }
259
      }
260
    }
261
  }
262

    
263
  SIGNAL(PCINT2_vect) {
264
    MegaPcIntISR();
265
  }
266
  
267
class Receiver_AeroQuadMega : public Receiver {
268
private:
269
  int receiverChannel[7];
270
  int receiverPin[7];
271
  //Receiver pin assignments for the Arduino Mega using an AeroQuad v1.x Shield
272
  //The defines below are for documentation only of the Mega receiver input
273
  //The real pin assignments happen in initializeMegaPcInt2()
274
  //If you are using an AQ 1.x Shield, put a jumper wire between the Shield and Mega as indicated in the comments below
275
  
276
  public:
277
  Receiver_AeroQuadMega() : Receiver(){}
278

    
279
  void initialize() {
280
    this->_initialize(); // load in calibration xmitFactor from EEPROM
281
    DDRK = 0;
282
    PORTK = 0;
283
    PCMSK2 |= 0x7F;//7f?!edit was 3F         //alter this to enable additional channel
284
    PCICR |= 0x1 << 2;
285
    
286

    
287
      receiverChannel[ROLL] = 63;
288
      receiverChannel[PITCH] = 64;
289
      receiverChannel[YAW] = 65;
290
      receiverChannel[THROTTLE] = 62;
291
      receiverChannel[MODE] = 66;
292
      receiverChannel[AUX] = 67;
293
      receiverChannel[AUX2] = 68;
294
      // defines ATmega328P pins (Arduino pins converted to ATmega328P pinouts)
295
      receiverPin[ROLL] = 1;
296
      receiverPin[PITCH] = 2;
297
      receiverPin[YAW] = 3;
298
      receiverPin[THROTTLE] = 0;
299
      receiverPin[MODE] = 4;
300
      receiverPin[AUX] = 5;
301
      receiverPin[AUX2] = 6;
302

    
303
    for (channel = ROLL; channel < LASTCHANNEL; channel++)
304
      pinData[receiverChannel[channel]].edge = FALLING_EDGE;
305
  }
306
  
307
  // Calculate PWM pulse width of receiver data
308
  // If invalid PWM measured, use last known good time
309
  void read(void) {
310
    uint16_t data[7];
311
    uint8_t oldSREG;
312
      
313
    oldSREG = SREG;
314
    cli();
315
    // Buffer receiver values read from pin change interrupt handler
316
    for (channel = ROLL; channel < LASTCHANNEL; channel++)
317
      data[channel] = pinData[receiverPin[channel]].lastGoodWidth;
318
    SREG = oldSREG;  
319
    
320
    for(channel = ROLL; channel < LASTCHANNEL; channel++) {
321
      // Apply transmitter calibration adjustment
322
      receiverData[channel] = (mTransmitter[channel] * data[channel]) + bTransmitter[channel];
323
      // Smooth the flight control transmitter inputs 
324
      transmitterCommandSmooth[channel] = smooth(receiverData[channel], transmitterCommandSmooth[channel], transmitterSmooth[channel]);
325
      //transmitterCommandSmooth[channel] = transmitterFilter[channel].filter(receiverData[channel]);
326
    }
327
    // Reduce transmitter commands using xmitFactor and center around 1500
328
    for (channel = ROLL; channel < THROTTLE; channel++)
329
      transmitterCommand[channel] = ((transmitterCommandSmooth[channel] - transmitterZero[channel]) * xmitFactor) + transmitterZero[channel];
330
    // No xmitFactor reduction applied for throttle, mode and AUX
331
    for (channel = THROTTLE; channel < LASTCHANNEL; channel++)
332
      transmitterCommand[channel] = transmitterCommandSmooth[channel];
333
  }
334
};
335
#endif
336