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

rescale deadbanded values to original range (#6925)

Browse source
This commit is contained in:
Alexander van Saase 2021-05-11 16:11:03 +02:00 • committed by GitHub
parent 004b3eab7c
commit 11d543f950
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 21 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/common/maths.c
View file

@ -142,6 +142,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value;
}
int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max)
{
if (ABS(value) < deadband) {
value = 0;
} else if (value > 0) {
value = scaleRange(value - deadband, 0, max - deadband, 0, max);
} else if (value < 0) {
value = scaleRange(value + deadband, min + deadband, 0, min, 0);
}
return value;
}
int32_t constrain(int32_t amt, int32_t low, int32_t high)
{
if (amt < low)

1
src/main/common/maths.h
View file

@ -132,6 +132,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
int gcd(int num, int denom);
int32_t applyDeadband(int32_t value, int32_t deadband);
int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max);
int32_t constrain(int32_t amt, int32_t low, int32_t high);
float constrainf(float amt, float low, float high);

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

@ -171,7 +171,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
int16_t stickDeflection;
stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
stickDeflection = applyDeadband(stickDeflection, deadband);
stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
return rcLookup(stickDeflection, rate);
}

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

@ -2716,8 +2716,8 @@ static bool adjustPositionFromRCInput(void)
}
else {
const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500);
const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
}

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

@ -106,7 +106,7 @@ void resetFixedWingAltitudeController(void)
bool adjustFixedWingAltitudeFromRCInput(void)
{
int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband);
int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500);
if (rcAdjustment) {
// set velocity proportional to stick movement
@ -281,7 +281,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) {
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
if (rcRollAdjustment) {
float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod;
@ -295,7 +295,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
bool adjustFixedWingPositionFromRCInput(void)
{
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
return (rcRollAdjustment);
}
@ -650,7 +650,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
}
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition)
rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500);
//if (navStateFlags & NAV_CTL_YAW)
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))

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

@ -132,7 +132,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
return true;
}
else {
const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband);
const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);
if (rcThrottleAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate;