From 5e3734946ebb3950848a9213537fc4f191d6a6af Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 23:36:45 +0100 Subject: [PATCH] # This is a combination of 2 commits. # The first commit's message is: Previously, at minimum throttle, the quad would do absolutely no self-leveling and simply run the motors at constant minthrottle. This allowed the chance for the quad to lose control during flight if the throttle was set to minimum, say, to drop from a high altitude to a lower one. With this edit, the quad will still self-level at minimum throttle when armed, allowing for safe decents from altitude. To prevent motors spinning when arming/disarming, the yaw input is ignored if the throttle is at minimum and we're using the sticks to arm/disarm. Conflicts: src/main/flight/mixer.c # This is the 2nd commit message: added cli command disable_pid_at_min_throttle --- src/main/config/config.c | 1 + src/main/config/config_master.h | 1 + src/main/flight/mixer.c | 12 ++++++++---- src/main/io/rc_controls.c | 2 +- src/main/io/rc_controls.h | 2 ++ src/main/io/serial_cli.c | 3 ++- src/main/mw.c | 10 +++++++++- 7 files changed, 24 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c8d44d675f..55150f758a 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -378,6 +378,7 @@ static void resetConf(void) masterConfig.disarm_kill_switch = 1; masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; + masterConfig.disable_pid_at_min_throttle = 0; masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.airplaneConfig.fixedwing_althold_dir = 1; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 79a093cf44..357ecbc0f8 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -65,6 +65,7 @@ typedef struct master_t { uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 uint8_t small_angle; + uint8_t disable_pid_at_min_throttle; // when enabled (nonzero), ignore pids at minimum throttle airplaneConfig_t airplaneConfig; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d79db0ef71..ea74f20217 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -671,8 +671,12 @@ void mixTable(void) if (ARMING_FLAG(ARMED)) { + // Find the maximum motor output. int16_t maxMotor = motor[0]; for (i = 1; i < motorCount; i++) { + // If one motor is above the maxthrottle threshold, we reduce the value + // of all motors by the amount of overshoot. That way, only one motor + // is at max and the relative power of each motor is preserved. if (motor[i] > maxMotor) { maxMotor = motor[i]; } @@ -691,16 +695,16 @@ void mixTable(void) motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); } } else { + // Don't spin the motors if FEATURE_MOTOR_STOP is enabled and we're + // at minimum throttle. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); if ((rcData[THROTTLE]) < rxConfig->mincheck) { - if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = escAndServoConfig->minthrottle; - else + if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; + } } } } - } else { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 450a63b1a4..27dfcd251f 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -64,7 +64,7 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e -bool isUsingSticksForArming(void) +bool areUsingSticksToArm(void) { return isUsingSticksToArm; } diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 3a8a5e846a..33821938c0 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -136,6 +136,8 @@ typedef struct rcControlsConfig_s { uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement } rcControlsConfig_t; +bool areUsingSticksToArm(void); + bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e04b6187ad..511d0c12f5 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -254,7 +254,7 @@ const clivalue_t valueTable[] = { { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT }, - { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX }, + { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE, &masterConfig.inputFilteringMode, 0, 1 }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, @@ -274,6 +274,7 @@ const clivalue_t valueTable[] = { { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.disarm_kill_switch, 0, 1 }, { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, 0, 60 }, { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, 0, 180 }, + { "disable_pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.disable_pid_at_min_throttle, 0, 1 }, { "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 }, diff --git a/src/main/mw.c b/src/main/mw.c index 0437c58b58..fb5f854569 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -529,7 +529,7 @@ void processRx(void) ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && masterConfig.auto_disarm_delay != 0 - && isUsingSticksForArming() + && areUsingSticksToArm() ) { if (throttleStatus == THROTTLE_LOW) { if ((int32_t)(disarmAt - millis()) < 0) // delay is over @@ -702,6 +702,14 @@ void loop(void) } #endif + // If we're armed, at minimum throttle, and we do arming via the + // sticks, do not process yaw input from the rx. We do this so the + // motors do not spin up while we are trying to arm or disarm. + if (areUsingSticksToArm() && + rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) { + rcCommand[YAW] = 0; + } + if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); }