| 11,38 → 11,68 |
|---|
| |
| #define STICK_FACTOR ((int16_t) 256) |
| |
| static int16_t stickValue = INT16_MIN; // Aktueller Stick-Wert |
| static int16_t stickValue; // Aktueller Stick-Wert |
| |
| static unsigned char expTable[] = { // Tabelle zum Mappen auf exponentielle Werten |
| 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, |
| 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, |
| 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, |
| 76,79,83,87,91,95,99,103,108,113,118,123,129,135,141,147,154,161,168,176,184,192, |
| 200,210,219,229,239,250 |
| }; |
| |
| int pitch_inc_value( void ) { |
| |
| int32_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
| // int16_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
| int8_t rawStickDiff = PPM_diff[EE_Parameter.Kanalbelegung[K_GAS]]; //-125..+125 |
| // int32_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; //-125..+125 |
| int16_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; //-125..+125 |
| |
| DebugOut.Analog[25] = rawStickValue; |
| |
| // rawStickValue = ((rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1)) / (PARAM_INC_GAS_SCALE+1); |
| rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1); |
| // rawStickValue = (rawStickValue * rawStickValue * rawStickValue) / (PARAM_INC_GAS_SCALE+1); |
| // rawStickValue = (rawStickValue * rawStickValue) / ((rawStickValue > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1); |
| |
| if (rawStickValue > 3) { |
| if (rawStickValue > 125) |
| rawStickValue = 125; |
| rawStickValue = expTable[rawStickValue] * PARAM_INC_GAS_P_SCALE; |
| } else if (rawStickValue < -3) { |
| if (rawStickValue < -125) |
| rawStickValue = -125; |
| rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE; |
| } |
| |
| DebugOut.Analog[23] = rawStickValue; |
| |
| // Neuer Stick-Wert |
| if (rawStickValue < 0 && stickValue < INT16_MIN - rawStickValue) // ToDo |
| stickValue = INT16_MIN; |
| else if (rawStickValue > 0 && stickValue > INT16_MAX - rawStickValue ) |
| stickValue = INT16_MAX; |
| if (rawStickValue < 0 && stickValue < -125*STICK_FACTOR - rawStickValue) // ToDo |
| stickValue = -125*STICK_FACTOR; |
| else if (rawStickValue > 0 && stickValue > 125*STICK_FACTOR - rawStickValue ) |
| stickValue = 125*STICK_FACTOR; |
| else if (rawStickValue != 0) |
| stickValue += rawStickValue; |
| |
| if (rawStickValue * rawStickDiff > 0) { // Add only when moving awy from center |
| if (abs(stickValue) < 125*STICK_FACTOR - abs(rawStickDiff * PARAM_INC_GAS_D_SCALE)) { // Do not wrap around |
| stickValue += rawStickDiff * PARAM_INC_GAS_D_SCALE; |
| DebugOut.Analog[24] = rawStickDiff * PARAM_INC_GAS_D_SCALE; |
| } |
| } |
| |
| DebugOut.Analog[26] = stickValue; |
| DebugOut.Analog[31] = stickValue / STICK_FACTOR; |
| DebugOut.Analog[31] = (stickValue / STICK_FACTOR) + 125; |
| |
| return stickValue / STICK_FACTOR + 125; |
| return (stickValue / STICK_FACTOR) + 125; //0..+250 |
| } |
| |
| void pitch_inc_init( void ) { |
| |
| printf("\r\npitch_inc_init()"); |
| printf("\r\n sizeof(expTable) = %d\r\n", sizeof(expTable)); |
| |
| stickValue = INT16_MIN; |
| |
| stickValue = -125*STICK_FACTOR; |
| |
| pitchValueFP = pitch_inc_value; |
| } |