Rev 1160 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1157 | krheinwald | 1 | /* pitch_inc.c |
| 2 | * |
||
| 3 | * Copyright 2009 Klaus Rheinwald |
||
| 4 | * |
||
| 5 | */ |
||
| 6 | |||
| 7 | #include "main.h" |
||
| 8 | #include "parameter.h" |
||
| 9 | #include "fc.h" |
||
| 10 | #include "pitch.h" |
||
| 11 | |||
| 12 | #define STICK_FACTOR ((int16_t) 256) |
||
| 13 | |||
| 1161 | krheinwald | 14 | static int16_t stickValue; // Aktueller Stick-Wert |
| 1157 | krheinwald | 15 | |
| 1161 | krheinwald | 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, |
||
| 21 | 200,210,219,229,239,250 |
||
| 22 | }; |
||
| 23 | |||
| 1157 | krheinwald | 24 | int pitch_inc_value( void ) { |
| 25 | |||
| 1161 | krheinwald | 26 | int8_t rawStickDiff = PPM_diff[EE_Parameter.Kanalbelegung[K_GAS]]; //-125..+125 |
| 27 | // int32_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; //-125..+125 |
||
| 28 | int16_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; //-125..+125 |
||
| 1157 | krheinwald | 29 | |
| 30 | DebugOut.Analog[25] = rawStickValue; |
||
| 31 | |||
| 1160 | krheinwald | 32 | // rawStickValue = ((rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1)) / (PARAM_INC_GAS_SCALE+1); |
| 1161 | krheinwald | 33 | // rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1); |
| 1160 | krheinwald | 34 | // rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1); |
| 1157 | krheinwald | 35 | |
| 1161 | krheinwald | 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) { |
||
| 41 | if (rawStickValue < -125) |
||
| 42 | rawStickValue = -125; |
||
| 43 | rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE; |
||
| 44 | } |
||
| 45 | |||
| 46 | DebugOut.Analog[23] = rawStickValue; |
||
| 47 | |||
| 1157 | krheinwald | 48 | // Neuer Stick-Wert |
| 1161 | krheinwald | 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; |
||
| 1157 | krheinwald | 53 | else if (rawStickValue != 0) |
| 54 | stickValue += rawStickValue; |
||
| 1161 | krheinwald | 55 | |
| 56 | 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 |
||
| 58 | stickValue += rawStickDiff * PARAM_INC_GAS_D_SCALE; |
||
| 59 | DebugOut.Analog[24] = rawStickDiff * PARAM_INC_GAS_D_SCALE; |
||
| 60 | } |
||
| 61 | } |
||
| 1157 | krheinwald | 62 | |
| 63 | DebugOut.Analog[26] = stickValue; |
||
| 1161 | krheinwald | 64 | DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + 125; |
| 1157 | krheinwald | 65 | |
| 1161 | krheinwald | 66 | return (stickValue / STICK_FACTOR) + 125; //0..+250 |
| 1157 | krheinwald | 67 | } |
| 68 | |||
| 69 | void pitch_inc_init( void ) { |
||
| 70 | |||
| 71 | printf("\r\npitch_inc_init()"); |
||
| 1161 | krheinwald | 72 | printf("\r\n sizeof(expTable) = %d\r\n", sizeof(expTable)); |
| 1157 | krheinwald | 73 | |
| 1161 | krheinwald | 74 | |
| 75 | stickValue = -125*STICK_FACTOR; |
||
| 1157 | krheinwald | 76 | |
| 77 | pitchValueFP = pitch_inc_value; |
||
| 78 | } |
||
| 79 |