rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix OSD throttle position element calculation and add 3D support 

Previously the Throttle Position element calculation was based solely on the raw rcCommand value and didn't take into account the min_check deadzone so the resulting displayed percentage was incorrect. Also 3D trottle handling was not considered.

Corrected the calculation and added support for 3D throttle modes. Reversed thrust will be represented with negative throttle percentages.
This commit is contained in:
Bruce Luckcuck 2018-12-08 12:58:06 -05:00
parent 586eef94e5
commit 5ed1bbb2a9
4 changed files with 26 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/main/fc/core.c
View File

@ -570,28 +570,38 @@ void runawayTakeoffTemporaryDisable(uint8_t disableFlag)
// calculate the throttle stick percent - integer math is good enough here.
uint8_t calculateThrottlePercent(void)
// returns negative values for reversed thrust in 3D mode
int8_t calculateThrottlePercent(void)
{
uint8_t ret = 0;
int channelData = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
if (featureIsEnabled(FEATURE_3D)
&& !IS_RC_MODE_ACTIVE(BOX3D)
&& !flight3DConfig()->switched_mode3d) {
if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) {
ret = 100;
} else {
if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
} else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
}
if (channelData > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
ret = ((channelData - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
} else if (channelData < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
ret = -((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - channelData) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
}
} else {
ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
ret = constrain(((channelData - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
if (featureIsEnabled(FEATURE_3D)
&& IS_RC_MODE_ACTIVE(BOX3D)
&& flight3DConfig()->switched_mode3d) {
ret = -ret; // 3D on a switch is active
}
}
return ret;
}
uint8_t calculateThrottlePercentAbs(void)
{
return ABS(calculateThrottlePercent());
}
static bool airmodeIsActivated;
bool isAirmodeActivated()
@ -629,7 +639,7 @@ bool processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const uint8_t throttlePercent = calculateThrottlePercent();
const uint8_t throttlePercent = calculateThrottlePercentAbs();
const bool launchControlActive = isLaunchControlActive();

4
src/main/fc/core.h
View File

@ -68,7 +68,8 @@ void updateArmingStatus(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
bool isFlipOverAfterCrashActive(void);
int8_t calculateThrottlePercent(void);
uint8_t calculateThrottlePercentAbs(void);
void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
bool isAirmodeActivated();
timeUs_t getLastDisarmTimeUs(void);
@ -78,4 +79,3 @@ void resetTryingToArm();
void subTaskTelemetryPollSensors(timeUs_t currentTimeUs);
bool isLaunchControlActive(void);

2
src/main/io/osd.c
View File

@ -764,7 +764,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_THROTTLE_POS:
buff[0] = SYM_THR;
buff[1] = SYM_THR1;
tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
tfp_sprintf(buff + 2, "%3d", calculateThrottlePercent());
break;
#if defined(USE_VTX_COMMON)

1
src/test/unit/osd_unittest.cc
View File

@ -1079,4 +1079,5 @@ extern "C" {
bool pidOsdAntiGravityActive(void) { return false; }
bool failsafeIsActive(void) { return false; }
bool gpsRescueIsConfigured(void) { return false; }
int8_t calculateThrottlePercent(void) { return 0; }
}