Subversion Repositories FlightCtrl

Rev

Rev 1869 | Rev 1960 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1869 Rev 1872
Line 72... Line 72...
72 
 
 72 
 
73 
/***************************************************************
 73 
/***************************************************************
74 
 *  16bit timer 1 is used to decode the PPM-Signal            
 74 
 *  16bit timer 1 is used to decode the PPM-Signal            
75 
 ***************************************************************/
 75 
 ***************************************************************/
76 
void RC_Init(void) {
 76 
void RC_Init(void) {
Line 77... Line 77...
77 
        uint8_t sreg = SREG;
 77 
  uint8_t sreg = SREG;
78 
 
 78 
 
Line 79... Line 79...
79 
        // disable all interrupts before reconfiguration
 79 
  // disable all interrupts before reconfiguration
80 
        cli();
 80 
  cli();
81 
 
 81 
 
82 
        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
 82 
  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
83 
        DDRD &= ~(1 << DDD6);
 83 
  DDRD &= ~(1 << DDD6);
84 
        PORTD |= (1 << PORTD6);
 84 
  PORTD |= (1 << PORTD6);
85 
 
 85 
 
86 
        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
 86 
  // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
87 
        // set as output
 87 
  // set as output
88 
        DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3);
 88 
  DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3);
89 
        // low level
 89 
  // low level
90 
        PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3));
 90 
  PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3));
91 
 
 91 
 
92 
        // PD3 can't be used if 2nd UART is activated
 92 
  // PD3 can't be used if 2nd UART is activated
93 
        // because TXD1 is at that port
 93 
  // because TXD1 is at that port
94 
        if (CPUType != ATMEGA644P) {
 94 
  if (CPUType != ATMEGA644P) {
95 
                DDRD |= (1 << PORTD3);
 95 
    DDRD |= (1 << PORTD3);
96 
                PORTD &= ~(1 << PORTD3);
 96 
    PORTD &= ~(1 << PORTD3);
97 
        }
 97 
  }
98 
 
 98 
 
99 
        // Timer/Counter1 Control Register A, B, C
 99 
  // Timer/Counter1 Control Register A, B, C
100 
 
 100 
 
101 
        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
 101 
  // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
102 
        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
 102 
  // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
103 
        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
 103 
  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
104 
        // Enable input capture noise cancler (bit: ICNC1=1)
 104 
  // Enable input capture noise cancler (bit: ICNC1=1)
105 
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
 105 
  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
106 
        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
 106 
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
107 
        // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
 107 
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
108 
        TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0)
 108 
  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0)
109 
                        | (1 << WGM11) | (1 << WGM10));
 109 
      | (1 << WGM11) | (1 << WGM10));
110 
        TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
 110 
  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
111 
        TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
 111 
  TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
112 
        TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
 112 
  TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
113 
 
 113 
 
114 
        // Timer/Counter1 Interrupt Mask Register
 114 
  // Timer/Counter1 Interrupt Mask Register
115 
 
 115 
 
116 
        // Enable Input Capture Interrupt (bit: ICIE1=1)
 116 
  // Enable Input Capture Interrupt (bit: ICIE1=1)
117 
        // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
 117 
  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
Line 118... Line 118...
118 
        // Enable Overflow Interrupt (bit: TOIE1=0)
 118 
  // Enable Overflow Interrupt (bit: TOIE1=0)
Line 119... Line 119...
119 
        TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1));
 119 
  TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1));
120 
        TIMSK1 |= (1 << ICIE1);
 120 
  TIMSK1 |= (1 << ICIE1);
Line 121... Line 121...
121 
 
 121 
 
122 
        RC_Quality = 0;
 122 
  RC_Quality = 0;
123 
 
 123 
 
124 
        SREG = sreg;
 124 
  SREG = sreg;
125 
}
 125 
}
126 
 
 126 
 
127 
/********************************************************************/
 127 
/********************************************************************/
128 
/*         Every time a positive edge is detected at PD6            */
 128 
/*         Every time a positive edge is detected at PD6            */
129 
/********************************************************************/
 129 
/********************************************************************/
130 
/*                               t-Frame
 130 
/*                               t-Frame
131 
 <----------------------------------------------------------------------->
 131 
 <----------------------------------------------------------------------->
Line 132... Line 132...
132 
    ____   ______   _____   ________                ______    sync gap      ____
 132 
 ____   ______   _____   ________                ______    sync gap      ____
133 
   |    | |      | |     | |        |              |      |                |
 133 
 |    | |      | |     | |        |              |      |                |
134 
   |    | |      | |     | |        |              |      |                |
 134 
 |    | |      | |     | |        |              |      |                |
Line 145... Line 145...
145 
 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
 145 
 The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
146 
 the syncronization gap.
 146 
 the syncronization gap.
147 
 */
 147 
 */
148 
ISR(TIMER1_CAPT_vect)
 148 
ISR(TIMER1_CAPT_vect)
149 
{ // typical rate of 1 ms to 2 ms
 149 
{ // typical rate of 1 ms to 2 ms
150 
        int16_t signal = 0, tmp;
 150 
  int16_t signal = 0, tmp;
151 
        static int16_t index;
 151 
  static int16_t index;
152 
        static uint16_t oldICR1 = 0;
 152 
  static uint16_t oldICR1 = 0;
153 
 
 153 
 
154 
        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
 154 
  // 16bit Input Capture Register ICR1 contains the timer value TCNT1
155 
        // at the time the edge was detected
 155 
  // at the time the edge was detected
156 
 
 156 
 
157 
        // calculate the time delay to the previous event time which is stored in oldICR1
 157 
  // calculate the time delay to the previous event time which is stored in oldICR1
158 
        // calculatiing the difference of the two uint16_t and converting the result to an int16_t
 158 
  // calculatiing the difference of the two uint16_t and converting the result to an int16_t
159 
        // implicit handles a timer overflow 65535 -> 0 the right way.
 159 
  // implicit handles a timer overflow 65535 -> 0 the right way.
160 
        signal = (uint16_t) ICR1 - oldICR1;
 160 
  signal = (uint16_t) ICR1 - oldICR1;
161 
        oldICR1 = ICR1;
 161 
  oldICR1 = ICR1;
162 
 
 162 
 
163 
        //sync gap? (3.52 ms < signal < 25.6 ms)
 163 
  //sync gap? (3.52 ms < signal < 25.6 ms)
164 
        if ((signal > 1100) && (signal < 8000)) {
 164 
  if ((signal > 1100) && (signal < 8000)) {
165 
                // if a sync gap happens and there where at least 4 channels decoded before
 165 
    // if a sync gap happens and there where at least 4 channels decoded before
166 
                // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
 166 
    // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
167 
                if (index >= 4) {
 167 
    if (index >= 4) {
168 
                        NewPpmData = 0; // Null means NewData for the first 4 channels
 168 
      NewPpmData = 0; // Null means NewData for the first 4 channels
169 
                }
 169 
    }
170 
                // synchronize channel index
 170 
    // synchronize channel index
171 
                index = 1;
 171 
    index = 1;
172 
        } else { // within the PPM frame
 172 
  } else { // within the PPM frame
173 
                if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels
 173 
    if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels
174 
                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)
 174 
      // check for valid signal length (0.8 ms < signal < 2.1984 ms)
175 
                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
 175 
      // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
176 
                        if ((signal > 250) && (signal < 687)) {
 176 
      if ((signal > 250) && (signal < 687)) {
177 
                                // shift signal to zero symmetric range  -154 to 159
 177 
        // shift signal to zero symmetric range  -154 to 159
178 
                                signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
 178 
        signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
179 
                                // check for stable signal
 179 
        // check for stable signal
180 
                                if (abs(signal - PPM_in[index]) < 6) {
 180 
        if (abs(signal - PPM_in[index]) < 6) {
181 
                                        if (RC_Quality < 200)
 181 
          if (RC_Quality < 200)
182 
                                                RC_Quality += 10;
 182 
            RC_Quality += 10;
183 
                                        else
 183 
          else
184 
                                                RC_Quality = 200;
 184 
            RC_Quality = 200;
185 
                                }
 185 
        }
186 
                                // If signal is the same as before +/- 1, just keep it there.
 186 
        // If signal is the same as before +/- 1, just keep it there.
187 
                                if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
 187 
        if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
188 
                                        // In addition, if the signal is very close to 0, just set it to 0.
 188 
          // In addition, if the signal is very close to 0, just set it to 0.
189 
                                        if (signal >= -1 && signal <= 1) {
 189 
          if (signal >= -1 && signal <= 1) {
190 
                                                tmp = 0;
 190 
            tmp = 0;
191 
                                        } else {
 191 
          } else {
192 
                                                tmp = PPM_in[index];
 192 
            tmp = PPM_in[index];
193 
                                        }
 193 
          }
194 
                                } else
 194 
        } else
195 
                                        tmp = signal;
 195 
          tmp = signal;
196 
                                // calculate signal difference on good signal level
 196 
        // calculate signal difference on good signal level
197 
                                if (RC_Quality >= 195)
 197 
        if (RC_Quality >= 195)
198 
                                        PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
 198 
          PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
199 
                                else
 199 
        else
200 
                                        PPM_diff[index] = 0;
 200 
          PPM_diff[index] = 0;
201 
                                PPM_in[index] = tmp; // update channel value
 201 
        PPM_in[index] = tmp; // update channel value
202 
                        }
 202 
      }
203 
                        index++; // next channel
 203 
      index++; // next channel
204 
                        // demux sum signal for channels 5 to 7 to J3, J4, J5
 204 
      // demux sum signal for channels 5 to 7 to J3, J4, J5
205 
                        // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
 205 
      // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
206 
                        // channels are usually available at the receiver anyway.
 206 
      // channels are usually available at the receiver anyway.
207 
                        // if(index == 5) J3HIGH; else J3LOW;
 207 
      // if(index == 5) J3HIGH; else J3LOW;
208 
                        // if(index == 6) J4HIGH; else J4LOW;
 208 
      // if(index == 6) J4HIGH; else J4LOW;
209 
                        // if(CPUType != ATMEGA644P) // not used as TXD1
 209 
      // if(CPUType != ATMEGA644P) // not used as TXD1
210 
                        //  {
 210 
      //  {
211 
                        //    if(index == 7) J5HIGH; else J5LOW;
 211 
      //    if(index == 7) J5HIGH; else J5LOW;
212 
                        //  }
 212 
      //  }
213 
                }
 213 
    }
214 
        }
 214 
  }
215 
}
 215 
}
Line 216... Line 216...
216 
 
 216 
 
217 
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
 217 
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
218 
#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
 218 
#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
219 
#define COMMAND_THRESHOLD 85
 219 
#define COMMAND_THRESHOLD 85
220 
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
 220 
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
Line 221... Line 221...
221 
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 221 
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
222 
 
 222 
 
223 
// Internal.
 223 
// Internal.
224 
uint8_t RC_getStickCommand(void) {
 224 
uint8_t RC_getStickCommand(void) {
225 
        if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
 225 
  if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
226 
                // vertical is up
 226 
    // vertical is up
227 
                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
 227 
    if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
228 
                        return COMMAND_GYROCAL;
 228 
      return COMMAND_GYROCAL;
229 
                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
 229 
    if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
230 
                        return COMMAND_ACCCAL;
 230 
      return COMMAND_ACCCAL;
231 
                return COMMAND_NONE;
 231 
    return COMMAND_NONE;
232 
        } else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
 232 
  } else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
233 
                // vertical is down
 233 
    // vertical is down
234 
                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
 234 
    if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
235 
                        return COMMAND_STOP;
 235 
      return COMMAND_STOP;
236 
                if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
 236 
    if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
237 
                        return COMMAND_START;
 237 
      return COMMAND_START;
238 
                return COMMAND_NONE;
 238 
    return COMMAND_NONE;
239 
        }
 239 
  }
240 
        // vertical is around center
 240 
  // vertical is around center
Line 241... Line 241...
241 
        return COMMAND_NONE;
 241 
  return COMMAND_NONE;
242 
}
 242 
}
243 
 
 243 
 
244 
/*
 244 
/*
245 
 * This must be called (as the only thing) for each control loop cycle (488 Hz).
 245 
 * This must be called (as the only thing) for each control loop cycle (488 Hz).
246 
 */
 246 
 */
247 
void RC_update() {
 247 
void RC_update() {
248 
        int16_t tmp1, tmp2;
 248 
  int16_t tmp1, tmp2;
249 
        if (RC_Quality) {
 249 
  if (RC_Quality) {
250 
                RC_Quality--;
 250 
    RC_Quality--;
251 
                if (NewPpmData-- == 0) {
 251 
    if (NewPpmData-- == 0) {
252 
                        RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP
 252 
      RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP
253 
                                        + RCDiff(CH_PITCH) * staticParams.StickD;
 253 
          + RCDiff(CH_PITCH) * staticParams.StickD;
254 
                        RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
 254 
      RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
255 
                                        + RCDiff(CH_ROLL) * staticParams.StickD;
 255 
          + RCDiff(CH_ROLL) * staticParams.StickD;
256 
                        RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
 256 
      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
257 
                                        * dynamicParams.UserParams[3] + 120;
 257 
          * dynamicParams.UserParams[3] + 120;
258 
                        if (RC_PRTY[CONTROL_THROTTLE] < 0)
 258 
      if (RC_PRTY[CONTROL_THROTTLE] < 0)
259 
                                RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
 259 
        RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
260 
                        tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
 260 
      tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
261 
                        // exponential stick sensitivity in yawing rate
 261 
      // exponential stick sensitivity in yawing rate
262 
                        tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1))
 262 
      tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1))
263 
                                        / 512L; // expo  y = ax + bx^2
 263 
          / 512L; // expo  y = ax + bx^2
264 
                        tmp2 += (staticParams.StickYawP * tmp1) / 4;
 264 
      tmp2 += (staticParams.StickYawP * tmp1) >> 2;
- 
 
 265 
      RC_PRTY[CONTROL_YAW] = tmp2;
265 
                        RC_PRTY[CONTROL_YAW] = tmp2;
 266 
    }
266 
                }
 267 
    uint8_t command = RC_getStickCommand();
267 
                uint8_t command = RC_getStickCommand();
 268 
 
268 
                if (lastRCCommand == command) {
 269 
    if (lastRCCommand == command) {
269 
                        // Keep timer from overrunning.
 270 
      // Keep timer from overrunning.
270 
                        if (commandTimer < COMMAND_TIMER)
 271 
      if (commandTimer < COMMAND_TIMER)
271 
                                commandTimer++;
 272 
        commandTimer++;
272 
                } else {
 273 
    } else {
273 
                        // There was a change.
 274 
      // There was a change.
274 
                        lastRCCommand = command;
 275 
      lastRCCommand = command;
275 
                        commandTimer = 0;
 276 
      commandTimer = 0;
276 
                }
 277 
    }
277 
        } else { // Bad signal
 278 
  } else { // Bad signal
278 
                RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
 279 
    RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
Line 279... Line 280...
279 
                                = RC_PRTY[CONTROL_YAW] = 0;
 280 
        = RC_PRTY[CONTROL_YAW] = 0;
280 
        }
 281 
  }
281 
}
 282 
}
282 
 
 283 
 
283 
/*
 284 
/*
284 
 * Get Pitch, Roll, Throttle, Yaw values
 285 
 * Get Pitch, Roll, Throttle, Yaw values
Line 285... Line 286...
285 
 */
 286 
 */
286 
int16_t* RC_getPRTY(void) {
 287 
int16_t* RC_getPRTY(void) {
287 
        return RC_PRTY;
 288 
  return RC_PRTY;
288 
}
 289 
}
289 
 
 290 
 
290 
/*
 291 
/*
291 
 * Get other channel value
 292 
 * Get other channel value
292 
 */
 293 
 */
293 
int16_t RC_getVariable(uint8_t varNum) {
 294 
int16_t RC_getVariable(uint8_t varNum) {
294 
        if (varNum < 4)
 295 
  if (varNum < 4)
295 
                // 0th variable is 5th channel (1-based) etc.
 296 
    // 0th variable is 5th channel (1-based) etc.
296 
                return RCChannel(varNum + 4) + POT_OFFSET;
 297 
    return RCChannel(varNum + 4) + POT_OFFSET;
297 
        /*
 298 
  /*
298 
         * Let's just say:
 299 
   * Let's just say:
299 
         * The RC variable 4 is hardwired to channel 5
 300 
   * The RC variable 4 is hardwired to channel 5
300 
         * The RC variable 5 is hardwired to channel 6
 301 
   * The RC variable 5 is hardwired to channel 6
301 
         * The RC variable 6 is hardwired to channel 7
 302 
   * The RC variable 6 is hardwired to channel 7
Line 302... Line 303...
302 
         * The RC variable 7 is hardwired to channel 8
 303 
   * The RC variable 7 is hardwired to channel 8
303 
         * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
 304 
   * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
304 
         */
 305 
   */
305 
        return PPM_in[varNum + 1] + POT_OFFSET;
 306 
  return PPM_in[varNum + 1] + POT_OFFSET;
306 
}
 307 
}
307 
 
 308 
 
308 
uint8_t RC_getSignalQuality(void) {
 309 
uint8_t RC_getSignalQuality(void) {
309 
        if (RC_Quality >= 160)
 310 
  if (RC_Quality >= 160)
310 
                return SIGNAL_GOOD;
 311 
    return SIGNAL_GOOD;
Line 311... Line 312...
311 
        if (RC_Quality >= 140)
 312 
  if (RC_Quality >= 140)
312 
                return SIGNAL_OK;
 313 
    return SIGNAL_OK;
313 
        if (RC_Quality >= 120)
 314 
  if (RC_Quality >= 120)
Line 323... Line 324...
323 
 * R617 receiver.) This calibration is not strictly necessary, but
 324 
 * R617 receiver.) This calibration is not strictly necessary, but
324 
 * for control logic that depends on the exact (non)center position
 325 
 * for control logic that depends on the exact (non)center position
325 
 * of a stick, it may be useful.
 326 
 * of a stick, it may be useful.
326 
 */
 327 
 */
327 
void RC_calibrate(void) {
 328 
void RC_calibrate(void) {
328 
        // Do nothing.
 329 
  // Do nothing.
329 
}
 330 
}
Line 330... Line 331...
330 
 
 331 
 
331 
/*
 332 
/*
332 
 if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
 333 
 if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
Line 338... Line 339...
338 
 }
 339 
 }
339 
 }
 340 
 }
340 
 */
 341 
 */
Line 341... Line 342...
341 
 
 342 
 
342 
uint8_t RC_getCommand(void) {
 343 
uint8_t RC_getCommand(void) {
343 
        if (commandTimer == COMMAND_TIMER) {
 344 
  if (commandTimer == COMMAND_TIMER) {
344 
                // Stick has been held long enough; command committed.
 345 
    // Stick has been held long enough; command committed.
345 
                return lastRCCommand;
 346 
    return lastRCCommand;
346 
        }
 347 
  }
347 
        // Not yet sure what the command is.
 348 
  // Not yet sure what the command is.
348 
        return COMMAND_NONE;
 349 
  return COMMAND_NONE;
Line 349... Line 350...
349 
}
 350 
}
350 
 
 351 
 
351 
/*
 352 
/*
Line 365... Line 366...
365 
#define ARGUMENT_THRESHOLD 70
 366 
#define ARGUMENT_THRESHOLD 70
366 
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
 367 
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
367 
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
 368 
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
Line 368... Line 369...
368 
 
 369 
 
369 
uint8_t RC_getArgument(void) {
 370 
uint8_t RC_getArgument(void) {
370 
        if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
 371 
  if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
371 
                // vertical is up
 372 
    // vertical is up
372 
                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
 373 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
373 
                        return 2;
 374 
      return 2;
374 
                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
 375 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
375 
                        return 4;
 376 
      return 4;
376 
                return 3;
 377 
    return 3;
377 
        } else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
 378 
  } else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
378 
                // vertical is down
 379 
    // vertical is down
379 
                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
 380 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
380 
                        return 8;
 381 
      return 8;
381 
                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
 382 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
382 
                        return 6;
 383 
      return 6;
383 
                return 7;
 384 
    return 7;
384 
        } else {
 385 
  } else {
385 
                // vertical is around center
 386 
    // vertical is around center
386 
                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
 387 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
387 
                        return 1;
 388 
      return 1;
388 
                if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
 389 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
389 
                        return 5;
 390 
      return 5;
390 
                return 0;
 391 
    return 0;
391 
        }
 392 
  }
Line 392... Line 393...
392 
}
 393 
}
393 
 
 394 
 
Line 394... Line 395...
394 
uint8_t RC_getLooping(uint8_t looping) {
 395 
uint8_t RC_getLooping(uint8_t looping) {
395 
        //  static uint8_t looping = 0;
 396 
  //  static uint8_t looping = 0;
396 
 
 397 
 
397 
        if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
 398 
  if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
398 
                        & CFG_LOOP_LEFT) {
 399 
      & CFG_LOOP_LEFT) {
399 
                looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
 400 
    looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
400 
        } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
 401 
  } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
401 
                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 402 
      < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
402 
                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
 403 
    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
403 
        }
 404 
  }
404 
 
 405 
 
405 
        if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
 406 
  if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
406 
                        && staticParams.BitConfig & CFG_LOOP_RIGHT) {
 407 
      && staticParams.BitConfig & CFG_LOOP_RIGHT) {
407 
                looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
 408 
    looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
408 
        } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
 409 
  } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
409 
                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 410 
      > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
410 
                looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
 411 
    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
411 
        }
 412 
  }
412 
 
 413 
 
413 
        if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
 414 
  if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
414 
                        && staticParams.BitConfig & CFG_LOOP_UP) {
 415 
      && staticParams.BitConfig & CFG_LOOP_UP) {
415 
                looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
 416 
    looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
416 
        } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
 417 
  } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
417 
                        < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 418 
      < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
418 
                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
 419 
    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
419 
        }
 420 
  }
420 
 
 421 
 
421 
        if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
 422 
  if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
422 
                        && staticParams.BitConfig & CFG_LOOP_DOWN) {
 423 
      && staticParams.BitConfig & CFG_LOOP_DOWN) {
423 
                looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
 424 
    looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
424 
        } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
 425 
  } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
Line 425... Line 426...
425 
                        > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
 426 
      > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
426 
                looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
 427 
    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
Line 427... Line 428...
427 
        }
 428 
  }
428 
 
 429 
 
429 
        return looping;
 430 
  return looping;
430 
}
 431 
}
431 
 
 432 
 
432 
uint8_t RC_testCompassCalState(void) {
 433 
uint8_t RC_testCompassCalState(void) {
433 
        static uint8_t stick = 1;
 434 
  static uint8_t stick = 1;
434 
        // if pitch is centered or top set stick to zero
 435 
  // if pitch is centered or top set stick to zero
435 
        if (RCChannel(CH_PITCH) > -20)
 436 
  if (RCChannel(CH_PITCH) > -20)
436 
                stick = 0;
 437 
    stick = 0;
437 
        // if pitch is down trigger to next cal state
 438 
  // if pitch is down trigger to next cal state
438 
        if ((RCChannel(CH_PITCH) < -70) && !stick) {
 439 
  if ((RCChannel(CH_PITCH) < -70) && !stick) {
439 
                stick = 1;
 440 
    stick = 1;
440 
                return 1;
 441 
    return 1;
441 
        }
 442 
  }
442 
        return 0;
 443 
  return 0;