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