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?
*/