Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

Replace PASSTHROUGH with MANUAL flight mode (FW) (#2661)

Browse source 
* FW: add manual flight mode
* manual mode: separate roll and pitch expo
* manual flight mode: cleaning
* replace PASSTHRU mode with MANUAL mode
* manual mode: merge pitch and roll expo
* manual mode: add OSD menu for manual rates and expo
* manual mode: forgot to add yaw rate adjustment in OSD menu
* manual mode: add adjustments
* manual mode: move rates applications in fc_core
* group controlRateConfig settings by function
* group controlRateConfig settings by function: fix ALIENFLIGHTF3 default rates config
* manual mode rc adjustments: adapt to fc723121 changes
* manual mode: clean rc adjustments
* add new MSPv2 messages: MSP2_INAV_RATE_PROFILE and MSP2_INAV_SET_RATE_PROFILE
This commit is contained in:
shellixyz 2018-01-31 10:44:43 +01:00 • committed by Konstantin Sharlaimov
parent a9bf9453dc
commit fd40892d8e
29 changed files with 356 additions and 158 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);

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

@ -259,6 +259,35 @@ static CMS_Menu cmsx_menuPidGpsnav = {
.entries = cmsx_menuPidGpsnavEntries, .entries = cmsx_menuPidGpsnavEntries,
}; };
//
// MANUAL Rate & Expo
//
static OSD_Entry cmsx_menuManualRateProfileEntries[] =
{
{ "-- MANUAL RATE --", OME_Label, NULL, profileIndexString, 0 },
OSD_SETTING_ENTRY("MANU ROLL RATE", SETTING_MANUAL_ROLL_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 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 }
};
static CMS_Menu cmsx_menuManualRateProfile = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUMANURATE",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cmsx_menuManualRateProfileEntries
};
// //
// Rate & Expo // Rate & Expo
// //
@ -423,6 +452,7 @@ static OSD_Entry cmsx_menuImuEntries[] =
// Rate profile dependent // Rate profile dependent
{"RATE PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_RATE_PROFILE_COUNT, 1}, 0}, {"RATE PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_RATE_PROFILE_COUNT, 1}, 0},
{"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0},
{"MANU RATE", OME_Submenu, cmsMenuChange, &cmsx_menuManualRateProfile, 0},
// Global // Global
{"GYRO GLB", OME_Submenu, cmsMenuChange, &cmsx_menuGyro, 0}, {"GYRO GLB", OME_Submenu, cmsMenuChange, &cmsx_menuGyro, 0},

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

@ -33,21 +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],
.throttle = {
.rcMid8 = 50,
.rcExpo8 = 0,
.dynPID = 0,
.pa_breakpoint = 1500
},
.stabilized = {
.rcExpo8 = 70, .rcExpo8 = 70,
.thrMid8 = 50,
.thrExpo8 = 0,
.dynThrPID = 0,
.rcYawExpo8 = 20, .rcYawExpo8 = 20,
.tpa_breakpoint = 1500,
.rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, .rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT,
.rates[FD_PITCH] = 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 .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
}
); );
} }
} }

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

@ -41,13 +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
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 rcExpo8;
uint8_t thrMid8;
uint8_t thrExpo8;
uint8_t rates[3];
uint8_t dynThrPID;
uint8_t rcYawExpo8; 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);

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

@ -294,9 +294,16 @@ void annexCode(void)
} }
else { else {
// Compute ROLL PITCH and YAW command // Compute ROLL PITCH and YAW command
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], 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], 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], 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
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;
}
//Compute THROTTLE command //Compute THROTTLE command
throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
@ -561,19 +568,20 @@ void processRx(timeUs_t currentTimeUs)
// Handle passthrough mode // Handle passthrough mode
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough
(!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating (!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating
ENABLE_FLIGHT_MODE(PASSTHRU_MODE); ENABLE_FLIGHT_MODE(MANUAL_MODE);
} else { } else {
DISABLE_FLIGHT_MODE(PASSTHRU_MODE); DISABLE_FLIGHT_MODE(MANUAL_MODE);
} }
IS_RC_MODE_ACTIVE(BOXMANUAL) ? ENABLE_FLIGHT_MODE(MANUAL_MODE) : DISABLE_FLIGHT_MODE(MANUAL_MODE);
} }
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
if (FLIGHT_MODE(PASSTHRU_MODE) || !ARMING_FLAG(ARMED)) { if (FLIGHT_MODE(MANUAL_MODE) || !ARMING_FLAG(ARMED)) {
/* In PASSTHRU mode we reset integrators prevent I-term wind-up (PID output is not used in PASSTHRU) */ /* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */
pidResetErrorAccumulators(); pidResetErrorAccumulators();
} }
else { else {

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

@ -531,15 +531,37 @@ 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->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 MSP2_INAV_RATE_PROFILE:
// throttle
sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
// stabilized
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
for (uint8_t i = 0 ; i < 3; ++i) {
sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t
}
// manual
sbufWriteU8(dst, currentControlRateProfile->manual.rcExpo8);
sbufWriteU8(dst, currentControlRateProfile->manual.rcYawExpo8);
for (uint8_t i = 0 ; i < 3; ++i) {
sbufWriteU8(dst, currentControlRateProfile->manual.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);
break; break;
case MSP_PID: case MSP_PID:
@ -1348,23 +1370,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();
@ -1373,6 +1395,45 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} }
break; break;
case MSP2_INAV_SET_RATE_PROFILE:
if (dataSize == 15) {
controlRateConfig_t *currentControlRateProfile_p = (controlRateConfig_t*)currentControlRateProfile; // need to cast away const to set controlRateProfile
// throttle
currentControlRateProfile_p->throttle.rcMid8 = sbufReadU8(src);
currentControlRateProfile_p->throttle.rcExpo8 = sbufReadU8(src);
currentControlRateProfile_p->throttle.dynPID = sbufReadU8(src);
currentControlRateProfile_p->throttle.pa_breakpoint = sbufReadU16(src);
// stabilized
currentControlRateProfile_p->stabilized.rcExpo8 = sbufReadU8(src);
currentControlRateProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src);
for (uint8_t i = 0; i < 3; ++i) {
rate = sbufReadU8(src);
if (i == FD_YAW) {
currentControlRateProfile_p->stabilized.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
} else {
currentControlRateProfile_p->stabilized.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
}
}
// manual
currentControlRateProfile_p->manual.rcExpo8 = sbufReadU8(src);
currentControlRateProfile_p->manual.rcYawExpo8 = sbufReadU8(src);
for (uint8_t i = 0; i < 3; ++i) {
rate = sbufReadU8(src);
if (i == FD_YAW) {
currentControlRateProfile_p->manual.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
} else {
currentControlRateProfile_p->manual.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
}
}
} else {
return MSP_RESULT_ERROR;
}
break;
case MSP_SET_MISC: case MSP_SET_MISC:
tmp = sbufReadU16(src); tmp = sbufReadU16(src);
if (tmp < 1600 && tmp > 1400) if (tmp < 1600 && tmp > 1400)

6
src/main/fc/fc_msp_box.c
View file

@ -47,7 +47,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXCAMSTAB, "CAMSTAB;", 8 }, { BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME { BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME
{ BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD { BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD
{ BOXPASSTHRU, "PASSTHRU;", 12 }, { BOXMANUAL, "MANUAL;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 }, { BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDLOW, "LEDLOW;", 15 }, { BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLIGHTS, "LIGHTS;", 16 }, { BOXLIGHTS, "LIGHTS;", 16 },
@ -186,7 +186,7 @@ void initActiveBoxIds(void)
#endif #endif
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; activeBoxIds[activeBoxIdCount++] = BOXMANUAL;
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH; activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
#if defined(AUTOTUNE_FIXED_WING) #if defined(AUTOTUNE_FIXED_WING)
@ -256,7 +256,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)), BOXPASSTHRU); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)), BOXMANUAL);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS);

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

@ -136,6 +136,26 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_ROLL_D, .adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }} .data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_MANUAL_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_MANUAL_RC_YAW_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_MANUAL_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_MANUAL_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_MANUAL_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
}, { }, {
.adjustmentFunction = ADJUSTMENT_PROFILE, .adjustmentFunction = ADJUSTMENT_PROFILE,
@ -208,20 +228,30 @@ 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_MANUAL_RC_EXPO:
newValue = constrain((int)controlRateConfig->manual.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->manual.rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_RC_EXPO, newValue);
break;
case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
newValue = constrain((int)controlRateConfig->manual.rcYawExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->manual.rcYawExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_RC_YAW_EXPO, newValue);
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,17 +261,32 @@ 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_MANUAL_ROLL_RATE:
newValue = constrain((int)controlRateConfig->manual.rates[FD_ROLL] + delta, 0, 100);
controlRateConfig->manual.rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_ROLL_RATE, newValue);
break;
case ADJUSTMENT_MANUAL_PITCH_RATE:
newValue = constrain((int)controlRateConfig->manual.rates[FD_PITCH] + delta, 0, 100);
controlRateConfig->manual.rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_PITCH_RATE, newValue);
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;
case ADJUSTMENT_MANUAL_YAW_RATE:
newValue = constrain((int)controlRateConfig->manual.rates[FD_YAW] + delta, 0, 100);
controlRateConfig->manual.rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P: case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidBank()->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c newValue = constrain((int)pidBank()->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c

7
src/main/fc/rc_adjustments.h
View file

@ -46,8 +46,13 @@ typedef enum {
ADJUSTMENT_ROLL_P = 18, ADJUSTMENT_ROLL_P = 18,
ADJUSTMENT_ROLL_I = 19, ADJUSTMENT_ROLL_I = 19,
ADJUSTMENT_ROLL_D = 20, ADJUSTMENT_ROLL_D = 20,
ADJUSTMENT_MANUAL_RC_EXPO = 21,
ADJUSTMENT_MANUAL_RC_YAW_EXPO = 22,
ADJUSTMENT_MANUAL_ROLL_RATE = 23,
ADJUSTMENT_MANUAL_PITCH_RATE = 24,
ADJUSTMENT_MANUAL_YAW_RATE = 25,
#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
ADJUSTMENT_PROFILE = 21, ADJUSTMENT_PROFILE = 26,
#endif #endif
ADJUSTMENT_FUNCTION_COUNT // must be last ADJUSTMENT_FUNCTION_COUNT // must be last
} adjustmentFunction_e; } adjustmentFunction_e;

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]
} }
} }

2
src/main/fc/rc_modes.h
View file

@ -34,7 +34,7 @@ typedef enum {
BOXCAMSTAB = 7, BOXCAMSTAB = 7,
BOXNAVRTH = 8, // old GPSHOME BOXNAVRTH = 8, // old GPSHOME
BOXNAVPOSHOLD = 9, // old GPSHOLD BOXNAVPOSHOLD = 9, // old GPSHOLD
BOXPASSTHRU = 10, BOXMANUAL = 10,
BOXBEEPERON = 11, BOXBEEPERON = 11,
BOXLEDLOW = 12, BOXLEDLOW = 12,
BOXLIGHTS = 13, BOXLIGHTS = 13,

2
src/main/fc/runtime_config.c
View file

@ -101,7 +101,7 @@ uint32_t sensorsMask(void)
flightModeForTelemetry_e getFlightModeForTelemetry(void) flightModeForTelemetry_e getFlightModeForTelemetry(void)
{ {
if (FLIGHT_MODE(PASSTHRU_MODE)) if (FLIGHT_MODE(MANUAL_MODE))
return FLM_MANUAL; return FLM_MANUAL;
if (FLIGHT_MODE(FAILSAFE_MODE)) if (FLIGHT_MODE(FAILSAFE_MODE))

14
src/main/fc/runtime_config.h
View file

@ -70,14 +70,14 @@ typedef enum {
NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD
HEADFREE_MODE = (1 << 6), HEADFREE_MODE = (1 << 6),
NAV_LAUNCH_MODE = (1 << 7), NAV_LAUNCH_MODE = (1 << 7),
PASSTHRU_MODE = (1 << 8), MANUAL_MODE = (1 << 8),
FAILSAFE_MODE = (1 << 10), FAILSAFE_MODE = (1 << 9),
AUTO_TUNE = (1 << 11), // old G-Tune AUTO_TUNE = (1 << 10), // old G-Tune
NAV_WP_MODE = (1 << 12), NAV_WP_MODE = (1 << 11),
UNUSED_MODE2 = (1 << 13), UNUSED_MODE2 = (1 << 12),
FLAPERON = (1 << 14), FLAPERON = (1 << 13),
#ifdef USE_FLM_TURN_ASSIST #ifdef USE_FLM_TURN_ASSIST
TURN_ASSISTANT = (1 << 15), TURN_ASSISTANT = (1 << 14),
#endif #endif
} flightModeFlags_e; } flightModeFlags_e;

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

@ -572,43 +572,64 @@ 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_yaw_expo
field: 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
field: manual.rates[FD_ROLL]
min: 0
max: 100
- name: manual_pitch_rate
field: manual.rates[FD_PITCH]
min: 0
max: 100
- name: manual_yaw_rate
field: manual.rates[FD_YAW]
min: 0
max: 100
- name: PG_SERIAL_CONFIG - name: PG_SERIAL_CONFIG
type: serialConfig_t type: serialConfig_t

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 {

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

@ -57,7 +57,7 @@ void hilUpdateControlState(void)
{ {
// FIXME: There must be a cleaner way to to this // FIXME: There must be a cleaner way to to this
// If HIL active, store PID outout into hilState and disable motor control // If HIL active, store PID outout into hilState and disable motor control
if (FLIGHT_MODE(PASSTHRU_MODE) || !STATE(FIXED_WING)) { if (FLIGHT_MODE(MANUAL_MODE) || !STATE(FIXED_WING)) {
hilToSIM.pidCommand[ROLL] = rcCommand[ROLL]; hilToSIM.pidCommand[ROLL] = rcCommand[ROLL];
hilToSIM.pidCommand[PITCH] = rcCommand[PITCH]; hilToSIM.pidCommand[PITCH] = rcCommand[PITCH];
hilToSIM.pidCommand[YAW] = rcCommand[YAW]; hilToSIM.pidCommand[YAW] = rcCommand[YAW];

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

@ -461,7 +461,7 @@ void mixTable(void)
{ {
int16_t input[3]; // RPY, range [-500:+500] int16_t input[3]; // RPY, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes // Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING) && FLIGHT_MODE(PASSTHRU_MODE)) { if (STATE(FIXED_WING) && FLIGHT_MODE(MANUAL_MODE)) {
// Direct passthru from RX // Direct passthru from RX
input[ROLL] = rcCommand[ROLL]; input[ROLL] = rcCommand[ROLL];
input[PITCH] = rcCommand[PITCH]; input[PITCH] = rcCommand[PITCH];

26
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:
@ -451,7 +451,7 @@ static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamic
} }
#ifdef AUTOTUNE_FIXED_WING #ifdef AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(PASSTHRU_MODE)) { if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm); autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
} }
#endif #endif
@ -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

4
src/main/flight/servos.c
View file

@ -43,6 +43,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/controlrate_profile.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -365,8 +366,7 @@ void servoMixer(float dT)
{ {
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
if (FLIGHT_MODE(PASSTHRU_MODE)) { if (FLIGHT_MODE(MANUAL_MODE)) {
// Direct passthru from RX
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; input[INPUT_STABILIZED_YAW] = rcCommand[YAW];

4
src/main/io/osd.c
View file

@ -951,8 +951,8 @@ static bool osdDrawSingleElement(uint8_t item)
p = "AIR "; p = "AIR ";
} }
if (FLIGHT_MODE(PASSTHRU_MODE)) if (FLIGHT_MODE(MANUAL_MODE))
p = "PASS"; p = "MANU";
else if (FLIGHT_MODE(FAILSAFE_MODE)) { else if (FLIGHT_MODE(FAILSAFE_MODE)) {
p = "!FS!"; p = "!FS!";
} else if (FLIGHT_MODE(NAV_RTH_MODE)) } else if (FLIGHT_MODE(NAV_RTH_MODE))

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -20,3 +20,5 @@
#define MSP2_INAV_STATUS 0x2000 #define MSP2_INAV_STATUS 0x2000
#define MSP2_INAV_OPTICAL_FLOW 0x2001 #define MSP2_INAV_OPTICAL_FLOW 0x2001
#define MSP2_INAV_RATE_PROFILE 0x2007
#define MSP2_INAV_SET_RATE_PROFILE 0x2008

6
src/main/navigation/navigation.c
View file

@ -2361,7 +2361,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
} }
} }
// RTH/Failsafe_RTH can override PASSTHRU // RTH/Failsafe_RTH can override MANUAL
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) { if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
// If we request forced RTH - attempt to activate it no matter what // If we request forced RTH - attempt to activate it no matter what
// This might switch to emergency landing controller if GPS is unavailable // This might switch to emergency landing controller if GPS is unavailable
@ -2369,8 +2369,8 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_RTH; return NAV_FSM_EVENT_SWITCH_TO_RTH;
} }
// PASSTHRU mode has priority over WP/PH/AH // MANUAL mode has priority over WP/PH/AH
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode
return NAV_FSM_EVENT_SWITCH_TO_IDLE; return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} }

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;
} }

2
src/main/rx/eleres.c
View file

@ -283,7 +283,7 @@ static void telemetryRX(void)
themp = (uint8_t)(thempfil/80 + 86); themp = (uint8_t)(thempfil/80 + 86);
if (FLIGHT_MODE(FAILSAFE_MODE)) wii_flymode = 7; if (FLIGHT_MODE(FAILSAFE_MODE)) wii_flymode = 7;
else if (FLIGHT_MODE(PASSTHRU_MODE)) wii_flymode = 8; else if (FLIGHT_MODE(MANUAL_MODE)) wii_flymode = 8;
else if (FLIGHT_MODE(NAV_RTH_MODE)) wii_flymode = 6; else if (FLIGHT_MODE(NAV_RTH_MODE)) wii_flymode = 6;
else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) wii_flymode = 5; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) wii_flymode = 5;
else if (FLIGHT_MODE(HEADFREE_MODE)) wii_flymode = 4; else if (FLIGHT_MODE(HEADFREE_MODE)) wii_flymode = 4;

6
src/main/target/ALIENFLIGHTF3/config.c
View file

@ -63,9 +63,9 @@ void targetConfiguration(void)
pidProfileMutable()->bank_mc.pid[PITCH].P = 36; pidProfileMutable()->bank_mc.pid[PITCH].P = 36;
failsafeConfigMutable()->failsafe_delay = 2; failsafeConfigMutable()->failsafe_delay = 2;
failsafeConfigMutable()->failsafe_off_delay = 0; failsafeConfigMutable()->failsafe_off_delay = 0;
controlRateProfilesMutable(0)->rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; controlRateProfilesMutable(0)->stabilized.rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
controlRateProfilesMutable(0)->rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; controlRateProfilesMutable(0)->stabilized.rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
controlRateProfilesMutable(0)->rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT; controlRateProfilesMutable(0)->stabilized.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
parseRcChannels("TAER1234"); parseRcChannels("TAER1234");
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R

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;
} }

2
src/main/telemetry/ltm.c
View file

@ -162,7 +162,7 @@ void ltm_sframe(sbuf_t *dst)
{ {
uint8_t lt_flightmode; uint8_t lt_flightmode;
if (FLIGHT_MODE(PASSTHRU_MODE)) if (FLIGHT_MODE(MANUAL_MODE))
lt_flightmode = 0; lt_flightmode = 0;
else if (FLIGHT_MODE(NAV_WP_MODE)) else if (FLIGHT_MODE(NAV_WP_MODE))
lt_flightmode = 10; lt_flightmode = 10;

2
src/main/telemetry/smartport.c
View file

@ -666,7 +666,7 @@ void handleSmartPortTelemetry(void)
tmpi += 10; tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE)) if (FLIGHT_MODE(HORIZON_MODE))
tmpi += 20; tmpi += 20;
if (FLIGHT_MODE(PASSTHRU_MODE)) if (FLIGHT_MODE(MANUAL_MODE))
tmpi += 40; tmpi += 40;
// hundreds column // hundreds column