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 |
|