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 GAS_SCALE       ((uint8_t) 4)

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

int pitch_inc_value( void ) {

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

        // Neuer Stick-Wert

        if (rawStickValue < 0 && stickValue < -INT16_MAX - rawStickValue) // ToDo

            stickValue = -INT16_MAX;

        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 = (int) MIN_GAS * STICK_FACTOR;

    pitchValueFP = pitch_inc_value;

}