From 5e3734946ebb3950848a9213537fc4f191d6a6af Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 23:36:45 +0100 Subject: [PATCH 1/5] # 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 c8d44d675..55150f758 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 79a093cf4..357ecbc0f 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 d79db0ef7..ea74f2021 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 450a63b1a..27dfcd251 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 3a8a5e846..33821938c 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 e04b6187a..511d0c12f 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 0437c58b5..fb5f85456 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); } From 1b1163da102c858b099426a3499af59459b72c82 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 23:42:18 +0100 Subject: [PATCH 2/5] 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 added cli command disable_pid_at_min_throttle (same as previous) --- src/main/config/config.c | 1 + src/main/config/config_master.h | 2 +- src/main/flight/mixer.c | 2 ++ 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 55150f758..45861c92e 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -380,6 +380,7 @@ static void resetConf(void) 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 357ecbc0f..6de9f68a1 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -65,7 +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 + 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 ea74f2021..f222d7887 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -701,6 +701,8 @@ void mixTable(void) if ((rcData[THROTTLE]) < rxConfig->mincheck) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; + } else if (masterConfig.disable_pid_at_min_throttle != 0) { + motor[i] = escAndServoConfig->minthrottle; } } } From acabbf41db4537697280d813c21e08c19749ee20 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 23:44:03 +0100 Subject: [PATCH 3/5] 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 --- docs/Cli.md | 1 + src/main/flight/mixer.c | 23 +++++++++++++++++++++-- src/main/io/rc_controls.c | 5 ++++- src/main/mw.c | 9 ++++----- 4 files changed, 30 insertions(+), 8 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 129b9bcd6..7c140be87 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 | +| disable_pid_at_min_throttle | If enabled, the copter will not 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/flight/mixer.c b/src/main/flight/mixer.c index f222d7887..d504bc473 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -25,31 +25,48 @@ #include "common/axis.h" #include "common/maths.h" +#include "common/color.h" #include "drivers/system.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" +#include "drivers/pwm_rx.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "drivers/serial.h" +#include "drivers/system.h" #include "rx/rx.h" #include "io/gimbal.h" #include "io/escservo.h" #include "io/rc_controls.h" +#include "io/gps.h" +#include "io/serial.h" +#include "io/ledstrip.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "sensors/barometer.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/lowpass.h" +#include "flight/failsafe.h" +#include "flight/navigation.h" + +#include "telemetry/telemetry.h" #include "config/runtime_config.h" #include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 @@ -695,8 +712,10 @@ 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. + // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, + // do not spin the motors. + // If we're at minimum throttle and disable_pid_at_min_throttle + // is enabled, spin motors at minimum throttle. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); if ((rcData[THROTTLE]) < rxConfig->mincheck) { if (feature(FEATURE_MOTOR_STOP)) { diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 27dfcd251..1c4a968f3 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 areUsingSticksToArm(void) + +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/mw.c b/src/main/mw.c index fb5f85456..0344bc595 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 - && areUsingSticksToArm() - ) { + && isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if ((int32_t)(disarmAt - millis()) < 0) // delay is over mwDisarm(); @@ -705,11 +703,12 @@ void loop(void) // 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() && + 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); } From ed434fe47b5865dd55907a59c0bfb84aac62dce5 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 22:50:27 +0000 Subject: [PATCH 4/5] Use a positive named setting and variable instead of a negative one to simplify the logic and aid understanding. --- docs/Cli.md | 2 +- src/main/config/config.c | 2 +- src/main/config/config_master.h | 2 +- src/main/flight/mixer.c | 4 +--- src/main/io/serial_cli.c | 2 +- 5 files changed, 5 insertions(+), 7 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 7c140be87..9980dc1c3 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -118,7 +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 | -| disable_pid_at_min_throttle | If enabled, the copter will not process the pid algorithm at minimum throttle. | 0 | 1 | 0 | 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 45861c92e..91620e0a5 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -378,7 +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.pid_at_min_throttle = 1; masterConfig.airplaneConfig.flaps_speed = 0; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 6de9f68a1..ef6f584c1 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -65,7 +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 + uint8_t pid_at_min_throttle; // when enabled pids are used at minimum throttle airplaneConfig_t airplaneConfig; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d504bc473..961b6675b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -714,13 +714,11 @@ void mixTable(void) } else { // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, // do not spin the motors. - // If we're at minimum throttle and disable_pid_at_min_throttle - // is enabled, spin motors 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->mincommand; - } else if (masterConfig.disable_pid_at_min_throttle != 0) { + } else if (masterConfig.pid_at_min_throttle == 0) { motor[i] = escAndServoConfig->minthrottle; } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 511d0c12f..3624e34ce 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -274,7 +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 }, + { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_at_min_throttle, 0, 1 }, { "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 }, From 4a12d00d1e23fbab5f7c0effda5bf07b79f7a1d5 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Mar 2015 23:00:22 +0000 Subject: [PATCH 5/5] Moving mixer config out of the profile. It doesn't really make sense. --- src/main/config/config.c | 21 +++++++++++++-------- src/main/config/config_master.h | 4 +++- src/main/config/config_profile.h | 3 --- src/main/flight/mixer.c | 18 +----------------- src/main/flight/mixer.h | 1 + src/main/io/serial_cli.c | 12 +++++++----- 6 files changed, 25 insertions(+), 34 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 91620e0a5..6994e231d 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; @@ -378,8 +388,8 @@ static void resetConf(void) masterConfig.disarm_kill_switch = 1; masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; - masterConfig.pid_at_min_throttle = 1; + resetMixerConfig(&masterConfig.mixerConfig); masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.airplaneConfig.fixedwing_althold_dir = 1; @@ -452,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 @@ -648,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 ef6f584c1..895fcf6c1 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -65,7 +65,9 @@ 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 pid_at_min_throttle; // when enabled pids are used at minimum throttle + + // mixer-related configuration + mixerConfig_t mixerConfig; airplaneConfig_t airplaneConfig; diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 1d7d285b5..7b2ccb510 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 961b6675b..42dcc4ca4 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -25,17 +25,14 @@ #include "common/axis.h" #include "common/maths.h" -#include "common/color.h" #include "drivers/system.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" -#include "drivers/pwm_rx.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/serial.h" #include "drivers/system.h" #include "rx/rx.h" @@ -43,30 +40,17 @@ #include "io/gimbal.h" #include "io/escservo.h" #include "io/rc_controls.h" -#include "io/gps.h" -#include "io/serial.h" -#include "io/ledstrip.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" -#include "sensors/boardalignment.h" -#include "sensors/gyro.h" -#include "sensors/battery.h" -#include "sensors/barometer.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/lowpass.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" - -#include "telemetry/telemetry.h" #include "config/runtime_config.h" #include "config/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" #define GIMBAL_SERVO_PITCH 0 #define GIMBAL_SERVO_ROLL 1 @@ -718,7 +702,7 @@ void mixTable(void) if ((rcData[THROTTLE]) < rxConfig->mincheck) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; - } else if (masterConfig.pid_at_min_throttle == 0) { + } else if (mixerConfig->pid_at_min_throttle == 0) { motor[i] = escAndServoConfig->minthrottle; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 627199af3..121ab8fa8 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/serial_cli.c b/src/main/io/serial_cli.c index 3624e34ce..cbdd3ad72 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -274,7 +274,6 @@ 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 }, - { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_at_min_throttle, 0, 1 }, { "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 }, @@ -382,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 },