From b595b49ca842a352c1cb8334892d2193dcd4cfa0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 7 Mar 2015 15:09:42 +0000 Subject: [PATCH] Decouple roll and pitch rates. MSP clients take note of updated MSP_RC_TUNING/MSP_SET_RC_TUNING commands. --- src/main/config/config.c | 7 +++++-- src/main/flight/mixer.c | 13 +++++++------ src/main/flight/pid.c | 31 +++++++++++++++++++++---------- src/main/io/display.c | 22 +++++++++------------- src/main/io/rc_controls.c | 10 ++++++---- src/main/io/rc_controls.h | 3 +-- src/main/io/serial_cli.c | 5 +++-- src/main/io/serial_msp.c | 26 ++++++++++++++++---------- src/main/mw.c | 4 ++-- 9 files changed, 70 insertions(+), 51 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 179dd0895..3f8ff440d 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -275,12 +275,15 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 90; controlRateConfig->rcExpo8 = 65; - controlRateConfig->rollPitchRate = 0; - controlRateConfig->yawRate = 0; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 0; controlRateConfig->tpa_breakpoint = 1500; + + for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { + controlRateConfig->rates[axis] = 0; + } + } void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c766ad665..d79db0ef7 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -546,12 +546,6 @@ static void airplaneMixer(void) void mixTable(void) { uint32_t i; - int8_t yawDirection3D = 1; - - // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { - yawDirection3D = -1; - } if (motorCount > 3) { // prevent "yaw jump" during yaw correction @@ -570,6 +564,13 @@ void mixTable(void) } #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) + int8_t yawDirection3D = 1; + + // Reverse yaw servo when inverted in 3D mode + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + yawDirection3D = -1; + } + // airplane / servo mixes switch (currentMixerMode) { case MIXER_BI: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f9558874..a009a5e52 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -139,9 +139,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode + uint8_t rate = controlRateConfig->rates[axis]; + if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate - AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f; + AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; } else { // calculate error and limit the angle to the max inclination #ifdef GPS @@ -163,7 +165,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa AngleRate = errorAngle * pidProfile->A_level; } else { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate + AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; @@ -310,14 +312,21 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; int32_t delta; - if (FLIGHT_MODE(HORIZON_MODE)) prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); + if (FLIGHT_MODE(HORIZON_MODE)) { + prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); + } // PITCH & ROLL for (axis = 0; axis < 2; axis++) { + rc = rcCommand[axis] << 1; + error = rc - (gyroData[axis] / 4); errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here - if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; + + if (ABS(gyroData[axis]) > (640 * 4)) { + errorGyroI[axis] = 0; + } ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 @@ -372,7 +381,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; #ifdef ALIENWII32 error = rc - gyroData[FD_YAW]; #else @@ -483,7 +492,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; #ifdef ALIENWII32 error = rc - gyroData[FD_YAW]; #else @@ -603,7 +612,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) Mwii3msTimescale /= 3000.0f; if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now - PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; + PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f); PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { @@ -614,7 +623,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; } } else { - int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; + int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5; error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better if (ABS(tmp) > 50) { @@ -657,9 +666,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { + uint8_t rate = controlRateConfig->rates[axis]; + // -----Get the desired angle rate depending on flight mode if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[YAW]) >> 5); + AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5); } else { // calculate error and limit the angle to max configured inclination #ifdef GPS @@ -677,7 +688,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat #endif if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4; + AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRateTmp to add a little auto-level feel AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8; diff --git a/src/main/io/display.c b/src/main/io/display.c index 65f94d217..11566c76b 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -271,26 +271,22 @@ void showProfilePage(void) controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex); - tfp_sprintf(lineBuffer, "RC Expo: %d", controlRateConfig->rcExpo8); + tfp_sprintf(lineBuffer, "Expo: %d, Rate: %d", + controlRateConfig->rcExpo8, + controlRateConfig->rcRate8 + ); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "RC Rate: %d", controlRateConfig->rcRate8); + tfp_sprintf(lineBuffer, "Rates R:%d P:%d Y:%d", + controlRateConfig->rates[FD_ROLL], + controlRateConfig->rates[FD_PITCH], + controlRateConfig->rates[FD_YAW] + ); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "R&P Rate: %d", controlRateConfig->rollPitchRate); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "Yaw Rate: %d", controlRateConfig->yawRate); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - } #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0])) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index e14683dc0..efd0761d2 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -402,12 +402,14 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm generateThrottleCurve(controlRateConfig, escAndServoConfig); break; case ADJUSTMENT_PITCH_ROLL_RATE: - newValue = (int)controlRateConfig->rollPitchRate + delta; - controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)controlRateConfig->rates[FD_PITCH] + delta; + controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)controlRateConfig->rates[FD_ROLL] + delta; + controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; case ADJUSTMENT_YAW_RATE: - newValue = (int)controlRateConfig->yawRate + delta; - controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)controlRateConfig->rates[FD_YAW] + delta; + controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a1549f5c5..56180c9af 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -122,8 +122,7 @@ typedef struct controlRateConfig_s { uint8_t rcExpo8; uint8_t thrMid8; uint8_t thrExpo8; - uint8_t rollPitchRate; - uint8_t yawRate; + uint8_t rates[3]; uint8_t dynThrPID; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated } controlRateConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3a90d0726..fb9240095 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -392,8 +392,9 @@ const clivalue_t valueTable[] = { { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 }, { "thr_mid", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, 0, 100 }, { "thr_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, 0, 100 }, - { "roll_pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rollPitchRate, 0, 100 }, - { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].yawRate, 0, 100 }, + { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], 0, 100 }, + { "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], 0, 100 }, + { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], 0, 100 }, { "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, 0, 100}, { "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX}, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 19e75a874..1861bc48a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -890,11 +890,12 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A break; case MSP_RC_TUNING: - headSerialReply(7); + headSerialReply(8); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); - serialize8(currentControlRateProfile->rollPitchRate); - serialize8(currentControlRateProfile->yawRate); + for (i = 0 ; i < 3; i++) { + serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t + } serialize8(currentControlRateProfile->dynThrPID); serialize8(currentControlRateProfile->thrMid8); serialize8(currentControlRateProfile->thrExpo8); @@ -1325,13 +1326,18 @@ static bool processInCommand(void) break; case MSP_SET_RC_TUNING: - currentControlRateProfile->rcRate8 = read8(); - currentControlRateProfile->rcExpo8 = read8(); - currentControlRateProfile->rollPitchRate = read8(); - currentControlRateProfile->yawRate = read8(); - currentControlRateProfile->dynThrPID = read8(); - currentControlRateProfile->thrMid8 = read8(); - currentControlRateProfile->thrExpo8 = read8(); + if (currentPort->dataSize == 8) { + currentControlRateProfile->rcRate8 = read8(); + currentControlRateProfile->rcExpo8 = read8(); + for (i = 0; i < 3; i++) { + currentControlRateProfile->rates[i] = read8(); + } + currentControlRateProfile->dynThrPID = read8(); + currentControlRateProfile->thrMid8 = read8(); + currentControlRateProfile->thrExpo8 = read8(); + } else { + headSerialError(0); + } break; case MSP_SET_MISC: tmp = read16(); diff --git a/src/main/mw.c b/src/main/mw.c index db8d34e4e..e678848ec 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -201,7 +201,7 @@ void annexCode(void) tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; - prop1 = 100 - (uint16_t)currentControlRateProfile->rollPitchRate * tmp / 500; + prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { @@ -212,7 +212,7 @@ void annexCode(void) } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; - prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500; + prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;