Subversion Repositories FlightCtrl

#include <inttypes.h>
#include "analog.h"
#include "attitude.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 LATCH_TIME 40

int32_t targetHeight;
int32_t rampedTargetHeight;

uint8_t heightRampingTimer = 0;
int32_t maxHeightThisFlight;
int32_t iHeight;

void HC_setGround(void) {
  analog_setGround();
  // This should also happen when height control is enabled in-flight.
  rampedTargetHeight = targetHeight = analog_getHeight();
  maxHeightThisFlight = 0;
  iHeight = 0;
}

void HC_periodicTask(void) {
  int32_t height = analog_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.
          rampedTargetHeight = 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 * 25L - 500L; // should be: 100 (or make a param out of it)
  }
 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
    heightRampingTimer = 0;
    if (rampedTargetHeight < targetHeight) {
      // climbing
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
        rampedTargetHeight += staticParams.heightSlewRate;
      } else {
        rampedTargetHeight = targetHeight;
      }
    } else {
      // descending
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
        rampedTargetHeight -= staticParams.heightSlewRate;
      } else {
        rampedTargetHeight = targetHeight;
      }
    }
  }

  // height, in meters (so the division factor is: 100)
  // debugOut.analog[31] = (117100 - filteredAirPressure) / 100;
  // Calculated 0 alt number: 108205
  // Experimental 0 alt number: 117100
}

// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
  HC_periodicTask();
  int16_t throttle = PRTY[CONTROL_THROTTLE];
  int32_t height = analog_getHeight();
  int32_t heightError = rampedTargetHeight - height;
  int16_t dHeight = analog_getDHeight();
  //lastHeight = height;
 
  if (heightError > 0) {
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
  } else {
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
  }

  if (dHeight > 0) {
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
  } else {
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
  }

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

#define IHEIGHT_SCALE 24
  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
  int16_t dThrottleI =  (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);

  if (dThrottleI > staticParams.heightControlMaxIntegral) {
    dThrottleI = staticParams.heightControlMaxIntegral;
    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
  } else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
    dThrottleI = -staticParams.heightControlMaxIntegral;
    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
  }

  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;

  debugOut.analog[24] = dThrottleP;
  debugOut.analog[25] = dThrottleI;
  debugOut.analog[26] = dThrottleD;

  //debugOut.analog[27] = dynamicParams.heightP;
  //debugOut.analog[28] = dynamicParams.heightI;
  //debugOut.analog[29] = dynamicParams.heightD;

  int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;

  if (dThrottle > staticParams.heightControlMaxThrottleChange)
    dThrottle = staticParams.heightControlMaxThrottleChange;
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
    dThrottle = -staticParams.heightControlMaxThrottleChange;

  debugOut.analog[19] = throttle;
  debugOut.analog[20] = dThrottle;
  debugOut.analog[21] = height;
  debugOut.analog[22] = rampedTargetHeight;
  debugOut.analog[23] = heightError;

  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;
  } else
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;

    */
 
  PRTY[CONTROL_THROTTLE] = 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?
 */