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;

}