From 9a7ff04422e0d3419364c429c7713c18f0a18f02 Mon Sep 17 00:00:00 2001 From: dongie Date: Mon, 6 Jan 2014 21:38:36 +0900 Subject: [PATCH] adding untested (and needing work) fixedwing althold implementation. see comments in code for things to fix. added new variable to cli, fixedwing_althold_dir (though its value isn't really clear, I think it should always be positive since servo direction mix is done later). --- src/cli.c | 1 + src/config.c | 4 +++- src/mixer.c | 8 ++++++++ src/mw.c | 50 +++++++++++++++++++++++++++++--------------------- src/mw.h | 2 ++ 5 files changed, 43 insertions(+), 22 deletions(-) diff --git a/src/cli.c b/src/cli.c index 535d413114..a2fe7b357b 100644 --- a/src/cli.c +++ b/src/cli.c @@ -124,6 +124,7 @@ const clivalue_t valueTable[] = { { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 }, { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 }, { "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 }, + { "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 }, { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 }, { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 9600, 19200 }, { "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 }, diff --git a/src/config.c b/src/config.c index 3be24b835e..0fbcf4f241 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 55; +static const uint8_t EEPROM_CONF_VERSION = 56; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -197,6 +197,8 @@ static void resetConf(void) mcfg.mincheck = 1100; mcfg.maxcheck = 1900; mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right + mcfg.flaps_speed = 0; + mcfg.fixedwing_althold_dir = 1; // Motor/ESC/Servo mcfg.minthrottle = 1150; mcfg.maxthrottle = 1850; diff --git a/src/mixer.c b/src/mixer.c index 8c71a44c3a..5402ad0b7d 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -211,6 +211,14 @@ void mixerInit(void) } } } + + // set flag that we're on something with wings + if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || + mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) + f.FIXED_WING = 1; + else + f.FIXED_WING = 0; + mixerResetMotors(); } diff --git a/src/mw.c b/src/mw.c index 1248bff6c6..eb0cc37ea8 100755 --- a/src/mw.c +++ b/src/mw.c @@ -831,34 +831,42 @@ void loop(void) if (f.BARO_MODE) { static uint8_t isAltHoldChanged = 0; static int16_t AltHoldCorr = 0; - if (cfg.alt_hold_fast_change) { - // rapid alt changes - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { - errorAltitudeI = 0; - isAltHoldChanged = 1; - rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral; + if (!f.FIXED_WING) { + // multirotor alt hold + if (cfg.alt_hold_fast_change) { + // rapid alt changes + if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { + errorAltitudeI = 0; + isAltHoldChanged = 1; + rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral; + } else { + if (isAltHoldChanged) { + AltHold = EstAlt; + isAltHoldChanged = 0; + } + rcCommand[THROTTLE] = initialThrottleHold + BaroPID; + } } else { - if (isAltHoldChanged) { + // slow alt changes for apfags + if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { + // 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] = initialThrottleHold + BaroPID; + rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle); } } else { - // slow alt changes for apfags - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { - // 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] = initialThrottleHold + BaroPID; - rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle); + // 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 * mcfg.fixedwing_althold_dir; } } } diff --git a/src/mw.h b/src/mw.h index eb1dbbb08d..75578a84eb 100755 --- a/src/mw.h +++ b/src/mw.h @@ -265,6 +265,7 @@ typedef struct master_t { uint16_t maxcheck; // maximum rc end uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster + int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable. @@ -314,6 +315,7 @@ typedef struct flags_t { uint8_t SMALL_ANGLES_25; uint8_t CALIBRATE_MAG; uint8_t VARIO_MODE; + uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code } flags_t; extern int16_t gyroZero[3];