Rev 1161 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1161 | Rev 1162 | ||
---|---|---|---|
Line 8... | Line 8... | ||
8 | #include "parameter.h" |
8 | #include "parameter.h" |
9 | #include "fc.h" |
9 | #include "fc.h" |
10 | #include "pitch.h" |
10 | #include "pitch.h" |
Line 11... | Line 11... | ||
11 | 11 | ||
- | 12 | #define STICK_FACTOR ((int16_t) 256) |
|
- | 13 | #define MAX_STICK 125 |
|
Line 12... | Line 14... | ||
12 | #define STICK_FACTOR ((int16_t) 256) |
14 | #define MIN_STICK -MAX_STICK |
Line 13... | Line 15... | ||
13 | 15 | ||
14 | static int16_t stickValue; // Aktueller Stick-Wert |
16 | static int16_t stickValue; // Aktueller Stick-Wert |
Line 32... | Line 34... | ||
32 | // rawStickValue = ((rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1)) / (PARAM_INC_GAS_SCALE+1); |
34 | // rawStickValue = ((rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1)) / (PARAM_INC_GAS_SCALE+1); |
33 | // rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1); |
35 | // rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1); |
34 | // rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1); |
36 | // rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1); |
Line 35... | Line 37... | ||
35 | 37 | ||
36 | if (rawStickValue > 3) { |
38 | if (rawStickValue > 3) { |
37 | if (rawStickValue > 125) |
39 | if (rawStickValue > MAX_STICK) |
38 | rawStickValue = 125; |
40 | rawStickValue = MAX_STICK; |
39 | rawStickValue = expTable[rawStickValue] * PARAM_INC_GAS_P_SCALE; |
41 | rawStickValue = expTable[rawStickValue] * PARAM_INC_GAS_P_SCALE; |
40 | } else if (rawStickValue < -3) { |
42 | } else if (rawStickValue < -3) { |
41 | if (rawStickValue < -125) |
43 | if (rawStickValue < MIN_STICK) |
42 | rawStickValue = -125; |
44 | rawStickValue = MIN_STICK; |
43 | rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE; |
45 | rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE; |
Line 44... | Line 46... | ||
44 | } |
46 | } |
Line 45... | Line 47... | ||
45 | 47 | ||
46 | DebugOut.Analog[23] = rawStickValue; |
48 | DebugOut.Analog[23] = rawStickValue; |
47 | 49 | ||
48 | // Neuer Stick-Wert |
50 | // Neuer Stick-Wert |
49 | if (rawStickValue < 0 && stickValue < -125*STICK_FACTOR - rawStickValue) // ToDo |
51 | if (rawStickValue < 0 && stickValue < MIN_STICK*STICK_FACTOR - rawStickValue) // ToDo |
50 | stickValue = -125*STICK_FACTOR; |
52 | stickValue = MIN_STICK*STICK_FACTOR; |
51 | else if (rawStickValue > 0 && stickValue > 125*STICK_FACTOR - rawStickValue ) |
53 | else if (rawStickValue > 0 && stickValue > MAX_STICK*STICK_FACTOR - rawStickValue ) |
Line 52... | Line 54... | ||
52 | stickValue = 125*STICK_FACTOR; |
54 | stickValue = MAX_STICK*STICK_FACTOR; |
53 | else if (rawStickValue != 0) |
55 | else if (rawStickValue != 0) |
54 | stickValue += rawStickValue; |
56 | stickValue += rawStickValue; |
55 | 57 | ||
56 | if (rawStickValue * rawStickDiff > 0) { // Add only when moving awy from center |
58 | if (rawStickValue * rawStickDiff > 0) { // Add only when moving awy from center |
57 | if (abs(stickValue) < 125*STICK_FACTOR - abs(rawStickDiff * PARAM_INC_GAS_D_SCALE)) { // Do not wrap around |
59 | if (abs(stickValue) < MAX_STICK*STICK_FACTOR - abs(rawStickDiff * PARAM_INC_GAS_D_SCALE)) { // Do not wrap around |
Line 58... | Line 60... | ||
58 | stickValue += rawStickDiff * PARAM_INC_GAS_D_SCALE; |
60 | stickValue += rawStickDiff * PARAM_INC_GAS_D_SCALE; |
59 | DebugOut.Analog[24] = rawStickDiff * PARAM_INC_GAS_D_SCALE; |
61 | DebugOut.Analog[24] = rawStickDiff * PARAM_INC_GAS_D_SCALE; |
Line 60... | Line 62... | ||
60 | } |
62 | } |
61 | } |
63 | } |
Line 62... | Line 64... | ||
62 | 64 | ||
Line 63... | Line 65... | ||
63 | DebugOut.Analog[26] = stickValue; |
65 | DebugOut.Analog[26] = stickValue; |
64 | DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + 125; |
66 | DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + MAX_STICK; |
Line 65... | Line 67... | ||
65 | 67 | ||
Line 66... | Line 68... | ||
66 | return (stickValue / STICK_FACTOR) + 125; //0..+250 |
68 | return (stickValue / STICK_FACTOR) + MAX_STICK; //0..+250 |
67 | } |
69 | } |