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("looptime", "%d", getPidUpdateRate());
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyroSyncDenominator); 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_rate", "%d", 100); //For compatibility reasons write rc_rate 100
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->rcYawExpo8); BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->throttle.rcMid8);
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->throttle.rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->throttle.dynPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint); BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->throttle.pa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL],
currentControlRateProfile->rates[PITCH], currentControlRateProfile->stabilized.rates[PITCH],
currentControlRateProfile->rates[YAW]); currentControlRateProfile->stabilized.rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", pidBank()->pid[PID_ROLL].P, BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", pidBank()->pid[PID_ROLL].P,
pidBank()->pid[PID_ROLL].I, pidBank()->pid[PID_ROLL].I,
pidBank()->pid[PID_ROLL].D); 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 PITCH RATE", SETTING_MANUAL_PITCH_RATE),
OSD_SETTING_ENTRY("MANU YAW RATE", SETTING_MANUAL_YAW_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 EXPO", SETTING_MANUAL_RC_EXPO),
OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_RC_MANUAL_YAW_EXPO), OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_MANUAL_RC_YAW_EXPO),
{ "BACK", OME_Back, NULL, NULL, 0 }, { "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 } { NULL, OME_END, NULL, NULL, 0 }

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

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

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

@ -41,16 +41,26 @@ and so on.
#define CONTROL_RATE_CONFIG_TPA_MAX 100 #define CONTROL_RATE_CONFIG_TPA_MAX 100
typedef struct controlRateConfig_s { typedef struct controlRateConfig_s {
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
uint8_t rcExpo8; struct {
uint8_t manual_rcExpo8; uint8_t rcMid8;
uint8_t thrMid8; uint8_t rcExpo8;
uint8_t thrExpo8; uint8_t dynPID;
uint8_t rates[3]; uint16_t pa_breakpoint; // Breakpoint where TPA is activated
uint8_t manual_rates[3]; } throttle;
uint8_t dynThrPID;
uint8_t rcYawExpo8; struct {
uint8_t manual_rcYawExpo8; uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} stabilized;
struct {
uint8_t rcExpo8;
uint8_t rcYawExpo8;
uint8_t rates[3];
} manual;
} controlRateConfig_t; } controlRateConfig_t;
PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); 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 { else {
// Compute ROLL PITCH and YAW command // Compute ROLL PITCH and YAW command
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcExpo8 : currentControlRateProfile->rcExpo8, rcControlsConfig()->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->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->rcYawExpo8, rcControlsConfig()->yaw_deadband); rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
// Apply manual control rates // Apply manual control rates
if (FLIGHT_MODE(MANUAL_MODE)) { if (FLIGHT_MODE(MANUAL_MODE)) {
rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual_rates[FD_ROLL] / 100L; rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L;
rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual_rates[FD_PITCH] / 100L; rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L;
rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual_rates[FD_YAW] / 100L; rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L;
} }
//Compute THROTTLE command //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: case MSP_RC_TUNING:
sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used 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++) { 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->throttle.dynPID);
sbufWriteU8(dst, currentControlRateProfile->thrMid8); sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
sbufWriteU8(dst, currentControlRateProfile->thrExpo8); sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint); sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8); sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
break; break;
case MSP_PID: case MSP_PID:
@ -1339,23 +1339,23 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize >= 10) { if (dataSize >= 10) {
sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons
// need to cast away const to set controlRateProfile // 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++) { for (int i = 0; i < 3; i++) {
rate = sbufReadU8(src); rate = sbufReadU8(src);
if (i == FD_YAW) { 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 { 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); rate = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
((controlRateConfig_t*)currentControlRateProfile)->thrMid8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->thrExpo8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->tpa_breakpoint = sbufReadU16(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src);
if (dataSize >= 11) { if (dataSize >= 11) {
((controlRateConfig_t*)currentControlRateProfile)->rcYawExpo8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadU8(src);
} }
schedulePidGainsUpdate(); schedulePidGainsUpdate();

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

@ -208,20 +208,20 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
int newValue; int newValue;
switch (adjustmentFunction) { switch (adjustmentFunction) {
case ADJUSTMENT_RC_EXPO: case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c newValue = constrain((int)controlRateConfig->stabilized.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue; controlRateConfig->stabilized.rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break; break;
case ADJUSTMENT_THROTTLE_EXPO: case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c newValue = constrain((int)controlRateConfig->throttle.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->thrExpo8 = newValue; controlRateConfig->throttle.rcExpo8 = newValue;
generateThrottleCurve(controlRateConfig); generateThrottleCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break; break;
case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_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); newValue = constrain((int)controlRateConfig->stabilized.rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue; controlRateConfig->stabilized.rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
schedulePidGainsUpdate(); schedulePidGainsUpdate();
@ -231,14 +231,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
FALLTHROUGH; FALLTHROUGH;
case ADJUSTMENT_ROLL_RATE: 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); newValue = constrain((int)controlRateConfig->stabilized.rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue; controlRateConfig->stabilized.rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_YAW_RATE: case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); newValue = constrain((int)controlRateConfig->stabilized.rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue; controlRateConfig->stabilized.rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; 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) 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++) { 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; uint8_t y = 1;
if (tmp > 0) if (tmp > 0)
y = 100 - controlRateConfig->thrMid8; y = 100 - controlRateConfig->throttle.rcMid8;
if (tmp < 0) if (tmp < 0)
y = controlRateConfig->thrMid8; y = controlRateConfig->throttle.rcMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; 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] 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"] headers: ["fc/controlrate_profile.h"]
value_type: CONTROL_RATE_VALUE value_type: CONTROL_RATE_VALUE
members: 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 - name: thr_mid
field: thrMid8 field: throttle.rcMid8
min: 0 min: 0
max: 100 max: 100
- name: thr_expo - 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 min: 0
max: 100 max: 100
# New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis. # 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 # Rate 180 (1800dps) is max. value gyro can measure reliably
- name: roll_rate - name: roll_rate
field: rates[FD_ROLL] field: stabilized.rates[FD_ROLL]
min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
- name: pitch_rate - name: pitch_rate
field: rates[FD_PITCH] field: stabilized.rates[FD_PITCH]
min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
- name: yaw_rate - name: yaw_rate
field: rates[FD_YAW] field: stabilized.rates[FD_YAW]
min: CONTROL_RATE_CONFIG_YAW_RATE_MIN min: CONTROL_RATE_CONFIG_YAW_RATE_MIN
max: CONTROL_RATE_CONFIG_YAW_RATE_MAX max: CONTROL_RATE_CONFIG_YAW_RATE_MAX
- name: tpa_rate - name: manual_rc_expo
field: dynThrPID field: manual.rcExpo8
min: 0 min: 0
max: CONTROL_RATE_CONFIG_TPA_MAX max: 100
- name: tpa_breakpoint - name: manual_rc_yaw_expo
min: PWM_RANGE_MIN field: manual.rcYawExpo8
max: PWM_RANGE_MAX min: 0
max: 100
- name: manual_roll_rate - name: manual_roll_rate
field: manual_rates[FD_ROLL] field: manual.rates[FD_ROLL]
min: 0 min: 0
max: 100 max: 100
- name: manual_pitch_rate - name: manual_pitch_rate
field: manual_rates[FD_PITCH] field: manual.rates[FD_PITCH]
min: 0 min: 0
max: 100 max: 100
- name: manual_yaw_rate - name: manual_yaw_rate
field: manual_rates[FD_YAW] field: manual.rates[FD_YAW]
min: 0 min: 0
max: 100 max: 100

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

@ -249,7 +249,7 @@ void failsafeApplyControlInput(void)
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); 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[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; autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
} }
else { 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_rate is amount of curve TPA applied to PIDs
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) // 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) { if (rcCommand[THROTTLE] > motorConfig()->minthrottle) {
// Calculate TPA according to throttle // 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 // Limit to [0.5; 2] range
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f); tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
@ -283,7 +283,7 @@ static float calculateFixedWingTPAFactor(void)
} }
// Attenuate TPA curve according to configured amount // 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 { else {
tpaFactor = 1.0f; tpaFactor = 1.0f;
@ -297,12 +297,12 @@ static float calculateMultirotorTPAFactor(void)
float tpaFactor; float tpaFactor;
// TPA should be updated only when TPA is actually set // 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; tpaFactor = 1.0f;
} else if (rcCommand[THROTTLE] < motorConfig()->maxthrottle) { } 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 { } else {
tpaFactor = (100 - currentControlRateProfile->dynThrPID) / 100.0f; tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f;
} }
return tpaFactor; 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 angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[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 // Apply simple LPF to angleRateTarget to make response less jerky
// Ideas behind this: // Ideas behind this:
@ -682,15 +682,15 @@ static void pidTurnAssistant(pidState_t *pidState)
imuTransformVectorEarthToBody(&targetRates); imuTransformVectorEarthToBody(&targetRates);
// Add in roll and pitch // 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[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->rates[PITCH] * 10.0f, currentControlRateProfile->rates[PITCH] * 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 // Replace YAW on quads - add it in on airplanes
if (STATE(FIXED_WING)) { 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 { 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 #endif
@ -713,7 +713,7 @@ void pidController(void)
if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) { if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) {
rateTarget = pidHeadingHold(); rateTarget = pidHeadingHold();
} else { } 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 // 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 timeMs_t currentTimeMs = millis();
const float absDesiredRateDps = fabsf(desiredRateDps); const float absDesiredRateDps = fabsf(desiredRateDps);
float maxDesiredRate = currentControlRateProfile->rates[axis] * 10.0f; float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
pidAutotuneState_e newState; pidAutotuneState_e newState;
// Use different max desired rate in ANGLE for pitch and roll // 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) // 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[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[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; 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].I = 20;
pidProfileMutable()->bank_mc.pid[PID_VEL_XY].D = 70; pidProfileMutable()->bank_mc.pid[PID_VEL_XY].D = 70;
((controlRateConfig_t*)currentControlRateProfile)->rcExpo8 = 60; ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = 60;
((controlRateConfig_t*)currentControlRateProfile)->rcYawExpo8 = 35; ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = 35;
((controlRateConfig_t*)currentControlRateProfile)->rates[FD_ROLL] = 54; ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = 54;
((controlRateConfig_t*)currentControlRateProfile)->rates[FD_PITCH] = 54; ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = 54;
((controlRateConfig_t*)currentControlRateProfile)->rates[FD_YAW] = 36; ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = 36;
((controlRateConfig_t*)currentControlRateProfile)->thrMid8 = 50; ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = 50;
((controlRateConfig_t*)currentControlRateProfile)->thrExpo8 = 0; ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0;
((controlRateConfig_t*)currentControlRateProfile)->dynThrPID = 10; ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10;
((controlRateConfig_t*)currentControlRateProfile)->tpa_breakpoint = 1600; ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600;
} }