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)

static int16_t          stickValue = INT16_MIN;                 // Aktueller Stick-Wert

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]];

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

        // 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;

        else if (rawStickValue != 0)

            stickValue += rawStickValue;

        DebugOut.Analog[26] = stickValue;

        DebugOut.Analog[31] = stickValue / STICK_FACTOR;

        return stickValue / STICK_FACTOR  + 125;

}

void pitch_inc_init( void ) {

    printf("\r\npitch_inc_init()");

    stickValue = INT16_MIN;

    pitchValueFP = pitch_inc_value;

}