10,6 → 10,8 |
#include "pitch.h" |
|
#define STICK_FACTOR ((int16_t) 256) |
#define MAX_STICK 125 |
#define MIN_STICK -MAX_STICK |
|
static int16_t stickValue; // Aktueller Stick-Wert |
|
34,12 → 36,12 |
// rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1); |
|
if (rawStickValue > 3) { |
if (rawStickValue > 125) |
rawStickValue = 125; |
if (rawStickValue > MAX_STICK) |
rawStickValue = MAX_STICK; |
rawStickValue = expTable[rawStickValue] * PARAM_INC_GAS_P_SCALE; |
} else if (rawStickValue < -3) { |
if (rawStickValue < -125) |
rawStickValue = -125; |
if (rawStickValue < MIN_STICK) |
rawStickValue = MIN_STICK; |
rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE; |
} |
|
46,15 → 48,15 |
DebugOut.Analog[23] = rawStickValue; |
|
// Neuer Stick-Wert |
if (rawStickValue < 0 && stickValue < -125*STICK_FACTOR - rawStickValue) // ToDo |
stickValue = -125*STICK_FACTOR; |
else if (rawStickValue > 0 && stickValue > 125*STICK_FACTOR - rawStickValue ) |
stickValue = 125*STICK_FACTOR; |
if (rawStickValue < 0 && stickValue < MIN_STICK*STICK_FACTOR - rawStickValue) // ToDo |
stickValue = MIN_STICK*STICK_FACTOR; |
else if (rawStickValue > 0 && stickValue > MAX_STICK*STICK_FACTOR - rawStickValue ) |
stickValue = MAX_STICK*STICK_FACTOR; |
else if (rawStickValue != 0) |
stickValue += rawStickValue; |
|
if (rawStickValue * rawStickDiff > 0) { // Add only when moving awy from center |
if (abs(stickValue) < 125*STICK_FACTOR - abs(rawStickDiff * PARAM_INC_GAS_D_SCALE)) { // Do not wrap around |
if (abs(stickValue) < MAX_STICK*STICK_FACTOR - abs(rawStickDiff * PARAM_INC_GAS_D_SCALE)) { // Do not wrap around |
stickValue += rawStickDiff * PARAM_INC_GAS_D_SCALE; |
DebugOut.Analog[24] = rawStickDiff * PARAM_INC_GAS_D_SCALE; |
} |
61,9 → 63,9 |
} |
|
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + 125; |
DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + MAX_STICK; |
|
return (stickValue / STICK_FACTOR) + 125; //0..+250 |
return (stickValue / STICK_FACTOR) + MAX_STICK; //0..+250 |
} |
|
void pitch_inc_init( void ) { |
72,7 → 74,7 |
printf("\r\n sizeof(expTable) = %d\r\n", sizeof(expTable)); |
|
|
stickValue = -125*STICK_FACTOR; |
stickValue = MIN_STICK*STICK_FACTOR; |
|
pitchValueFP = pitch_inc_value; |
} |