Subversion Repositories FlightCtrl

Rev

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

Rev 2099 Rev 2102
Line 9... Line 9...
9 
#include "output.h"
 9 
#include "output.h"
Line 10... Line 10...
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_CHANNELS];
- 
 
 13 
volatile uint8_t RCQuality;
13 
volatile uint8_t RCQuality;
 14 
 
14 
uint8_t lastRCCommand = COMMAND_NONE;
 15 
uint8_t lastRCCommand = COMMAND_NONE;
Line -... Line 16...
- 
 
 16 
uint8_t commandTimer = 0;
- 
 
 17 
 
15 
uint8_t commandTimer = 0;
 18 
uint8_t lastFlightMode = FLIGHT_MODE_NONE;
16 
 
 19 
 
17 
/***************************************************************
 20 
/***************************************************************
18 
 *  16bit timer 1 is used to decode the PPM-Signal            
 21 
 *  16bit timer 1 is used to decode the PPM-Signal            
19 
 ***************************************************************/
 22 
 ***************************************************************/
Line 147... Line 150...
147 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
 150 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
148 
#define COMMAND_THRESHOLD 85
 151 
#define COMMAND_THRESHOLD 85
149 
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
 152 
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
150 
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 153 
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
Line -... Line 154...
- 
 
 154 
 
- 
 
 155 
#define RC_SCALING 4
- 
 
 156 
 
- 
 
 157 
uint8_t getControlModeSwitch() {
- 
 
 158 
        int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
- 
 
 159 
        uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
- 
 
 160 
                (channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
- 
 
 161 
        return flightMode;
- 
 
 162 
}
- 
 
 163 
 
- 
 
 164 
// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice.
- 
 
 165 
// Maybe simply: Very very low throttle.
- 
 
 166 
// Throttle xlow for COMMAND_TIMER: GYROCAL (once).
- 
 
 167 
// mode switched: CHMOD
- 
 
 168 
 
- 
 
 169 
uint8_t RC_getCommand(void) {
- 
 
 170 
        uint8_t flightMode = getControlModeSwitch();
- 
 
 171 
 
- 
 
 172 
        if (lastFlightMode != flightMode) {
- 
 
 173 
                lastFlightMode = flightMode;
- 
 
 174 
                lastRCCommand = COMMAND_CHMOD;
- 
 
 175 
                return lastRCCommand;
- 
 
 176 
        }
- 
 
 177 
 
- 
 
 178 
        int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
- 
 
 179 
        if (channel <= -32) { // <= 900 us
- 
 
 180 
                if (commandTimer == COMMAND_TIMER) {
- 
 
 181 
                        lastRCCommand = COMMAND_GYROCAL;
- 
 
 182 
                }
- 
 
 183 
                if (commandTimer <= COMMAND_TIMER) {
- 
 
 184 
                        commandTimer++;
- 
 
 185 
                }
- 
 
 186 
        } else commandTimer = 0;
- 
 
 187 
        return lastRCCommand;
- 
 
 188 
}
- 
 
 189 
 
- 
 
 190 
uint8_t RC_getArgument(void) {
- 
 
 191 
        return lastFlightMode;
- 
 
 192 
}
151 
 
 193 
 
152 
/*
 194 
/*
153 
 * Get Pitch, Roll, Throttle, Yaw values
 195 
 * Get Pitch, Roll, Throttle, Yaw values
154 
 */
 196 
 */
155 
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
 197 
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
156 
  if (RCQuality) {
 198 
  if (RCQuality) {
157 
    RCQuality--;
 199 
    RCQuality--;
158 
    PRTY[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR);
 200 
    PRTY[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR) * RC_SCALING;
159 
    PRTY[CONTROL_AILERONS]   = RCChannel(CH_AILERONS);
 201 
    PRTY[CONTROL_AILERONS]   = RCChannel(CH_AILERONS) * RC_SCALING;
160 
    PRTY[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);
 202 
    PRTY[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE) * RC_SCALING;
- 
 
 203 
    PRTY[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)   * RC_SCALING;
- 
 
 204 
 
- 
 
 205 
    debugOut.analog[16] = PRTY[CONTROL_ELEVATOR];
Line 161... Line 206...
161 
    PRTY[CONTROL_RUDDER]     = RCChannel(CH_RUDDER);
 206 
    debugOut.analog[17] = PRTY[CONTROL_THROTTLE];
162 
 
 207 
 
163 
    uint8_t command = COMMAND_NONE; //RC_getStickCommand();
 208 
    uint8_t command = COMMAND_NONE; //RC_getStickCommand();
164 
    if (lastRCCommand == command) {
 209 
    if (lastRCCommand == command) {
Line 207... Line 252...
207 
 * of a stick, it may be useful.
 252 
 * of a stick, it may be useful.
208 
 */
 253 
 */
209 
void RC_calibrate(void) {
 254 
void RC_calibrate(void) {
210 
  // Do nothing.
 255 
  // Do nothing.
211 
}
 256 
}
212 
 
 - 
 
213 
uint8_t RC_getCommand(void) {
 - 
 
214 
  if (commandTimer == COMMAND_TIMER) {
 - 
 
215 
    // Stick has been held long enough; command committed.
 - 
 
216 
    return lastRCCommand;
 - 
 
217 
  }
 - 
 
218 
  // Not yet sure what the command is.
 - 
 
219 
  return COMMAND_NONE;
 - 
 
220 
}
 - 
 
221 
 
 - 
 
222 
/*
 - 
 
223 
 * Command arguments on R/C:
 - 
 
224 
 * 2--3--4
 - 
 
225 
 * |     |  +
 - 
 
226 
 * 1  0  5  ^ 0
 - 
 
227 
 * |     |  |  
 - 
 
228 
 * 8--7--6
 - 
 
229 
 *    
 - 
 
230 
 * + <--
 - 
 
231 
 *    0
 - 
 
232 
 *
 - 
 
233 
 * Not in any of these positions: 0
 - 
 
234 
 */
 - 
 
235 
 
 - 
 
236 
#define ARGUMENT_THRESHOLD 70
 - 
 
237 
#define ARGUMENT_CHANNEL_VERTICAL CH_ELEVATOR
 - 
 
238 
#define ARGUMENT_CHANNEL_HORIZONTAL CH_AILERONS
 - 
 
239 
 
 - 
 
240 
uint8_t RC_getArgument(void) {
 - 
 
241 
  if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
 - 
 
242 
    // vertical is up
 - 
 
243 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
 - 
 
244 
      return 2;
 - 
 
245 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
 - 
 
246 
      return 4;
 - 
 
247 
    return 3;
 - 
 
248 
  } else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
 - 
 
249 
    // vertical is down
 - 
 
250 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
 - 
 
251 
      return 8;
 - 
 
252 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
 - 
 
253 
      return 6;
 - 
 
254 
    return 7;
 - 
 
255 
  } else {
 - 
 
256 
    // vertical is around center
 - 
 
257 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
 - 
 
258 
      return 1;
 - 
 
259 
    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
 - 
 
260 
      return 5;
 - 
 
261 
    return 0;
 - 
 
262 
  }
 - 
 
263 
}
 -