Subversion Repositories FlightCtrl

#include <inttypes.h>
#include "attitude.h"
#include "uart0.h"
#include "configuration.h"
#include "controlMixer.h"

// for digital / LED debug.
#include "output.h"

// For scope debugging only!
#include "rc.h"

#define INTEGRAL_LIMIT 100000

/*
 #define DEBUGINTEGRAL 0
 #define DEBUGDIFFERENTIAL 0
 #define DEBUGHOVERTHROTTLE 0
 #define DEBUGHEIGHTSWITCH 0
 */

#define LATCH_TIME 40

int32_t groundPressure;

int32_t targetHeight;
int32_t rampedTargetHeight;

#define DEFAULT_HOVERTHROTTLE 50
int32_t stronglyFilteredHeightDiff = 0;
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
#define HOVERTHROTTLEFILTER 25

uint8_t heightRampingTimer = 0;
int32_t maxHeight;
int32_t iHeight;
/*
 These parameters are free to take:
 uint8_t HeightMinGas;           // Value : 0-100
 uint8_t HeightD;                // Value : 0-250
 uint8_t MaxHeight;              // Value : 0-32
 uint8_t HeightP;                // Value : 0-32
 uint8_t Height_Gain;            // Value : 0-50
 uint8_t Height_ACC_Effect;      // Value : 0-250
 */

int32_t getHeight(void) {
        return groundPressure - filteredAirPressure;
}

void HC_setGround(void) {
        groundPressure = filteredAirPressure;
        // This should also happen when height control is enabled in-flight.
        rampedTargetHeight = getHeight();
        maxHeight = 0;
        iHeight = 0;
}

void HC_update(void) {
        int32_t height = getHeight();
        static uint8_t setHeightLatch = 0;

        if (height > maxHeight)
                maxHeight = height;

        if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
                // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
                // DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
                if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) {
                        // Switch is ON
                        if (setHeightLatch <= LATCH_TIME) {
                                if (setHeightLatch == LATCH_TIME) {
                                        // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
                                        targetHeight = height;
                                        // DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
                                }
                                // Time not yet reached.
                                setHeightLatch++;
                        }
                } else {
                        // Switch is OFF.
                        setHeightLatch = 0;
                        // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
                }
        } else {
                // Switch is not activated; take the "max-height" as the target height.
                // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
                targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
        }

        if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
                heightRampingTimer = 0;
                if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
                        rampedTargetHeight += staticParams.Height_Gain;
                } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
                        rampedTargetHeight -= staticParams.Height_Gain;
                }
        }

        // height, in meters (so the division factor is: 100)
        DebugOut.Analog[30] = height / 10;
}

// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH
// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH

// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
uint16_t HC_getThrottle(uint16_t throttle) {
        int32_t height = getHeight();
        int32_t heightError = rampedTargetHeight - height;

        static int32_t lastHeight;

        int16_t dHeight = height - lastHeight;
        lastHeight = height;

        // DebugOut.Analog[20] = dHeight;
        // DebugOut.Analog[21] = dynamicParams.MaxHeight;

        // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
        // iHeight += heightError;

        if (dHeight > 0) {
                DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF;
                DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF;
        } else if (dHeight < 0) {
                DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF;
                DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF;
        }

        /*
         if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
         else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
         else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
         else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
         */

        int16_t dThrottle = heightError * staticParams.HeightP / 1000
                        /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
                                        * staticParams.HeightD;

        // the "minGas" is now a limit for how much up / down the throttle can be varied
        if (dThrottle > staticParams.HeightMinGas)
                dThrottle = staticParams.HeightMinGas;
        else if (dThrottle < -staticParams.HeightMinGas)
                dThrottle = -staticParams.HeightMinGas;

        // TODO: Eliminate repetition.
        if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
                if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH)
                                || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) {
                        // If switch is not in use --> Just apply height control.
                        // If switch is in use     --> only apply height control when switch is also ON.
                        throttle += dThrottle;
                }
        }

        /* Experiment: Find hover-throttle */
        stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
                        * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
        stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
                        - 1) + throttle) / HOVERTHROTTLEFILTER;

        if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
                        && stronglyFilteredHeightDiff > -3) {
                hoverThrottle = stronglyFilteredThrottle;
                DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE;
                // DebugOut.Analog[18] = hoverThrottle;
        } else
                DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE;

    DebugOut.Analog[20] = dThrottle;
    DebugOut.Analog[21] = hoverThrottle;

        return throttle;
}

/*
 For a variometer thingy:
 When switch is thrown on, freeze throttle (capture it into variable)
 For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
 Output = frozen throttle + whatever is computed +/-. Integral?
 */