Subversion Repositories FlightCtrl

Rev

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
}