diff --git a/docs/Cli.md b/docs/Cli.md index 129b9bcd66..9980dc1c36 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -118,6 +118,7 @@ Re-apply any new defaults as desired. | disarm_kill_switch | Enabled by default. Disarms the motors independently of throttle value. Setting to 0 reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | 0 | 1 | 1 | Master | UINT8 | | auto_disarm_delay | | 0 | 60 | 5 | Master | UINT8 | | small_angle | If the copter tilt angle exceed this value the copter will refuse to arm. default is 25°. | 0 | 180 | 25 | Master | UINT8 | +| pid_at_min_throttle | If enabled, the copter will process the pid algorithm at minimum throttle. | 0 | 1 | 0 | Master | UINT8 | | flaps_speed | | 0 | 100 | 0 | Master | UINT8 | | fixedwing_althold_dir | | -1 | 1 | 1 | Master | INT8 | | reboot_character | | 48 | 126 | 82 | Master | UINT8 | diff --git a/src/main/config/config.c b/src/main/config/config.c index c8d44d675f..6994e231dc 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -119,7 +119,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 93; +static const uint8_t EEPROM_CONF_VERSION = 94; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -293,6 +293,16 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->alt_hold_fast_change = 1; } +void resetMixerConfig(mixerConfig_t *mixerConfig) { + mixerConfig->pid_at_min_throttle = 1; + mixerConfig->yaw_direction = 1; +#ifdef USE_SERVOS + mixerConfig->tri_unarmed_servo = 1; + mixerConfig->servo_lowpass_freq = 400; + mixerConfig->servo_lowpass_enable = 0; +#endif +} + uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; @@ -379,6 +389,8 @@ static void resetConf(void) masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; + resetMixerConfig(&masterConfig.mixerConfig); + masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.airplaneConfig.fixedwing_althold_dir = 1; @@ -450,11 +462,6 @@ static void resetConf(void) currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } - currentProfile->mixerConfig.yaw_direction = 1; - currentProfile->mixerConfig.tri_unarmed_servo = 1; - currentProfile->mixerConfig.servo_lowpass_freq = 400; - currentProfile->mixerConfig.servo_lowpass_enable = 0; - // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; #endif @@ -646,7 +653,7 @@ void activateConfig(void) #endif &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, - ¤tProfile->mixerConfig, + &masterConfig.mixerConfig, &masterConfig.airplaneConfig, &masterConfig.rxConfig ); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 79a093cf44..895fcf6c1a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -66,6 +66,9 @@ typedef struct master_t { uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 uint8_t small_angle; + // mixer-related configuration + mixerConfig_t mixerConfig; + airplaneConfig_t airplaneConfig; #ifdef GPS diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 1d7d285b59..7b2ccb5103 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -57,9 +57,6 @@ typedef struct profile_s { // Failsafe related configuration failsafeConfig_t failsafeConfig; - // mixer-related configuration - mixerConfig_t mixerConfig; - #ifdef GPS gpsProfile_t gpsProfile; #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d79db0ef71..42dcc4ca4d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -33,6 +33,7 @@ #include "drivers/pwm_mapping.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "drivers/system.h" #include "rx/rx.h" @@ -671,8 +672,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 +696,18 @@ void mixTable(void) motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); } } else { + // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, + // do not spin the motors. 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 if (mixerConfig->pid_at_min_throttle == 0) { + motor[i] = escAndServoConfig->minthrottle; + } } } } - } else { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 627199af3b..121ab8fa88 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -64,6 +64,7 @@ typedef struct mixer_t { } mixer_t; typedef struct mixerConfig_s { + uint8_t pid_at_min_throttle; // when enabled pids are used at minimum throttle int8_t yaw_direction; #ifdef USE_SERVOS uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 450a63b1a4..1c4a968f36 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -58,17 +58,20 @@ static escAndServoConfig_t *escAndServoConfig; static pidProfile_t *pidProfile; +// true if arming is done via the sticks (as opposed to a switch) static bool isUsingSticksToArm = true; int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e + bool isUsingSticksForArming(void) { return isUsingSticksToArm; } + bool areSticksInApModePosition(uint16_t ap_mode) { return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; 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..cbdd3ad723 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 }, @@ -381,12 +381,15 @@ const clivalue_t valueTable[] = { { "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_angle, 1, 900 }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, - { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, + + { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.mixerConfig.pid_at_min_throttle, 0, 1 }, + { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.yaw_direction, -1, 1 }, #ifdef USE_SERVOS - { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, - { "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400}, - { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, + { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.tri_unarmed_servo, 0, 1 }, + { "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, 10, 400}, + { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_enable, 0, 1 }, #endif + { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 }, diff --git a/src/main/mw.c b/src/main/mw.c index 0437c58b58..0344bc595e 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -525,12 +525,10 @@ void processRx(void) } // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough - if ( - ARMING_FLAG(ARMED) + if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && masterConfig.auto_disarm_delay != 0 - && isUsingSticksForArming() - ) { + && isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if ((int32_t)(disarmAt - millis()) < 0) // delay is over mwDisarm(); @@ -702,6 +700,15 @@ 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 (isUsingSticksForArming() && + 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); }