Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-18 05:45:28 +03:00
Code Issues Wiki Activity

Disable ALTHOLD on user motor stop request

Browse source
This commit is contained in:
Michel Pastor 2018-06-04 15:10:06 +02:00
parent 99f7b1daf2
commit f046554d97
4 changed files with 40 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -38,6 +38,7 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
@ -350,17 +351,11 @@ void mixTable(const float dT)
}
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED)) {
bool failsafeMotorStop = failsafeRequiresMotorStop();
bool navMotorStop = !failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE);
bool userMotorStop = !navigationIsFlyingAutonomousMode() && !failsafeIsActive() && (rcData[THROTTLE] < rxConfig()->mincheck);
if (failsafeMotorStop || navMotorStop || userMotorStop) {
if (feature(FEATURE_3D)) {
motor[i] = PWM_RANGE_MIDDLE;
}
else {
motor[i] = motorConfig()->mincommand;
}
if (ARMING_FLAG(ARMED) && (getMotorStatus() != MOTOR_RUNNING)) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
} else {
motor[i] = motorConfig()->minthrottle;
}
}
}
@ -373,3 +368,14 @@ void mixTable(const float dT)
/* Apply motor acceleration/deceleration limit */
applyMotorRateLimiting(dT);
}
motorStatus_e getMotorStatus(void)
{
if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)))
return MOTOR_STOPPED_AUTO;
if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!navigationIsFlyingAutonomousMode()) && (!failsafeIsActive()) && (rcData[THROTTLE] < rxConfig()->mincheck))
return MOTOR_STOPPED_USER;
return MOTOR_RUNNING;
}

7
src/main/flight/mixer.h
View file

@ -85,12 +85,19 @@ typedef struct motorConfig_s {
PG_DECLARE(motorConfig_t, motorConfig);
typedef enum {
MOTOR_STOPPED_USER,
MOTOR_STOPPED_AUTO,
MOTOR_RUNNING
} motorStatus_e;
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
uint8_t getMotorCount(void);
float getMotorMixRange(void);
bool mixerIsOutputSaturated(void);
motorStatus_e getMotorStatus(void);
void writeAllMotors(int16_t mc);
void mixerUsePWMIOConfiguration(void);

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

@ -41,6 +41,7 @@
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/beeper.h"
@ -2762,7 +2763,7 @@ rthState_e getStateOfForcedRTH(void)
bool navigationIsControllingThrottle(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER));
}
bool navigationIsFlyingAutonomousMode(void)

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

@ -527,23 +527,25 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
#ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY
// Don't apply anything if ground speed is too low (<3m/s)
if (posControl.actualState.velXY > 300) {
if (navStateFlags & NAV_CTL_ALT)
#else
if (true) {
#endif
if (navStateFlags & NAV_CTL_ALT) {
if (getMotorStatus() == MOTOR_STOPPED_USER) {
// Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control
resetFixedWingAltitudeController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
} else
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
}
if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTimeUs);
}
else {
} else {
posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0;
}
#else
if (navStateFlags & NAV_CTL_ALT)
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTimeUs);
#endif
//if (navStateFlags & NAV_CTL_YAW)
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))