Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Improve Virtual Current Sensor

Browse source 
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:
breadoven 2020-12-03 23:31:54 +00:00
parent 416670ddc5
commit 2e38d12f32
1 changed files with 11 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/sensors/battery.c
View file

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