mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-18 05:45:28 +03:00
group controlRateConfig settings by function
This commit is contained in:
parent
9c28150281
commit
fc723121b2
14 changed files with 145 additions and 126 deletions
|
@ -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);
|
||||
|
|
|
@ -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 }
|
||||
|
|
|
@ -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],
|
||||
.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
|
||||
.throttle = {
|
||||
.rcMid8 = 50,
|
||||
.rcExpo8 = 0,
|
||||
.dynPID = 0,
|
||||
.pa_breakpoint = 1500
|
||||
},
|
||||
|
||||
.stabilized = {
|
||||
.rcExpo8 = 70,
|
||||
.rcYawExpo8 = 20,
|
||||
.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 = {
|
||||
.rcExpo8 = 70,
|
||||
.rcYawExpo8 = 20,
|
||||
.rates[FD_ROLL] = 100,
|
||||
.rates[FD_PITCH] = 100,
|
||||
.rates[FD_YAW] = 100
|
||||
}
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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
|
||||
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;
|
||||
|
||||
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 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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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]
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue