Subversion Repositories FlightCtrl

Rev

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

Rev 2160 Rev 2189
Line 1... Line 1...
1
#include <stdlib.h>
1
#include <stdlib.h>
2
#include <avr/io.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
3
#include <avr/interrupt.h>
Line 4... Line 4...
4
 
4
 
5
#include "rc.h"
5
#include "rc.h"
6
#include "controlMixer.h"
6
//#include "controlMixer.h"
7
#include "configuration.h"
7
#include "configuration.h"
8
#include "commands.h"
8
#include "commands.h"
Line 9... Line 9...
9
#include "output.h"
9
#include "definitions.h"
10
 
10
 
11
// The channel array is 0-based!
11
// The channel array is 0-based!
12
volatile int16_t PPM_in[MAX_CHANNELS];
12
volatile int16_t PPM_in[MAX_CONTROLCHANNELS];
13
volatile int16_t PPM_diff[MAX_CHANNELS];
13
volatile int16_t PPM_diff[MAX_CONTROLCHANNELS];
Line 14... Line 14...
14
volatile uint16_t RC_buffer[MAX_CHANNELS];
14
volatile uint16_t RC_buffer[MAX_CONTROLCHANNELS];
15
volatile uint8_t inBfrPnt = 0;
15
volatile uint8_t inBfrPnt;
16
 
16
 
Line 17... Line 17...
17
volatile uint8_t RCQuality;
17
volatile uint8_t RCQuality;
Line 18... Line 18...
18
uint8_t lastRCCommand = COMMAND_NONE;
18
uint8_t lastRCCommand;
19
uint8_t commandTimer = 0;
19
uint8_t commandTimer;
20
 
20
 
21
#define TIME(s) ((int16_t)(((long)F_CPU/(long)64000)*(float)s + 0.5f))
21
#define TIME(s) ((int16_t)(((F_CPU/8000)*(float)s + 0.5f)))
Line 46... Line 46...
46
    PORTD &= ~(1<<PORTD3);
46
    PORTD &= ~(1<<PORTD3);
47
  }
47
  }
Line 48... Line 48...
48
 
48
 
49
  // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
49
  // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
50
  // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
50
  // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
51
  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
51
  // Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=1)
52
  // Enable input capture noise cancler (bit: ICNC1=1)
52
  // Enable input capture noise canceler (bit: ICNC1=1)
53
  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
53
  // Therefore the counter increments at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
54
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
54
  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
55
  TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
55
  TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
56
  TCCR1B &= ~((1<<WGM13) | (1<<WGM12)  | (1<<CS12));
56
  TCCR1B &= ~((1<<WGM13) | (1<<WGM12)  | (1<<CS12));
57
  TCCR1B |= (1<<CS11) | (1<<CS10) | (1<<ICNC1);
57
  TCCR1B |= (1<<CS11)    | (1<<ICNC1);
Line 58... Line 58...
58
  TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
58
  TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
59
 
59
 
60
  if (channelMap.RCPolarity) {
60
  if (channelMap.RCPolarity) {
Line 69... Line 69...
69
  // Enable Input Capture Interrupt (bit: ICIE1=1)
69
  // Enable Input Capture Interrupt (bit: ICIE1=1)
70
  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
70
  // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
71
  // Enable Overflow Interrupt (bit: TOIE1=0)
71
  // Enable Overflow Interrupt (bit: TOIE1=0)
72
  TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
72
  TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
73
  TIMSK1 |= (1<<ICIE1);
73
  TIMSK1 |= (1<<ICIE1);
74
 
-
 
75
  RCQuality = 0;
74
  RCQuality = 0;
76
 
-
 
77
  SREG = sreg;
75
  SREG = sreg;
78
}
76
}
Line 79... Line 77...
79
 
77
 
80
/*
78
/*
Line 85... Line 83...
85
  uint16_t signal = (uint16_t)ICR1 - oldICR1;
83
  uint16_t signal = (uint16_t)ICR1 - oldICR1;
86
  oldICR1 = ICR1;
84
  oldICR1 = ICR1;
87
  //sync gap? (3.5 ms < signal < 25.6 ms)
85
  //sync gap? (3.5 ms < signal < 25.6 ms)
88
  if (signal > TIME(3.5)) {
86
  if (signal > TIME(3.5)) {
89
        inBfrPnt = 0;
87
        inBfrPnt = 0;
90
  } else if (inBfrPnt<MAX_CHANNELS) {
88
  } else if (inBfrPnt<MAX_CONTROLCHANNELS) {
91
        RC_buffer[inBfrPnt++] = signal;
89
        RC_buffer[inBfrPnt++] = signal;
92
        if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
90
        if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
93
  }
91
  }
94
}
92
}
Line 115... Line 113...
115
 the syncronization gap.
113
 the syncronization gap.
116
 */
114
 */
Line 117... Line 115...
117
 
115
 
118
void RC_process(void) {
116
void RC_process(void) {
119
  if (RCQuality) RCQuality--;
117
  if (RCQuality) RCQuality--;
120
  for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
118
  for (uint8_t channel=0; channel<MAX_CONTROLCHANNELS; channel++) {
121
        uint16_t signal = RC_buffer[channel];
119
        uint16_t signal = RC_buffer[channel];
122
        if (signal != 0) {
120
        if (signal != 0) {
123
          RC_buffer[channel] = 0; // reset to flag value already used.
121
          RC_buffer[channel] = 0; // reset to flag value already used.
124
      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
122
      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
125
        signal -= TIME(1.5);
123
        signal -= TIME(1.5) /*  + channelMap.HWTrim */;
126
        PPM_diff[channel] = signal - PPM_in[channel];
124
        PPM_diff[channel] = signal - PPM_in[channel];
127
        PPM_in[channel] = signal;
125
        PPM_in[channel] = signal;
128
      }
126
      }
129
    }
127
    }
130
  }
128
  }
Line 131... Line 129...
131
}
129
}
132
 
130
 
133
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
131
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
134
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
132
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
135
#define COMMAND_THRESHOLD 85
133
#define COMMAND_THRESHOLD TIME(0.35f)
Line 136... Line 134...
136
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
134
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
137
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
135
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
Line 158... Line 156...
158
}
156
}
Line 159... Line 157...
159
 
157
 
160
/*
158
/*
161
 * Get Pitch, Roll, Throttle, Yaw values
159
 * Get Pitch, Roll, Throttle, Yaw values
162
 */
160
 */
163
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
161
void RC_periodicTaskAndRPTY(int16_t* RPTY) {
164
  int16_t tmp1, tmp2;
162
  int16_t tmp1, tmp2;
165
  RC_process();
163
  RC_process();
166
  if (RCQuality) {
164
  if (RCQuality) {
167
    RCQuality--;
165
    RCQuality--;
168
    PRTY[CONTROL_PITCH]     = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
166
    RPTY[CONTROL_ROLL]      = ((RCChannel(CH_ROLL) * staticParams.stickP) >> 3) + RCDiff(CH_ROLL) * staticParams.stickD;
169
    PRTY[CONTROL_ROLL]      = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
167
    RPTY[CONTROL_PITCH]     = ((RCChannel(CH_PITCH) * staticParams.stickP) >> 3) + RCDiff(CH_PITCH) * staticParams.stickD;
170
    int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
168
    int16_t throttle        = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + TIME(0.4);
171
    // Negative throttle values are taken as zero.
169
    // Negative throttle values are taken as zero.
172
    if (throttle > 0)
170
    if (throttle > 0)
-
 
171
      RPTY[CONTROL_THROTTLE]  = throttle;
-
 
172
    else
-
 
173
      RPTY[CONTROL_THROTTLE]  = 0;
173
      PRTY[CONTROL_THROTTLE]  = throttle;
174
 
174
    tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
175
    tmp1 = RCChannel(CH_YAW); // - RCDiff(CH_YAW);
-
 
176
    // exponential stick sensitivity in yawing rate
175
    // exponential stick sensitivity in yawing rate
177
    // 
176
    tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo  y = ax + bx^2
178
    tmp2 = ((int32_t)staticParams.stickYawP * (int32_t)tmp1 * abs(tmp1)) >> 14; // expo  y = ax + bx^2
-
 
179
    tmp2 += (staticParams.stickYawP * tmp1) >> 3;
177
    tmp2 += (staticParams.stickYawP * tmp1) >> 2;
180
   
Line 178... Line 181...
178
    PRTY[CONTROL_YAW] = tmp2;
181
    RPTY[CONTROL_YAW] = (/*(RCChannel(CH_YAW) * staticParams.stickYawP) >> 3*/ tmp2);
-
 
182
 
179
 
183
    uint8_t command = RC_getStickCommand();
180
    uint8_t command = RC_getStickCommand();
184
 
181
    if (lastRCCommand == command) {
185
    if (lastRCCommand == command) {
182
      // Keep timer from overrunning.
186
      // Keep timer from overrunning.
183
      if (commandTimer < COMMAND_TIMER)
187
      if (commandTimer < COMMAND_TIMER)
Line 194... Line 198...
194
 * Get other channel value
198
 * Get other channel value
195
 */
199
 */
196
int16_t RC_getVariable(uint8_t varNum) {
200
int16_t RC_getVariable(uint8_t varNum) {
197
  if (varNum < 4)
201
  if (varNum < 4)
198
    // 0th variable is 5th channel (1-based) etc.
202
    // 0th variable is 5th channel (1-based) etc.
199
    return RCChannel(varNum + CH_POTS) + POT_OFFSET;
203
    return (RCChannel(varNum + CH_VARIABLES) >> 3) + VARIABLES_OFFSET;
200
  /*
204
  /*
201
   * Let's just say:
205
   * Let's just say:
202
   * The RC variable i is hardwired to channel i, i>=4
206
   * The RC variable i is hardwired to channel i, i>=4
203
   */
207
   */
204
  return PPM_in[varNum] + POT_OFFSET;
208
  return (PPM_in[varNum] >> 3) + VARIABLES_OFFSET;
205
}
209
}
Line 206... Line 210...
206
 
210
 
207
uint8_t RC_getSignalQuality(void) {
211
uint8_t RC_getSignalQuality(void) {
208
  if (RCQuality >= 160)
212
  if (RCQuality >= 160)
Line 259... Line 263...
259
 *    0
263
 *    0
260
 *
264
 *
261
 * Not in any of these positions: 0
265
 * Not in any of these positions: 0
262
 */
266
 */
Line 263... Line 267...
263
 
267
 
264
#define ARGUMENT_THRESHOLD 70
268
#define ARGUMENT_THRESHOLD TIME(0.35f)
265
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
269
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
Line 266... Line 270...
266
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
270
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
267
 
271