rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Extract Altitude Hold code from mw main loop into methods.

This commit is contained in:
Dominic Clifton 2014-06-21 23:42:11 +01:00
parent 9afcb20b7e
commit aeca17a0a9
1 changed files with 75 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
src/main/mw.c
View File

@ -348,14 +348,84 @@ void updateInflightCalibrationState(void)
}
}
#ifdef BARO
static int16_t initialThrottleHold;
void multirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
// multirotor alt hold
if (currentProfile.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
errorAltitudeI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
AltHold += AltHoldCorr / 2000;
AltHoldCorr %= 2000;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
AltHoldCorr = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
}
}
void fixedWingAltHold()
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
}
void updateAltHold()
{
if (f.FIXED_WING) {
fixedWingAltHold();
} else {
multirotorAltHold();
}
}
void updateAltHoldState()
{
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else {
f.BARO_MODE = 0;
}
}
#endif
void loop(void)
{
int i;
#ifdef BARO
static int16_t initialThrottleHold;
#endif
static uint32_t loopTime;
uint32_t auxState = 0;
@ -444,18 +514,7 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else {
f.BARO_MODE = 0;
}
updateAltHoldState();
}
#endif
@ -569,44 +628,7 @@ void loop(void)
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
if (!f.FIXED_WING) {
// multirotor alt hold
if (currentProfile.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
errorAltitudeI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
AltHold += AltHoldCorr / 2000;
AltHoldCorr %= 2000;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
AltHoldCorr = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
}
} else {
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
}
updateAltHold();
}
}
#endif