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

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

}

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

        //DebugOut.Analog[18] = dThrottle;

        //DebugOut.Analog[19] = iHeight / 10000L;

        // TODO: Eliminate repitition.

        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;

        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?

 */