Subversion Repositories FlightCtrl

Rev

Rev 1160 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1160 Rev 1161
Line 9... Line 9...
9 
#include "fc.h"
 9 
#include "fc.h"
10 
#include "pitch.h"
 10 
#include "pitch.h"
Line 11... Line 11...
11 
 
 11 
 
Line 12... Line 12...
12 
#define STICK_FACTOR    ((int16_t) 256)
 12 
#define STICK_FACTOR    ((int16_t) 256)
- 
 
 13 
 
- 
 
 14 
static int16_t          stickValue;                             // Aktueller Stick-Wert
- 
 
 15 
 
- 
 
 16 
static unsigned char expTable[] = {                             // Tabelle zum Mappen auf exponentielle Werten
- 
 
 17 
    1,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,4,4,4,4,4,4,5,5,5,5,6,
- 
 
 18 
    6,6,6,7,7,7,8,8,8,9,9,10,10,10,11,11,12,12,13,14,14,15,15,16,17,18,18,19,20,21,22,
- 
 
 19 
    23,24,25,26,27,29,30,31,33,34,36,37,39,41,43,45,47,49,51,53,56,58,61,64,66,69,73,
- 
 
 20 
    76,79,83,87,91,95,99,103,108,113,118,123,129,135,141,147,154,161,168,176,184,192,
Line 13... Line 21...
13 
 
 21 
    200,210,219,229,239,250
Line -... Line 22...
- 
 
 22 
};
14 
static int16_t          stickValue = INT16_MIN;                 // Aktueller Stick-Wert
 23 
 
15 
 
 24 
int pitch_inc_value( void ) {
Line 16... Line 25...
16 
int pitch_inc_value( void ) {
 25 
 
Line 17... Line 26...
17 
 
 26 
        int8_t  rawStickDiff  = PPM_diff[EE_Parameter.Kanalbelegung[K_GAS]];      //-125..+125
18 
        int32_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
 27 
//      int32_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];        //-125..+125
19 
//      int16_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
 28 
        int16_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];        //-125..+125
Line -... Line 29...
- 
 
 29 
 
- 
 
 30 
        DebugOut.Analog[25] = rawStickValue;
- 
 
 31 
 
- 
 
 32 
//      rawStickValue = ((rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1))  / (PARAM_INC_GAS_SCALE+1);
- 
 
 33 
//      rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1);
- 
 
 34 
//      rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1);
- 
 
 35 
 
- 
 
 36 
        if (rawStickValue > 3) {
- 
 
 37 
            if (rawStickValue > 125)
- 
 
 38 
                rawStickValue = 125;
- 
 
 39 
            rawStickValue = expTable[rawStickValue] * PARAM_INC_GAS_P_SCALE;
- 
 
 40 
        } else if (rawStickValue < -3) {
20 
 
 41 
            if (rawStickValue < -125)
21 
        DebugOut.Analog[25] = rawStickValue;
 42 
                rawStickValue = -125;
22 
 
 43 
            rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE;
23 
//      rawStickValue = ((rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1))  / (PARAM_INC_GAS_SCALE+1);
 44 
        }
24 
        rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1);
 45 
       
25 
//      rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1);
 46 
        DebugOut.Analog[23] = rawStickValue;
26 
 
 47 
 
- 
 
 48 
        // Neuer Stick-Wert
- 
 
 49 
        if (rawStickValue < 0 && stickValue < -125*STICK_FACTOR - rawStickValue) // ToDo
- 
 
 50 
            stickValue = -125*STICK_FACTOR;
- 
 
 51 
        else if (rawStickValue > 0 && stickValue > 125*STICK_FACTOR - rawStickValue )
- 
 
 52 
            stickValue = 125*STICK_FACTOR;
- 
 
 53 
        else if (rawStickValue != 0)
- 
 
 54 
            stickValue += rawStickValue;
Line 27... Line 55...
27 
        // Neuer Stick-Wert
 55 
 
28 
        if (rawStickValue < 0 && stickValue < INT16_MIN - rawStickValue) // ToDo
 56 
        if (rawStickValue * rawStickDiff > 0) {     // Add only when moving awy from center
Line 29... Line 57...
29 
            stickValue = INT16_MIN;
 57 
            if (abs(stickValue) < 125*STICK_FACTOR - abs(rawStickDiff * PARAM_INC_GAS_D_SCALE)) {  // Do not wrap around
30 
        else if (rawStickValue > 0 && stickValue > INT16_MAX - rawStickValue )
 58 
                stickValue += rawStickDiff * PARAM_INC_GAS_D_SCALE;
Line 31... Line 59...
31 
            stickValue = INT16_MAX;
 59 
                DebugOut.Analog[24] = rawStickDiff * PARAM_INC_GAS_D_SCALE;
Line 32... Line 60...
32 
        else if (rawStickValue != 0)
 60 
            }
- 
 
 61 
        }
- 
 
 62 
               
Line 33... Line 63...
33 
            stickValue += rawStickValue;
 63 
        DebugOut.Analog[26] = stickValue;
Line 34... Line 64...
34 
               
 64 
        DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + 125;
35 
        DebugOut.Analog[26] = stickValue;
 65