From 392e31a9a51ca2af7e98c5bf194638ae5b11e4ee Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 8 May 2018 15:14:41 +0200 Subject: [PATCH] Feature SERVO_TILT removed --- docs/Cli.md | 2 +- src/main/fc/cli.c | 2 +- src/main/fc/config.h | 6 ++--- src/main/fc/fc_core.c | 5 ---- src/main/fc/fc_msp_box.c | 4 +-- src/main/flight/mixer.h | 1 - src/main/flight/servos.c | 57 ++++++++++++++-------------------------- 7 files changed, 27 insertions(+), 50 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 810cf3f76f..ec36da864a 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -341,7 +341,7 @@ Re-apply any new defaults as desired. | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | -| gimbal_mode | NORMAL | When feature SERVO_TILT is enabled, this can be either NORMAL or MIXTILT | +| gimbal_mode | NORMAL | This can be either NORMAL or MIXTILT. NORMAL mean each axis is outputed to separated servos. MIXTILT combines PITCH and ROLL. Refer to gimbal documentation | | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | | fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index beaf1831a7..2e0243bd81 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -141,7 +141,7 @@ static const char* const emptyName = "-"; // sync this with features_e static const char * const featureNames[] = { "RX_PPM", "VBAT", "TX_PROF_SEL", "", "MOTOR_STOP", - "SERVO_TILT", "SOFTSERIAL", "GPS", "", + "", "SOFTSERIAL", "GPS", "", "", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 96bdaad7f1..0f2cfb8c59 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -48,7 +48,7 @@ typedef enum { FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_UNUSED_2 = 1 << 3, // RX_SERIAL FEATURE_MOTOR_STOP = 1 << 4, - FEATURE_SERVO_TILT = 1 << 5, + NOT_USED_10 = 1 << 5, // was FEATURE_SERVO_TILT FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE @@ -63,13 +63,13 @@ typedef enum { FEATURE_DASHBOARD = 1 << 17, FEATURE_UNUSED_7 = 1 << 18, // Unused in INAV FEATURE_BLACKBOX = 1 << 19, - FEATURE_UNUSED_10 = 1 << 20, // was FEATURE_CHANNEL_FORWARDING + FEATURE_UNUSED_10 = 1 << 20, // was FEATURE_CHANNEL_FORWARDING FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_VTX = 1 << 24, FEATURE_UNUSED_8 = 1 << 25, // RX_SPI - FEATURE_UNUSED_9 = 1 << 26, //SOFTSPI + FEATURE_UNUSED_9 = 1 << 26, //SOFTSPI FEATURE_PWM_SERVO_DRIVER = 1 << 27, FEATURE_PWM_OUTPUT_ENABLE = 1 << 28, FEATURE_OSD = 1 << 29, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4f6a9118cb..304b9ac52c 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -802,11 +802,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processServoAutotrim(); } - // Servo tilt is not part of servo mixer, but uses servos - if (feature(FEATURE_SERVO_TILT)) { - processServoTilt(); - } - //Servos should be filtered or written only when mixer is using servos or special feaures are enabled if (isServoOutputEnabled()) { writeServos(); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index bd0880c788..1587c21ea0 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -173,8 +173,8 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; } - if (feature(FEATURE_SERVO_TILT)) - activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; + //Camstab mode is enabled always + activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; #ifdef USE_GPS if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index da0fc992c4..7fe3baa62c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -95,7 +95,6 @@ void mixerUpdateStateFlags(void); void mixerResetDisarmedMotors(void); void mixTable(void); void writeMotors(void); -void processServoTilt(void); void processServoAutotrim(void); void stopMotors(void); void stopPwmAllMotors(void); \ No newline at end of file diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 6071b0809d..f056a6c2f9 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -143,9 +143,6 @@ void servosInit(void) mixerUsesServos = 1; } - // enable servos for mixes that require them. note, this shifts motor counts. - servoOutputEnabled = mixerUsesServos || feature(FEATURE_SERVO_TILT); - for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) servoComputeScalingFactors(i); @@ -214,7 +211,6 @@ void writeServos(void) filterServos(); int servoIndex = 0; - bool zeroServoValue = false; /* @@ -224,22 +220,13 @@ void writeServos(void) zeroServoValue = true; } - // Write mixer servo outputs - // mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos - if (mixerUsesServos && servoRuleCount) { - for (int i = minServoIndex; i <= maxServoIndex; i++) { - if (zeroServoValue) { - pwmWriteServo(servoIndex++, 0); - } else { - pwmWriteServo(servoIndex++, servo[i]); - } + for (int i = minServoIndex; i <= maxServoIndex; i++) { + if (zeroServoValue) { + pwmWriteServo(servoIndex++, 0); + } else { + pwmWriteServo(servoIndex++, servo[i]); } } - - if (feature(FEATURE_SERVO_TILT)) { - pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]); - pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]); - } } void servoMixer(float dT) @@ -265,8 +252,21 @@ void servoMixer(float dT) input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0; - input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); - input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); + if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { + const int gimbalPitch = scaleRange(attitude.values.pitch, -1800, 1800, -360, +360); + const int gimbalRoll = scaleRange(attitude.values.roll, -1800, 1800, -360, +360); + + if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) { + input[INPUT_GIMBAL_PITCH] = gimbalPitch - gimbalRoll; + input[INPUT_GIMBAL_ROLL] = gimbalPitch + gimbalRoll; + } else { + input[INPUT_GIMBAL_PITCH] = gimbalPitch; + input[INPUT_GIMBAL_ROLL] = gimbalRoll; + } + } else { + input[INPUT_GIMBAL_PITCH] = 0; + input[INPUT_GIMBAL_ROLL] = 0; + } input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] @@ -346,23 +346,6 @@ void servoMixer(float dT) } } -void processServoTilt(void) -{ - // center at fixed position, or vary either pitch or roll by RC channel - servo[SERVO_GIMBAL_PITCH] = servoParams(SERVO_GIMBAL_PITCH)->middle; - servo[SERVO_GIMBAL_ROLL] = servoParams(SERVO_GIMBAL_PITCH)->middle; - - if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) { - servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 - (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; - servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 + (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; - } else { - servo[SERVO_GIMBAL_PITCH] += (int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch / 50; - servo[SERVO_GIMBAL_ROLL] += (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; - } - } -} - #define SERVO_AUTOTRIM_TIMER_MS 2000 typedef enum {