rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Decouple roll and pitch rates. MSP clients take note of updated 

MSP_RC_TUNING/MSP_SET_RC_TUNING commands.
This commit is contained in:
Dominic Clifton 2015-03-07 15:09:42 +00:00
parent 26bb86898e
commit b595b49ca8
9 changed files with 70 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/config/config.c
View File

@ -275,12 +275,15 @@ void resetSerialConfig(serialConfig_t *serialConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 90; controlRateConfig->rcRate8 = 90;
controlRateConfig->rcExpo8 = 65; controlRateConfig->rcExpo8 = 65;
controlRateConfig->rollPitchRate = 0;
controlRateConfig->yawRate = 0;
controlRateConfig->thrMid8 = 50; controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0; controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0; controlRateConfig->dynThrPID = 0;
controlRateConfig->tpa_breakpoint = 1500; controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 0;
}
} }
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {

13
src/main/flight/mixer.c
View File

@ -546,12 +546,6 @@ static void airplaneMixer(void)
void mixTable(void) void mixTable(void)
{ {
uint32_t i; 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) { if (motorCount > 3) {
// prevent "yaw jump" during yaw correction // prevent "yaw jump" during yaw correction
@ -570,6 +564,13 @@ void mixTable(void)
} }
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) #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 // airplane / servo mixes
switch (currentMixerMode) { switch (currentMixerMode) {
case MIXER_BI: case MIXER_BI:

31
src/main/flight/pid.c
View File

@ -139,9 +139,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// ----------PID controller---------- // ----------PID controller----------
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
uint8_t rate = controlRateConfig->rates[axis];
if (axis == FD_YAW) { if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate // 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 { } else {
// calculate error and limit the angle to the max inclination // calculate error and limit the angle to the max inclination
#ifdef GPS #ifdef GPS
@ -163,7 +165,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
AngleRate = errorAngle * pidProfile->A_level; AngleRate = errorAngle * pidProfile->A_level;
} else { } else {
//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID //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)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; 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 }; static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 };
int32_t delta; 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 // PITCH & ROLL
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
rc = rcCommand[axis] << 1; rc = rcCommand[axis] << 1;
error = rc - (gyroData[axis] / 4); error = rc - (gyroData[axis] / 4);
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here 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 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 //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 #ifdef ALIENWII32
error = rc - gyroData[FD_YAW]; error = rc - gyroData[FD_YAW];
#else #else
@ -483,7 +492,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
#endif #endif
} }
//YAW //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 #ifdef ALIENWII32
error = rc - gyroData[FD_YAW]; error = rc - gyroData[FD_YAW];
#else #else
@ -603,7 +612,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
Mwii3msTimescale /= 3000.0f; Mwii3msTimescale /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now 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); int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { 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; ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
} }
} else { } 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 error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp) > 50) { if (ABS(tmp) > 50) {
@ -657,9 +666,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// ----------PID controller---------- // ----------PID controller----------
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 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 { } else {
// calculate error and limit the angle to max configured inclination // calculate error and limit the angle to max configured inclination
#ifdef GPS #ifdef GPS
@ -677,7 +688,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
#endif #endif
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID 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)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel // mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8; AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;

22
src/main/io/display.c
View File

@ -271,26 +271,22 @@ void showProfilePage(void)
controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex); 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(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); 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(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); 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_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)

10
src/main/io/rc_controls.c
View File

@ -402,12 +402,14 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
generateThrottleCurve(controlRateConfig, escAndServoConfig); generateThrottleCurve(controlRateConfig, escAndServoConfig);
break; break;
case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_ROLL_RATE:
newValue = (int)controlRateConfig->rollPitchRate + delta; newValue = (int)controlRateConfig->rates[FD_PITCH] + delta;
controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_YAW_RATE: case ADJUSTMENT_YAW_RATE:
newValue = (int)controlRateConfig->yawRate + delta; newValue = (int)controlRateConfig->rates[FD_YAW] + delta;
controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
break; break;
case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {

3
src/main/io/rc_controls.h
View File

@ -122,8 +122,7 @@ typedef struct controlRateConfig_s {
uint8_t rcExpo8; uint8_t rcExpo8;
uint8_t thrMid8; uint8_t thrMid8;
uint8_t thrExpo8; uint8_t thrExpo8;
uint8_t rollPitchRate; uint8_t rates[3];
uint8_t yawRate;
uint8_t dynThrPID; uint8_t dynThrPID;
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
} controlRateConfig_t; } controlRateConfig_t;

5
src/main/io/serial_cli.c
View File

@ -392,8 +392,9 @@ const clivalue_t valueTable[] = {
{ "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 }, { "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_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 }, { "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 }, { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], 0, 100 },
{ "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].yawRate, 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_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}, { "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},

16
src/main/io/serial_msp.c
View File

@ -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 serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break; break;
case MSP_RC_TUNING: case MSP_RC_TUNING:
headSerialReply(7); headSerialReply(8);
serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcRate8);
serialize8(currentControlRateProfile->rcExpo8); serialize8(currentControlRateProfile->rcExpo8);
serialize8(currentControlRateProfile->rollPitchRate); for (i = 0 ; i < 3; i++) {
serialize8(currentControlRateProfile->yawRate); serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
}
serialize8(currentControlRateProfile->dynThrPID); serialize8(currentControlRateProfile->dynThrPID);
serialize8(currentControlRateProfile->thrMid8); serialize8(currentControlRateProfile->thrMid8);
serialize8(currentControlRateProfile->thrExpo8); serialize8(currentControlRateProfile->thrExpo8);
@ -1325,13 +1326,18 @@ static bool processInCommand(void)
break; break;
case MSP_SET_RC_TUNING: case MSP_SET_RC_TUNING:
if (currentPort->dataSize == 8) {
currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcRate8 = read8();
currentControlRateProfile->rcExpo8 = read8(); currentControlRateProfile->rcExpo8 = read8();
currentControlRateProfile->rollPitchRate = read8(); for (i = 0; i < 3; i++) {
currentControlRateProfile->yawRate = read8(); currentControlRateProfile->rates[i] = read8();
}
currentControlRateProfile->dynThrPID = read8(); currentControlRateProfile->dynThrPID = read8();
currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrMid8 = read8();
currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->thrExpo8 = read8();
} else {
headSerialError(0);
}
break; break;
case MSP_SET_MISC: case MSP_SET_MISC:
tmp = read16(); tmp = read16();

4
src/main/mw.c
View File

@ -201,7 +201,7 @@ void annexCode(void)
tmp2 = tmp / 100; tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 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; prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) { } else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) { if (currentProfile->rcControlsConfig.yaw_deadband) {
@ -212,7 +212,7 @@ void annexCode(void)
} }
} }
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; 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. // 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; dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;