Subversion Repositories FlightCtrl

Rev

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