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