Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-18 05:45:28 +03:00
Code Issues Wiki Activity

group controlRateConfig settings by function

Browse source
This commit is contained in:
Michel Pastor 2018-01-24 20:27:14 +01:00
parent 9c28150281
commit fc723121b2
14 changed files with 145 additions and 126 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/blackbox/blackbox.c
View file

@ -1383,15 +1383,15 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getPidUpdateRate());
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyroSyncDenominator);
BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", 100); //For compatibility reasons write rc_rate 100
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
currentControlRateProfile->rates[PITCH],
currentControlRateProfile->rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->throttle.rcMid8);
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->throttle.rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->throttle.dynPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->throttle.pa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL],
currentControlRateProfile->stabilized.rates[PITCH],
currentControlRateProfile->stabilized.rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", pidBank()->pid[PID_ROLL].P,
pidBank()->pid[PID_ROLL].I,
pidBank()->pid[PID_ROLL].D);

4
src/main/cms/cms_menu_imu.c
View file

@ -270,8 +270,8 @@ static OSD_Entry cmsx_menuManualRateProfileEntries[] =
OSD_SETTING_ENTRY("MANU PITCH RATE", SETTING_MANUAL_PITCH_RATE),
OSD_SETTING_ENTRY("MANU YAW RATE", SETTING_MANUAL_YAW_RATE),
OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_RC_MANUAL_EXPO),
OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_RC_MANUAL_YAW_EXPO),
OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_MANUAL_RC_EXPO),
OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_MANUAL_RC_YAW_EXPO),
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }

28
src/main/fc/controlrate_profile.c
View file

@ -33,26 +33,34 @@
const controlRateConfig_t *currentControlRateProfile;
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 0);
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 1);
void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
{
for (int i = 0; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) {
RESET_CONFIG(controlRateConfig_t, &instance[i],
.throttle = {
.rcMid8 = 50,
.rcExpo8 = 0,
.dynPID = 0,
.pa_breakpoint = 1500
},
.stabilized = {
.rcExpo8 = 70,
.manual_rcExpo8 = 70,
.thrMid8 = 50,
.thrExpo8 = 0,
.dynThrPID = 0,
.rcYawExpo8 = 20,
.manual_rcYawExpo8 = 20,
.tpa_breakpoint = 1500,
.rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT,
.rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT,
.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT,
.manual_rates[FD_ROLL] = 100,
.manual_rates[FD_PITCH] = 100,
.manual_rates[FD_YAW] = 100
},
.manual = {
.rcExpo8 = 70,
.rcYawExpo8 = 20,
.rates[FD_ROLL] = 100,
.rates[FD_PITCH] = 100,
.rates[FD_YAW] = 100
}
);
}
}

26
src/main/fc/controlrate_profile.h
View file

@ -41,16 +41,26 @@ and so on.
#define CONTROL_RATE_CONFIG_TPA_MAX 100
typedef struct controlRateConfig_s {
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
struct {
uint8_t rcMid8;
uint8_t rcExpo8;
uint8_t dynPID;
uint16_t pa_breakpoint; // Breakpoint where TPA is activated
} throttle;
struct {
uint8_t rcExpo8;
uint8_t manual_rcExpo8;
uint8_t thrMid8;
uint8_t thrExpo8;
uint8_t rates[3];
uint8_t manual_rates[3];
uint8_t dynThrPID;
uint8_t rcYawExpo8;
uint8_t manual_rcYawExpo8;
uint8_t rates[3];
} stabilized;
struct {
uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} manual;
} controlRateConfig_t;
PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);

12
src/main/fc/fc_core.c
View file

@ -294,15 +294,15 @@ void annexCode(void)
}
else {
// Compute ROLL PITCH and YAW command
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcExpo8 : currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcExpo8 : currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcYawExpo8 : currentControlRateProfile->rcYawExpo8, rcControlsConfig()->yaw_deadband);
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
// Apply manual control rates
if (FLIGHT_MODE(MANUAL_MODE)) {
rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual_rates[FD_ROLL] / 100L;
rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual_rates[FD_PITCH] / 100L;
rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual_rates[FD_YAW] / 100L;
rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L;
rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L;
rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L;
}
//Compute THROTTLE command

30
src/main/fc/fc_msp.c
View file

@ -522,15 +522,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_RC_TUNING:
sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used
sbufWriteU8(dst, currentControlRateProfile->rcExpo8);
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
for (int i = 0 ; i < 3; i++) {
sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t
}
sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
sbufWriteU8(dst, currentControlRateProfile->thrMid8);
sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8);
sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
break;
case MSP_PID:
@ -1339,23 +1339,23 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize >= 10) {
sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons
// need to cast away const to set controlRateProfile
((controlRateConfig_t*)currentControlRateProfile)->rcExpo8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = sbufReadU8(src);
for (int i = 0; i < 3; i++) {
rate = sbufReadU8(src);
if (i == FD_YAW) {
((controlRateConfig_t*)currentControlRateProfile)->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
}
else {
((controlRateConfig_t*)currentControlRateProfile)->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
}
}
rate = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
((controlRateConfig_t*)currentControlRateProfile)->thrMid8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->thrExpo8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->tpa_breakpoint = sbufReadU16(src);
((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src);
if (dataSize >= 11) {
((controlRateConfig_t*)currentControlRateProfile)->rcYawExpo8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadU8(src);
}
schedulePidGainsUpdate();

20
src/main/fc/rc_adjustments.c
View file

@ -208,20 +208,20 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
int newValue;
switch (adjustmentFunction) {
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue;
newValue = constrain((int)controlRateConfig->stabilized.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->stabilized.rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->thrExpo8 = newValue;
newValue = constrain((int)controlRateConfig->throttle.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->throttle.rcExpo8 = newValue;
generateThrottleCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
newValue = constrain((int)controlRateConfig->stabilized.rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->stabilized.rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
schedulePidGainsUpdate();
@ -231,14 +231,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
FALLTHROUGH;
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
newValue = constrain((int)controlRateConfig->stabilized.rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->stabilized.rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
newValue = constrain((int)controlRateConfig->stabilized.rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->stabilized.rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
schedulePidGainsUpdate();
break;

10
src/main/fc/rc_curves.c
View file

@ -38,16 +38,16 @@ int16_t lookupThrottleRCMid; // THROTTLE curve mid point
void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
{
lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
const int16_t tmp = 10 * i - controlRateConfig->thrMid8;
const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
uint8_t y = 1;
if (tmp > 0)
y = 100 - controlRateConfig->thrMid8;
y = 100 - controlRateConfig->throttle.rcMid8;
if (tmp < 0)
y = controlRateConfig->thrMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
y = controlRateConfig->throttle.rcMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = motorConfig()->minthrottle + (int32_t) (motorConfig()->maxthrottle - motorConfig()->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
}
}

61
src/main/fc/settings.yaml
View file

@ -555,61 +555,62 @@ groups:
headers: ["fc/controlrate_profile.h"]
value_type: CONTROL_RATE_VALUE
members:
- name: rc_expo
field: rcExpo8
min: 0
max: 100
- name: rc_manual_expo
field: manual_rcExpo8
min: 0
max: 100
- name: rc_yaw_expo
field: rcYawExpo8
min: 0
max: 100
- name: rc_manual_yaw_expo
field: manual_rcYawExpo8
min: 0
max: 100
- name: thr_mid
field: thrMid8
field: throttle.rcMid8
min: 0
max: 100
- name: thr_expo
field: thrExpo8
field: throttle.rcExpo8
min: 0
max: 100
- name: tpa_rate
field: throttle.dynPID
min: 0
max: CONTROL_RATE_CONFIG_TPA_MAX
- name: tpa_breakpoint
field: throttle.pa_breakpoint
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: rc_expo
field: stabilized.rcExpo8
min: 0
max: 100
- name: rc_yaw_expo
field: stabilized.rcYawExpo8
min: 0
max: 100
# New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis.
# Rate 180 (1800dps) is max. value gyro can measure reliably
- name: roll_rate
field: rates[FD_ROLL]
field: stabilized.rates[FD_ROLL]
min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
- name: pitch_rate
field: rates[FD_PITCH]
field: stabilized.rates[FD_PITCH]
min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
- name: yaw_rate
field: rates[FD_YAW]
field: stabilized.rates[FD_YAW]
min: CONTROL_RATE_CONFIG_YAW_RATE_MIN
max: CONTROL_RATE_CONFIG_YAW_RATE_MAX
- name: tpa_rate
field: dynThrPID
- name: manual_rc_expo
field: manual.rcExpo8
min: 0
max: CONTROL_RATE_CONFIG_TPA_MAX
- name: tpa_breakpoint
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
max: 100
- name: manual_rc_yaw_expo
field: manual.rcYawExpo8
min: 0
max: 100
- name: manual_roll_rate
field: manual_rates[FD_ROLL]
field: manual.rates[FD_ROLL]
min: 0
max: 100
- name: manual_pitch_rate
field: manual_rates[FD_PITCH]
field: manual.rates[FD_PITCH]
min: 0
max: 100
- name: manual_yaw_rate
field: manual_rates[FD_YAW]
field: manual.rates[FD_YAW]
min: 0
max: 100

2
src/main/flight/failsafe.c
View file

@ -249,7 +249,7 @@ void failsafeApplyControlInput(void)
if (STATE(FIXED_WING)) {
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->rates[FD_YAW]);
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
}
else {

24
src/main/flight/pid.c
View file

@ -270,10 +270,10 @@ static float calculateFixedWingTPAFactor(void)
// tpa_rate is amount of curve TPA applied to PIDs
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
if (currentControlRateProfile->dynThrPID != 0 && currentControlRateProfile->tpa_breakpoint > motorConfig()->minthrottle) {
if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > motorConfig()->minthrottle) {
if (rcCommand[THROTTLE] > motorConfig()->minthrottle) {
// Calculate TPA according to throttle
tpaFactor = 0.5f + ((float)(currentControlRateProfile->tpa_breakpoint - motorConfig()->minthrottle) / (rcCommand[THROTTLE] - motorConfig()->minthrottle) / 2.0f);
tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - motorConfig()->minthrottle) / (rcCommand[THROTTLE] - motorConfig()->minthrottle) / 2.0f);
// Limit to [0.5; 2] range
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
@ -283,7 +283,7 @@ static float calculateFixedWingTPAFactor(void)
}
// Attenuate TPA curve according to configured amount
tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->dynThrPID / 100.0f);
tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->throttle.dynPID / 100.0f);
}
else {
tpaFactor = 1.0f;
@ -297,12 +297,12 @@ static float calculateMultirotorTPAFactor(void)
float tpaFactor;
// TPA should be updated only when TPA is actually set
if (currentControlRateProfile->dynThrPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) {
tpaFactor = 1.0f;
} else if (rcCommand[THROTTLE] < motorConfig()->maxthrottle) {
tpaFactor = (100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcCommand[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (motorConfig()->maxthrottle - currentControlRateProfile->tpa_breakpoint)) / 100.0f;
tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (motorConfig()->maxthrottle - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f;
} else {
tpaFactor = (100 - currentControlRateProfile->dynThrPID) / 100.0f;
tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f;
}
return tpaFactor;
@ -387,7 +387,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->rates[axis] * 10.0f, currentControlRateProfile->rates[axis] * 10.0f);
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
// Apply simple LPF to angleRateTarget to make response less jerky
// Ideas behind this:
@ -682,15 +682,15 @@ static void pidTurnAssistant(pidState_t *pidState)
imuTransformVectorEarthToBody(&targetRates);
// Add in roll and pitch
pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.V.X, -currentControlRateProfile->rates[ROLL] * 10.0f, currentControlRateProfile->rates[ROLL] * 10.0f);
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.V.Y, -currentControlRateProfile->rates[PITCH] * 10.0f, currentControlRateProfile->rates[PITCH] * 10.0f);
pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.V.X, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.V.Y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
// Replace YAW on quads - add it in on airplanes
if (STATE(FIXED_WING)) {
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.V.Z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->rates[YAW] * 10.0f, currentControlRateProfile->rates[YAW] * 10.0f);
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.V.Z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
}
else {
pidState[YAW].rateTarget = constrainf(targetRates.V.Z, -currentControlRateProfile->rates[YAW] * 10.0f, currentControlRateProfile->rates[YAW] * 10.0f);
pidState[YAW].rateTarget = constrainf(targetRates.V.Z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
}
}
#endif
@ -713,7 +713,7 @@ void pidController(void)
if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) {
rateTarget = pidHeadingHold();
} else {
rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->rates[axis]);
rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]);
}
// Limit desired rate to something gyro can measure reliably

2
src/main/flight/pid_autotune.c
View file

@ -165,7 +165,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
{
const timeMs_t currentTimeMs = millis();
const float absDesiredRateDps = fabsf(desiredRateDps);
float maxDesiredRate = currentControlRateProfile->rates[axis] * 10.0f;
float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
pidAutotuneState_e newState;
// Use different max desired rate in ANGLE for pitch and roll

2
src/main/navigation/navigation_fixedwing.c
View file

@ -524,7 +524,7 @@ void applyFixedWingEmergencyLandingController(void)
// FIXME: Use altitude controller if available (similar to MC code)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->rates[FD_YAW]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
}

18
src/main/target/FALCORE/config.c
View file

@ -196,13 +196,13 @@ void targetConfiguration(void)
pidProfileMutable()->bank_mc.pid[PID_VEL_XY].I = 20;
pidProfileMutable()->bank_mc.pid[PID_VEL_XY].D = 70;
((controlRateConfig_t*)currentControlRateProfile)->rcExpo8 = 60;
((controlRateConfig_t*)currentControlRateProfile)->rcYawExpo8 = 35;
((controlRateConfig_t*)currentControlRateProfile)->rates[FD_ROLL] = 54;
((controlRateConfig_t*)currentControlRateProfile)->rates[FD_PITCH] = 54;
((controlRateConfig_t*)currentControlRateProfile)->rates[FD_YAW] = 36;
((controlRateConfig_t*)currentControlRateProfile)->thrMid8 = 50;
((controlRateConfig_t*)currentControlRateProfile)->thrExpo8 = 0;
((controlRateConfig_t*)currentControlRateProfile)->dynThrPID = 10;
((controlRateConfig_t*)currentControlRateProfile)->tpa_breakpoint = 1600;
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = 60;
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = 35;
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = 54;
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = 54;
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = 36;
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = 50;
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0;
((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10;
((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600;
}