Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2188 → Rev 2189

/branches/dongfang_FC_rewrite/rc.c
3,22 → 3,22
#include <avr/interrupt.h>
 
#include "rc.h"
#include "controlMixer.h"
//#include "controlMixer.h"
#include "configuration.h"
#include "commands.h"
#include "output.h"
#include "definitions.h"
 
// The channel array is 0-based!
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint16_t RC_buffer[MAX_CHANNELS];
volatile uint8_t inBfrPnt = 0;
volatile int16_t PPM_in[MAX_CONTROLCHANNELS];
volatile int16_t PPM_diff[MAX_CONTROLCHANNELS];
volatile uint16_t RC_buffer[MAX_CONTROLCHANNELS];
volatile uint8_t inBfrPnt;
 
volatile uint8_t RCQuality;
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t commandTimer = 0;
uint8_t lastRCCommand;
uint8_t commandTimer;
 
#define TIME(s) ((int16_t)(((long)F_CPU/(long)64000)*(float)s + 0.5f))
#define TIME(s) ((int16_t)(((F_CPU/8000)*(float)s + 0.5f)))
 
/***************************************************************
* 16bit timer 1 is used to decode the PPM-Signal
48,13 → 48,13
 
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise cancler (bit: ICNC1=1)
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
// Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise canceler (bit: ICNC1=1)
// Therefore the counter increments at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
TCCR1B &= ~((1<<WGM13) | (1<<WGM12) | (1<<CS12));
TCCR1B |= (1<<CS11) | (1<<CS10) | (1<<ICNC1);
TCCR1B |= (1<<CS11) | (1<<ICNC1);
TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
 
if (channelMap.RCPolarity) {
71,9 → 71,7
// Enable Overflow Interrupt (bit: TOIE1=0)
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
TIMSK1 |= (1<<ICIE1);
 
RCQuality = 0;
 
SREG = sreg;
}
 
87,7 → 85,7
//sync gap? (3.5 ms < signal < 25.6 ms)
if (signal > TIME(3.5)) {
inBfrPnt = 0;
} else if (inBfrPnt<MAX_CHANNELS) {
} else if (inBfrPnt<MAX_CONTROLCHANNELS) {
RC_buffer[inBfrPnt++] = signal;
if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
}
117,12 → 115,12
 
void RC_process(void) {
if (RCQuality) RCQuality--;
for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
for (uint8_t channel=0; channel<MAX_CONTROLCHANNELS; channel++) {
uint16_t signal = RC_buffer[channel];
if (signal != 0) {
RC_buffer[channel] = 0; // reset to flag value already used.
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
signal -= TIME(1.5);
signal -= TIME(1.5) /* + channelMap.HWTrim */;
PPM_diff[channel] = signal - PPM_in[channel];
PPM_in[channel] = signal;
}
132,7 → 130,7
 
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
#define COMMAND_THRESHOLD 85
#define COMMAND_THRESHOLD TIME(0.35f)
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
160,24 → 158,30
/*
* Get Pitch, Roll, Throttle, Yaw values
*/
void RC_periodicTaskAndPRTY(int16_t* PRTY) {
void RC_periodicTaskAndRPTY(int16_t* RPTY) {
int16_t tmp1, tmp2;
RC_process();
if (RCQuality) {
RCQuality--;
PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
RPTY[CONTROL_ROLL] = ((RCChannel(CH_ROLL) * staticParams.stickP) >> 3) + RCDiff(CH_ROLL) * staticParams.stickD;
RPTY[CONTROL_PITCH] = ((RCChannel(CH_PITCH) * staticParams.stickP) >> 3) + RCDiff(CH_PITCH) * staticParams.stickD;
int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + TIME(0.4);
// Negative throttle values are taken as zero.
if (throttle > 0)
PRTY[CONTROL_THROTTLE] = throttle;
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
RPTY[CONTROL_THROTTLE] = throttle;
else
RPTY[CONTROL_THROTTLE] = 0;
 
tmp1 = RCChannel(CH_YAW); // - RCDiff(CH_YAW);
// exponential stick sensitivity in yawing rate
tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2
tmp2 += (staticParams.stickYawP * tmp1) >> 2;
PRTY[CONTROL_YAW] = tmp2;
//
tmp2 = ((int32_t)staticParams.stickYawP * (int32_t)tmp1 * abs(tmp1)) >> 14; // expo y = ax + bx^2
tmp2 += (staticParams.stickYawP * tmp1) >> 3;
RPTY[CONTROL_YAW] = (/*(RCChannel(CH_YAW) * staticParams.stickYawP) >> 3*/ tmp2);
 
uint8_t command = RC_getStickCommand();
 
if (lastRCCommand == command) {
// Keep timer from overrunning.
if (commandTimer < COMMAND_TIMER)
196,12 → 200,12
int16_t RC_getVariable(uint8_t varNum) {
if (varNum < 4)
// 0th variable is 5th channel (1-based) etc.
return RCChannel(varNum + CH_POTS) + POT_OFFSET;
return (RCChannel(varNum + CH_VARIABLES) >> 3) + VARIABLES_OFFSET;
/*
* Let's just say:
* The RC variable i is hardwired to channel i, i>=4
*/
return PPM_in[varNum] + POT_OFFSET;
return (PPM_in[varNum] >> 3) + VARIABLES_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
261,7 → 265,7
* Not in any of these positions: 0
*/
 
#define ARGUMENT_THRESHOLD 70
#define ARGUMENT_THRESHOLD TIME(0.35f)
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL