Rev 2069 |
Rev 2074 |
Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#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 3
int32_t targetHeight;
//int32_t rampedTargetHeight;
//rampinguint8_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;
}
uint8_t HC_isSwitchOn() {
return (dynamicParams.heightSetting >= 255/3);
}
void HC_periodicTask(void) {
int32_t height = analog_getHeight();
static uint8_t setHeightLatch = 0;
if (height > maxHeightThisFlight)
maxHeightThisFlight = height;
// debugOut.analog[25] = dynamicParams.heightSetting;
if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
if (HC_isSwitchOn()) {
// 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;
iHeight = 0;
}
// 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[24] = (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 = /*ramped*/ targetHeight - height;
int16_t dHeight = analog_getDHeight();
debugOut.analog[22] = height/10L;
debugOut.analog[23] = dHeight;
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) || HC_isSwitchOn()) {
// 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?
*/