11,68 → 11,38 |
|
#define STICK_FACTOR ((int16_t) 256) |
|
static int16_t stickValue; // Aktueller Stick-Wert |
static int16_t stickValue = INT16_MIN; // 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 ) { |
|
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 |
int32_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
// int16_t rawStickValue = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; |
|
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 < -125*STICK_FACTOR - rawStickValue) // ToDo |
stickValue = -125*STICK_FACTOR; |
else if (rawStickValue > 0 && stickValue > 125*STICK_FACTOR - rawStickValue ) |
stickValue = 125*STICK_FACTOR; |
if (rawStickValue < 0 && stickValue < INT16_MIN - rawStickValue) // ToDo |
stickValue = INT16_MIN; |
else if (rawStickValue > 0 && stickValue > INT16_MAX - rawStickValue ) |
stickValue = INT16_MAX; |
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) + 125; |
DebugOut.Analog[31] = stickValue / STICK_FACTOR; |
|
return (stickValue / STICK_FACTOR) + 125; //0..+250 |
return stickValue / STICK_FACTOR + 125; |
} |
|
void pitch_inc_init( void ) { |
|
printf("\r\npitch_inc_init()"); |
printf("\r\n sizeof(expTable) = %d\r\n", sizeof(expTable)); |
|
|
stickValue = -125*STICK_FACTOR; |
stickValue = INT16_MIN; |
|
pitchValueFP = pitch_inc_value; |
} |