Subversion Repositories FlightCtrl

Rev

Rev 1161 | Go to most recent revision | 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 
}