Subversion Repositories FlightCtrl

/* pitch_inc.c
 *
 * Copyright 2009 Klaus Rheinwald
 *
 */

#include "main.h"
#include "parameter.h"
#include "fc.h"
#include "pitch.h"

#define STICK_FACTOR    ((int16_t) 256)
#define MAX_STICK       125
#define MIN_STICK       -MAX_STICK

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 ) {

        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 > 0 ? PARAM_INC_GAS_SCALE : -PARAM_INC_GAS_SCALE) +1);

        if (rawStickValue > 3) {
            if (rawStickValue > MAX_STICK)
                rawStickValue = MAX_STICK;
            rawStickValue = expTable[rawStickValue] * PARAM_INC_GAS_P_SCALE;
        } else if (rawStickValue < -3) {
            if (rawStickValue < MIN_STICK)
                rawStickValue = MIN_STICK;
            rawStickValue = -expTable[-rawStickValue] * PARAM_INC_GAS_P_SCALE;
        }
       
        DebugOut.Analog[23] = rawStickValue;

        // Neuer Stick-Wert
        if (rawStickValue < 0 && stickValue < MIN_STICK*STICK_FACTOR - rawStickValue) // ToDo
            stickValue = MIN_STICK*STICK_FACTOR;
        else if (rawStickValue > 0 && stickValue > MAX_STICK*STICK_FACTOR - rawStickValue )
            stickValue = MAX_STICK*STICK_FACTOR;
        else if (rawStickValue != 0)
            stickValue += rawStickValue;

        if (rawStickValue * rawStickDiff > 0) {     // Add only when moving awy from center
            if (abs(stickValue) < MAX_STICK*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) + MAX_STICK;

        return (stickValue / STICK_FACTOR) + MAX_STICK;        //0..+250
}

void pitch_inc_init( void ) {

    printf("\r\npitch_inc_init()");
    printf("\r\n sizeof(expTable) = %d\r\n", sizeof(expTable));


    stickValue = MIN_STICK*STICK_FACTOR;
   
    pitchValueFP = pitch_inc_value;
}