Subversion Repositories FlightCtrl

Rev

Rev 2135 | Rev 2141 | 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];
2132 - 13
volatile uint16_t RC_buffer[MAX_CHANNELS];
14
volatile uint8_t inBfrPnt = 0;
15
 
2108 - 16
volatile uint8_t RCQuality;
17
 
18
uint8_t lastRCCommand = COMMAND_NONE;
19
uint8_t lastFlightMode = FLIGHT_MODE_NONE;
20
 
2109 - 21
#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))
22
 
2108 - 23
/***************************************************************
24
 *  16bit timer 1 is used to decode the PPM-Signal            
25
 ***************************************************************/
26
void RC_Init(void) {
27
  uint8_t sreg = SREG;
28
 
29
  // disable all interrupts before reconfiguration
30
  cli();
31
 
32
  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
2109 - 33
  DDRB &= ~(1<<0);
34
  PORTB |= (1<<PORTB0);
2108 - 35
 
36
  // Timer/Counter1 Control Register A, B, C
37
  // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
38
  // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
39
  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
40
  // Enable input capture noise cancler (bit: ICNC1=1)
41
  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
42
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
43
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
44
  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
45
  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
2109 - 46
  TCCR1B |= (1 << CS11) | (1 << ICES1) | (1 << ICNC1);
2108 - 47
  TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
48
 
49
  // Timer/Counter1 Interrupt Mask Register
50
  // Enable Input Capture Interrupt (bit: ICIE1=1)
51
  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
52
  // Enable Overflow Interrupt (bit: TOIE1=0)
53
  TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
54
  TIMSK1 |= (1<<ICIE1);
55
 
56
  RCQuality = 0;
57
 
58
  SREG = sreg;
59
}
60
 
2132 - 61
/*
62
 * This new and much faster interrupt handler should reduce servo jolts.
63
 */
64
ISR(TIMER1_CAPT_vect) {
65
  static uint16_t oldICR1 = 0;
66
  uint16_t signal = (uint16_t)ICR1 - oldICR1;
67
  oldICR1 = ICR1;
68
  //sync gap? (3.5 ms < signal < 25.6 ms)
69
  if (signal > TIME(3.5)) {
70
        inBfrPnt = 0;
71
  } else if (inBfrPnt<MAX_CHANNELS) {
72
        RC_buffer[inBfrPnt++] = signal;
73
  }
74
}
75
 
2108 - 76
/********************************************************************/
77
/*         Every time a positive edge is detected at PD6            */
78
/********************************************************************/
79
/*                               t-Frame
80
    <----------------------------------------------------------------------->
81
     ____   ______   _____   ________                ______    sync gap      ____
82
    |    | |      | |     | |        |              |      |                |
83
    |    | |      | |     | |        |              |      |                |
84
 ___|    |_|      |_|     |_|        |_.............|      |________________|
85
    <-----><-------><------><-----------            <------>                <---
86
 t0       t1      t2       t4                     tn                     t0
87
 
88
 The PPM-Frame length is 22.5 ms.
89
 Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
90
 The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
91
 The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.
92
 The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
93
 The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
94
 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
95
 the syncronization gap.
96
 */
2132 - 97
void RC_process(void) {
98
        if (RCQuality) RCQuality--;
99
  for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
100
        uint16_t signal = RC_buffer[channel];
101
        if (signal != 0) {
102
          RC_buffer[channel] = 0; // reset to flag value already used.
2109 - 103
      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
2132 - 104
        signal -= (TIME(1.5) - 128 + channelMap.HWTrim);
105
        if (abs(signal - PPM_in[channel]) < TIME(0.05)) {
106
                // With 7 channels and 50 frames/sec, we get 350 channel values/sec.
2108 - 107
          if (RCQuality < 200)
2132 - 108
            RCQuality += 2;
2108 - 109
        }
2132 - 110
        PPM_in[channel] = signal;
2108 - 111
      }
112
    }
113
  }
114
}
115
 
116
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
117
 
118
uint8_t getControlModeSwitch(void) {
2109 - 119
        int16_t channel = RCChannel(CH_MODESWITCH);
120
        uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
2108 - 121
        return flightMode;
122
}
123
 
124
// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
125
// Maybe simply: Very very low throttle.
126
// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
127
// mode switched: CHMOD
128
 
129
uint8_t RC_getCommand(void) {
130
        uint8_t flightMode = getControlModeSwitch();
131
 
132
        if (lastFlightMode != flightMode) {
133
                lastFlightMode = flightMode;
134
                lastRCCommand = COMMAND_CHMOD;
135
                return lastRCCommand;
136
        }
137
 
138
        int16_t channel = RCChannel(CH_THROTTLE);
139
 
2109 - 140
        if (channel <= -TIME(0.55)) {
2132 - 141
          int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);
142
          if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
143
                lastRCCommand = COMMAND_GYROCAL;
144
          else
145
                lastRCCommand = COMMAND_RCCAL;
2108 - 146
        } else {
147
          lastRCCommand = COMMAND_NONE;
148
        }
149
        return lastRCCommand;
150
}
151
 
152
uint8_t RC_getArgument(void) {
153
        return lastFlightMode;
154
}
155
 
156
/*
157
 * Get Pitch, Roll, Throttle, Yaw values
158
 */
159
void RC_periodicTaskAndPRYT(int16_t* PRYT) {
2132 - 160
  RC_process();
161
 
162
  PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR)   - rcTrim.trim[CH_ELEVATOR];
163
  PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS)   - rcTrim.trim[CH_AILERONS];
164
  PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)     - rcTrim.trim[CH_RUDDER];
165
  PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);  // no trim on throttle!
2135 - 166
 
167
  debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
168
  debugOut.analog[21] = PRYT[CONTROL_AILERONS];
169
  debugOut.analog[22] = PRYT[CONTROL_RUDDER];
170
  debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
2108 - 171
}
172
 
173
/*
174
 * Get other channel value
175
 */
176
int16_t RC_getVariable(uint8_t varNum) {
2135 - 177
  if (varNum < 4) {
2108 - 178
    // 0th variable is 5th channel (1-based) etc.
2135 - 179
    int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset;
180
    if (varNum<2) debugOut.analog[18+varNum] = result;
181
    return result;
182
  }
2108 - 183
  /*
184
   * Let's just say:
185
   * The RC variable i is hardwired to channel i, i>=4
186
   */
2132 - 187
  return (PPM_in[varNum] >> 3) + channelMap.variableOffset;
2108 - 188
}
189
 
190
uint8_t RC_getSignalQuality(void) {
191
  if (RCQuality >= 160)
192
    return SIGNAL_GOOD;
193
  if (RCQuality >= 140)
194
    return SIGNAL_OK;
195
  if (RCQuality >= 120)
196
    return SIGNAL_BAD;
197
  return SIGNAL_LOST;
198
}
199
 
200
void RC_calibrate(void) {
2132 - 201
  rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
202
  rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
203
  rcTrim.trim[CH_RUDDER]   = RCChannel(CH_RUDDER);
204
  rcTrim.trim[CH_THROTTLE] = 0;
2108 - 205
}
2115 - 206
 
2132 - 207
int16_t RC_getZeroThrottle(void) {
2136 - 208
        return TIME (1.0f);
2115 - 209
}
2132 - 210
 
211
void RC_setZeroTrim(void) {
212
  for (uint8_t i=0; i<MAX_CHANNELS; i++) {
213
    rcTrim.trim[i] = 0;
214
  }
215
}