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

Add sticks restriction to continuous trim

Browse source
This commit is contained in:
Alexander van Saase 2021-06-05 11:31:16 +02:00
parent cf7d62a350
commit 81fdf6d5ae
9 changed files with 36 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

12
docs/Settings.md
View file

@ -482,6 +482,16 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho
---
### control_deadband
Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered.
| Default | Min | Max |
| --- | --- | --- |
| 10 | 2 | 250 |
---
### cpu_underclock
This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz
@ -4664,7 +4674,7 @@ Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER
### pos_hold_deadband
Stick deadband in [r/c points], applied after r/c deadband and expo
Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes.
| Default | Min | Max |
| --- | --- | --- |

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

@ -216,7 +216,7 @@ static void updateArmingStatus(void)
/* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */
if (isNavLaunchEnabled()) {
if (areSticksDeflectedMoreThanPosHoldDeadband()) {
if (areSticksDeflected()) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
} else {
DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);

7
src/main/fc/rc_controls.c
View file

@ -71,12 +71,13 @@ stickPositions_e rcStickPositions;
FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 3);
PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = SETTING_DEADBAND_DEFAULT,
.yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT,
.pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT,
.control_deadband = SETTING_CONTROL_DEADBAND_DEFAULT,
.alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT,
.mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT,
.airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT,
@ -97,9 +98,9 @@ bool areSticksInApModePosition(uint16_t ap_mode)
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
bool areSticksDeflectedMoreThanPosHoldDeadband(void)
bool areSticksDeflected(void)
{
return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
return ABS(rcCommand[ROLL]) > rcControlsConfig()->control_deadband || ABS(rcCommand[PITCH]) > rcControlsConfig()->control_deadband || ABS(rcCommand[YAW]) > rcControlsConfig()->control_deadband;
}
throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)

5
src/main/fc/rc_controls.h
View file

@ -84,7 +84,8 @@ extern int16_t rcCommand[4];
typedef struct rcControlsConfig_s {
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
uint8_t pos_hold_deadband; // Deadband for position hold
uint8_t control_deadband; // General deadband to check if sticks are deflected
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC
uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered
@ -106,7 +107,7 @@ stickPositions_e getRcStickPositions(void);
bool checkStickPosition(stickPositions_e stickPos);
bool areSticksInApModePosition(uint16_t ap_mode);
bool areSticksDeflectedMoreThanPosHoldDeadband(void);
bool areSticksDeflected(void);
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);

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

@ -1505,7 +1505,12 @@ groups:
min: 0
max: 100
- name: pos_hold_deadband
description: "Stick deadband in [r/c points], applied after r/c deadband and expo"
description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes."
default_value: 10
min: 2
max: 250
- name: control_deadband
description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered."
default_value: 10
min: 2
max: 250

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

@ -1314,15 +1314,9 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
//Iterm should freeze when sticks are deflected
bool areSticksDeflected = false;
for (int stick = ROLL; stick <= YAW; stick++) {
areSticksDeflected = areSticksDeflected ||
rxGetChannelValue(stick) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) ||
rxGetChannelValue(stick) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband);
}
if (
!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
areSticksDeflected ||
areSticksDeflected() ||
(!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
navigationIsControllingAltitude()
) {

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

@ -490,6 +490,7 @@ void processServoAutotrimMode(void)
#define SERVO_AUTOTRIM_CENTER_MIN 1300
#define SERVO_AUTOTRIM_CENTER_MAX 1700
#define SERVO_AUTOTRIM_UPDATE_SIZE 5
#define SERVO_AUTOTRIM_ATIITUDE_LIMIT 50 // 5 degrees
void processContinuousServoAutotrim(const float dT)
{
@ -497,21 +498,22 @@ void processContinuousServoAutotrim(const float dT)
static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static uint32_t servoMiddleUpdateCount;
const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
const float targetRateMagnitude = getTotalRateTarget();
const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, sqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
if (ARMING_FLAG(ARMED)) {
trimState = AUTOTRIM_COLLECTING;
if ((millis() - lastUpdateTimeMs) > 500) {
const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f;
const bool sticksAreCentered = !areSticksDeflected();
const bool planeIsFlyingLevel = (ABS(attitude.values.pitch) + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT
&& ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT;
if (
planeIsFlyingStraight &&
noRotationCommanded &&
planeIsFlyingLevel &&
sticksAreCentered &&
!FLIGHT_MODE(MANUAL_MODE) &&
isGPSHeadingValid() // TODO: proper flying detection
) {

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

@ -1728,7 +1728,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if (feature(FEATURE_FW_LAUNCH)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflected())) {
abortFixedWingLaunch();
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}

4
src/main/navigation/navigation_fw_launch.c
View file

@ -251,7 +251,7 @@ static inline bool isLaunchMaxAltitudeReached(void)
static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
{
return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflected();
}
static void resetPidsIfNeeded(void) {
@ -435,7 +435,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
if (areSticksDeflectedMoreThanPosHoldDeadband()) {
if (areSticksDeflected()) {
return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
}
if (elapsedTimeMs > endTimeMs) {