mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
Improve Virtual Current Sensor
Virtual current sensor improved by taking into account ability to run motors in Nav modes with throttle low and motor stop active.
This commit is contained in:
parent
416670ddc5
commit
2e38d12f32
1 changed files with 11 additions and 1 deletions
|
@ -44,6 +44,7 @@
|
|||
#include "flight/mixer.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
|
@ -467,7 +468,16 @@ void currentMeterUpdate(timeUs_t timeDelta)
|
|||
amperage = batteryMetersConfig()->current.offset;
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
||||
int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
|
||||
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
|
||||
bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE;
|
||||
bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
|
||||
int32_t throttleOffset;
|
||||
|
||||
if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop
|
||||
throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
||||
} else {
|
||||
throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
|
||||
}
|
||||
int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
|
||||
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue