Subversion Repositories FlightCtrl

#include <inttypes.h>

#include "attitude.h"

#include "uart0.h"

#include "configuration.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;

uint8_t heightRampingTimer = 0;

int32_t maxHeightThisFlight;

int32_t iHeight;

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

  maxHeightThisFlight = 0;

  iHeight = 0;

}

void HC_update(void) {

  int32_t height = getHeight();

  static uint8_t setHeightLatch = 0;

  if (height > maxHeightThisFlight)

    maxHeightThisFlight = height;

  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_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.

    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 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;

        }

        // Time not yet reached.

        setHeightLatch++;

      }

    } else {

      // Switch is OFF.

      setHeightLatch = 0;

    }

  } else {

    // Switch is not activated; take the "max-height" as the target height.

    targetHeight = (uint16_t) dynamicParams.heightSetting * 100;

  }

  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {

    heightRampingTimer = 0;

    if (rampedTargetHeight < targetHeight) {

      // climbing

      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {

        rampedTargetHeight += staticParams.heightSlewRate;

      } else {

        rampedTargetHeight = targetHeight;

      }

    } else if (rampedTargetHeight != targetHeight) {

      // descending

      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {

        rampedTargetHeight -= staticParams.heightSlewRate;

      } else {

        rampedTargetHeight = targetHeight;

      }

    }

  }

  // height, in meters (so the division factor is: 100)

  debugOut.analog[30] = height / 10;

}

// 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[0] &= ~DEBUG_HEIGHT_DIFF;

    debugOut.digital[1] |= 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) << 10)

    /*+ 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.heightControlMaxThrottleChange)

    dThrottle = staticParams.heightControlMaxThrottleChange;

  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)

    dThrottle = -staticParams.heightControlMaxThrottleChange;

  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {

    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)

        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 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 */

#define DEFAULT_HOVERTHROTTLE 50

int32_t stronglyFilteredHeightDiff = 0;

uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;

uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;

#define HOVERTHROTTLEFILTER 25

  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?

 */