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

first update

Browse source
This commit is contained in:
breadoven 2022-12-01 12:38:54 +00:00
parent 4f49dfb621
commit a7cef4708e
11 changed files with 40 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -212,10 +212,10 @@ static void updateArmingStatus(void)
/* CHECK: Throttle */
if (!armingConfig()->fixed_wing_auto_arm) {
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
if (throttleStickIsLow()) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
}
}
@ -630,13 +630,13 @@ void processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
const bool lowThrottle = throttleStickIsLow();
// When armed and motors aren't spinning, do beeps periodically
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
static bool armedBeeperOn = false;
if (throttleStatus == THROTTLE_LOW) {
if (lowThrottle) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
@ -645,7 +645,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
processRcStickPositions(throttleStatus);
processRcStickPositions();
processAirmode();
updateActivatedModes();
@ -759,7 +759,7 @@ void processRx(timeUs_t currentTimeUs)
pidResetErrorAccumulators();
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
if (throttleStatus == THROTTLE_LOW) {
if (lowThrottle) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
ENABLE_STATE(ANTI_WINDUP);
@ -778,7 +778,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
if (throttleStatus == THROTTLE_LOW) {
if (lowThrottle) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
ENABLE_STATE(ANTI_WINDUP);
@ -802,7 +802,7 @@ void processRx(timeUs_t currentTimeUs)
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
DISABLE_STATE(ANTI_WINDUP);
//This case applies only to MR when Airmode management is throttle threshold activated
if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) {
if (lowThrottle && !STATE(AIRMODE_ACTIVE)) {
pidResetErrorAccumulators();
}
}

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

@ -127,6 +127,11 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e
return THROTTLE_HIGH;
}
bool throttleStickIsLow(void)
{
return calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}
int16_t throttleStickMixedValue(void)
{
int16_t throttleValue;
@ -181,7 +186,7 @@ static void updateRcStickPositions(void)
rcStickPositions = tmp;
}
void processRcStickPositions(throttleStatus_e throttleStatus)
void processRcStickPositions(void)
{
static timeMs_t lastTickTimeMs = 0;
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@ -209,9 +214,11 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
emergencyArmingUpdate(armingSwitchIsActive);
const bool lowThrottle = throttleStickIsLow();
if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
if (!lowThrottle) {
tryArm();
return;
}
@ -227,7 +234,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
if (armingConfig()->disarm_kill_switch || lowThrottle) {
disarm(DISARM_SWITCH);
}
}

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

@ -112,6 +112,7 @@ bool isRollPitchStickDeflected(uint8_t deadband);
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
int16_t throttleStickMixedValue(void);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);
void processRcStickPositions(void);
bool throttleStickIsLow(void);
int32_t getRcStickDeflection(int32_t axis);

2
src/main/flight/failsafe.c
View file

@ -387,7 +387,7 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
if (!throttleStickIsLow()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (!receivingRxDataAndNotFailsafeMode) {

6
src/main/flight/mixer.c
View file

@ -626,11 +626,8 @@ motorStatus_e getMotorStatus(void)
}
const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
const bool throttleStickLow =
(calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW);
if (throttleStickLow && fixedWingOrAirmodeNotActive) {
if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) {
if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) {
// If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS
// and either on a plane or on a quad with inactive airmode - stop motor
@ -652,7 +649,6 @@ motorStatus_e getMotorStatus(void)
return MOTOR_STOPPED_USER;
}
}
}
return MOTOR_RUNNING;

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

@ -4327,8 +4327,7 @@ bool isNavLaunchEnabled(void)
bool abortLaunchAllowed(void)
{
// allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
}
int32_t navigationGetHomeHeading(void)

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

@ -685,12 +685,11 @@ bool isFixedWingLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static bool fixAxisCheck = false;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
// Basic condition to start looking for landing
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| FLIGHT_MODE(FAILSAFE_MODE)
|| (!navigationIsControllingThrottle() && throttleIsLow);
|| (!navigationIsControllingThrottle() && throttleStickIsLow());
if (!startCondition || posControl.flags.resetLandingDetector) {
return fixAxisCheck = posControl.flags.resetLandingDetector = false;

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

@ -248,11 +248,6 @@ static void applyThrottleIdleLogic(bool forceMixerIdle)
}
}
static inline bool isThrottleLow(void)
{
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}
static inline bool isLaunchMaxAltitudeReached(void)
{
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
@ -272,7 +267,7 @@ static inline bool isProbablyNotFlying(void)
static void resetPidsIfNeeded(void) {
// Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) {
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && throttleStickIsLow())) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
@ -292,7 +287,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
{
UNUSED(currentTimeUs);
if (!isThrottleLow()) {
if (!throttleStickIsLow()) {
if (isThrottleIdleEnabled()) {
return FW_LAUNCH_EVENT_SUCCESS;
}
@ -312,7 +307,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -330,7 +325,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -349,7 +344,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -427,7 +422,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
if (navConfig()->fw.launch_manual_throttle) {
// reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
if (isThrottleLow()) {
if (throttleStickIsLow()) {
fwLaunch.currentStateTimeUs = currentTimeUs;
fwLaunch.pitchAngle = 0;
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {

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

@ -176,14 +176,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
void setupMulticopterAltitudeController(void)
{
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
const bool lowThrottle = throttleStickIsLow();
if (navConfig()->general.flags.use_thr_mid_for_althold) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
// If throttle status is THROTTLE_LOW - use Thr Mid anyway
if (throttleStatus == THROTTLE_LOW) {
// If throttle is LOW - use Thr Mid anyway
if (lowThrottle) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
@ -198,7 +198,7 @@ void setupMulticopterAltitudeController(void)
// Force AH controller to initialize althold integral for pending takeoff on reset
// Signal for that is low throttle _and_ low actual altitude
if (throttleStatus == THROTTLE_LOW && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
if (lowThrottle && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
prepareForTakeoffOnReset = true;
}
}
@ -724,13 +724,12 @@ bool isMulticopterLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static timeUs_t landingDetectorStartedAt;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
/* Basic condition to start looking for landing
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
|| (!navigationIsFlyingAutonomousMode() && throttleIsLow);
|| (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
if (!startCondition || posControl.flags.resetLandingDetector) {
landingDetectorStartedAt = 0;

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

@ -547,7 +547,6 @@ void currentMeterUpdate(timeUs_t timeDelta)
case CURRENT_SENSOR_VIRTUAL:
amperage = batteryMetersConfig()->current.offset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
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));
@ -556,7 +555,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
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;
throttleOffset = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
}
int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;

4
src/main/telemetry/frsky_d.c
View file

@ -195,10 +195,10 @@ static void sendThrottleOrBatterySizeAsRpm(void)
{
sendDataHead(ID_RPM);
if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
if (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) {
throttleForRPM = 0;
}
serialize16(throttleForRPM);
} else {
serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));