Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2073 → Rev 2074

/branches/dongfang_FC_rewrite/heightControl.c
14,10 → 14,12
 
#define LATCH_TIME 3
 
int32_t setHeight;
int32_t targetHeight;
//int32_t rampedTargetHeight;
 
//rampinguint8_t heightRampingTimer = 0;
uint16_t testOscPrescaler = 0;
uint8_t testOscTimer = 0;
 
int32_t maxHeightThisFlight;
int32_t iHeight;
 
24,7 → 26,8
void HC_setGround(void) {
analog_setGround();
// This should also happen when height control is enabled in-flight.
/*rampedTargetHeight = */ targetHeight = analog_getHeight();
setHeight = targetHeight = analog_getHeight();
testOscTimer = 0;
maxHeightThisFlight = 0;
iHeight = 0;
}
48,7 → 51,8
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;
setHeight = height;
testOscTimer = 0;
iHeight = 0;
}
// Time not yet reached.
60,7 → 64,7
}
} 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)
setHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it)
}
/*
83,7 → 87,27
}
}
*/
// uint8_t heightControlTestOscPeriod;
// uint8_t heightControlTestOscAmplitude;
 
testOscPrescaler++;
if (testOscPrescaler == 488) {
testOscPrescaler = 0;
testOscTimer++;
if (testOscTimer == staticParams.heightControlTestOscPeriod) {
testOscTimer = 0;
iHeight = 0;
} else if (testOscTimer == staticParams.heightControlTestOscPeriod/2) {
iHeight = 0;
}
}
 
if (testOscTimer < staticParams.heightControlTestOscPeriod/2)
targetHeight = setHeight;
else
targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * 25L;
 
//if (staticParams.)
// height, in meters (so the division factor is: 100)
// debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
// Calculated 0 alt number: 108205
90,6 → 114,10
// Experimental 0 alt number: 117100
}
 
#define LOG_PHEIGHT_SCALE 10
#define LOG_IHEIGHT_SCALE 24
#define LOG_DHEIGHT_SCALE 6
 
// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
115,7 → 143,7
}
 
int32_t heightErrorForIntegral = heightError;
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10;
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE;
 
if (heightErrorForIntegral > heightErrorForIntegralLimit) {
heightErrorForIntegral = heightErrorForIntegralLimit;
138,8 → 166,8
iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
}
 
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;
 
debugOut.analog[10] = dThrottleP;
debugOut.analog[11] = dThrottleI;