mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 14:25:16 +03:00
first update
This commit is contained in:
parent
4f49dfb621
commit
a7cef4708e
11 changed files with 40 additions and 45 deletions
|
@ -212,10 +212,10 @@ static void updateArmingStatus(void)
|
||||||
/* CHECK: Throttle */
|
/* CHECK: Throttle */
|
||||||
if (!armingConfig()->fixed_wing_auto_arm) {
|
if (!armingConfig()->fixed_wing_auto_arm) {
|
||||||
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
|
// 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) {
|
if (throttleStickIsLow()) {
|
||||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
|
|
||||||
} else {
|
|
||||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
|
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
|
||||||
|
} else {
|
||||||
|
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -630,13 +630,13 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
failsafeUpdateState();
|
failsafeUpdateState();
|
||||||
|
|
||||||
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
const bool lowThrottle = throttleStickIsLow();
|
||||||
|
|
||||||
// When armed and motors aren't spinning, do beeps periodically
|
// When armed and motors aren't spinning, do beeps periodically
|
||||||
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
|
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
|
||||||
static bool armedBeeperOn = false;
|
static bool armedBeeperOn = false;
|
||||||
|
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
if (lowThrottle) {
|
||||||
beeper(BEEPER_ARMED);
|
beeper(BEEPER_ARMED);
|
||||||
armedBeeperOn = true;
|
armedBeeperOn = true;
|
||||||
} else if (armedBeeperOn) {
|
} else if (armedBeeperOn) {
|
||||||
|
@ -645,7 +645,7 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
processRcStickPositions(throttleStatus);
|
processRcStickPositions();
|
||||||
processAirmode();
|
processAirmode();
|
||||||
updateActivatedModes();
|
updateActivatedModes();
|
||||||
|
|
||||||
|
@ -759,7 +759,7 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
pidResetErrorAccumulators();
|
pidResetErrorAccumulators();
|
||||||
}
|
}
|
||||||
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
|
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
if (lowThrottle) {
|
||||||
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
|
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
|
||||||
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
|
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
|
||||||
ENABLE_STATE(ANTI_WINDUP);
|
ENABLE_STATE(ANTI_WINDUP);
|
||||||
|
@ -778,7 +778,7 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
|
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
if (lowThrottle) {
|
||||||
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
|
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
|
||||||
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
|
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
|
||||||
ENABLE_STATE(ANTI_WINDUP);
|
ENABLE_STATE(ANTI_WINDUP);
|
||||||
|
@ -802,7 +802,7 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
|
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
|
||||||
DISABLE_STATE(ANTI_WINDUP);
|
DISABLE_STATE(ANTI_WINDUP);
|
||||||
//This case applies only to MR when Airmode management is throttle threshold activated
|
//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();
|
pidResetErrorAccumulators();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -127,6 +127,11 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e
|
||||||
return THROTTLE_HIGH;
|
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 throttleStickMixedValue(void)
|
||||||
{
|
{
|
||||||
int16_t throttleValue;
|
int16_t throttleValue;
|
||||||
|
@ -181,7 +186,7 @@ static void updateRcStickPositions(void)
|
||||||
rcStickPositions = tmp;
|
rcStickPositions = tmp;
|
||||||
}
|
}
|
||||||
|
|
||||||
void processRcStickPositions(throttleStatus_e throttleStatus)
|
void processRcStickPositions(void)
|
||||||
{
|
{
|
||||||
static timeMs_t lastTickTimeMs = 0;
|
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
|
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);
|
bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
|
||||||
emergencyArmingUpdate(armingSwitchIsActive);
|
emergencyArmingUpdate(armingSwitchIsActive);
|
||||||
|
|
||||||
|
const bool lowThrottle = throttleStickIsLow();
|
||||||
|
|
||||||
if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
|
if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
|
||||||
// Auto arm on throttle when using fixedwing and motorstop
|
// Auto arm on throttle when using fixedwing and motorstop
|
||||||
if (throttleStatus != THROTTLE_LOW) {
|
if (!lowThrottle) {
|
||||||
tryArm();
|
tryArm();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -227,7 +234,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
|
||||||
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
|
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
|
||||||
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
|
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
|
||||||
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
|
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
|
||||||
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
|
if (armingConfig()->disarm_kill_switch || lowThrottle) {
|
||||||
disarm(DISARM_SWITCH);
|
disarm(DISARM_SWITCH);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -112,6 +112,7 @@ bool isRollPitchStickDeflected(uint8_t deadband);
|
||||||
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
|
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
|
||||||
int16_t throttleStickMixedValue(void);
|
int16_t throttleStickMixedValue(void);
|
||||||
rollPitchStatus_e calculateRollPitchCenterStatus(void);
|
rollPitchStatus_e calculateRollPitchCenterStatus(void);
|
||||||
void processRcStickPositions(throttleStatus_e throttleStatus);
|
void processRcStickPositions(void);
|
||||||
|
bool throttleStickIsLow(void);
|
||||||
|
|
||||||
int32_t getRcStickDeflection(int32_t axis);
|
int32_t getRcStickDeflection(int32_t axis);
|
||||||
|
|
|
@ -387,7 +387,7 @@ void failsafeUpdateState(void)
|
||||||
case FAILSAFE_IDLE:
|
case FAILSAFE_IDLE:
|
||||||
if (armed) {
|
if (armed) {
|
||||||
// Track throttle command below minimum time
|
// 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;
|
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
|
||||||
}
|
}
|
||||||
if (!receivingRxDataAndNotFailsafeMode) {
|
if (!receivingRxDataAndNotFailsafeMode) {
|
||||||
|
|
|
@ -626,11 +626,8 @@ motorStatus_e getMotorStatus(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
|
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 ((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
|
// 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
|
// 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_STOPPED_USER;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return MOTOR_RUNNING;
|
return MOTOR_RUNNING;
|
||||||
|
|
|
@ -4327,8 +4327,7 @@ bool isNavLaunchEnabled(void)
|
||||||
bool abortLaunchAllowed(void)
|
bool abortLaunchAllowed(void)
|
||||||
{
|
{
|
||||||
// allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
|
// 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 throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
|
||||||
return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int32_t navigationGetHomeHeading(void)
|
int32_t navigationGetHomeHeading(void)
|
||||||
|
|
|
@ -685,12 +685,11 @@ bool isFixedWingLandingDetected(void)
|
||||||
{
|
{
|
||||||
DEBUG_SET(DEBUG_LANDING, 4, 0);
|
DEBUG_SET(DEBUG_LANDING, 4, 0);
|
||||||
static bool fixAxisCheck = false;
|
static bool fixAxisCheck = false;
|
||||||
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
|
|
||||||
|
|
||||||
// Basic condition to start looking for landing
|
// Basic condition to start looking for landing
|
||||||
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|
||||||
|| FLIGHT_MODE(FAILSAFE_MODE)
|
|| FLIGHT_MODE(FAILSAFE_MODE)
|
||||||
|| (!navigationIsControllingThrottle() && throttleIsLow);
|
|| (!navigationIsControllingThrottle() && throttleStickIsLow());
|
||||||
|
|
||||||
if (!startCondition || posControl.flags.resetLandingDetector) {
|
if (!startCondition || posControl.flags.resetLandingDetector) {
|
||||||
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
|
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
|
||||||
|
|
|
@ -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)
|
static inline bool isLaunchMaxAltitudeReached(void)
|
||||||
{
|
{
|
||||||
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
|
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) {
|
static void resetPidsIfNeeded(void) {
|
||||||
// Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
|
// 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();
|
pidResetErrorAccumulators();
|
||||||
pidResetTPAFilter();
|
pidResetTPAFilter();
|
||||||
}
|
}
|
||||||
|
@ -292,7 +287,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
|
||||||
{
|
{
|
||||||
UNUSED(currentTimeUs);
|
UNUSED(currentTimeUs);
|
||||||
|
|
||||||
if (!isThrottleLow()) {
|
if (!throttleStickIsLow()) {
|
||||||
if (isThrottleIdleEnabled()) {
|
if (isThrottleIdleEnabled()) {
|
||||||
return FW_LAUNCH_EVENT_SUCCESS;
|
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)
|
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
|
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)
|
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
|
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)
|
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
|
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) {
|
if (navConfig()->fw.launch_manual_throttle) {
|
||||||
// reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
|
// 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.currentStateTimeUs = currentTimeUs;
|
||||||
fwLaunch.pitchAngle = 0;
|
fwLaunch.pitchAngle = 0;
|
||||||
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
|
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
|
||||||
|
|
|
@ -176,14 +176,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
|
||||||
|
|
||||||
void setupMulticopterAltitudeController(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) {
|
if (navConfig()->general.flags.use_thr_mid_for_althold) {
|
||||||
altHoldThrottleRCZero = rcLookupThrottleMid();
|
altHoldThrottleRCZero = rcLookupThrottleMid();
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
// If throttle status is THROTTLE_LOW - use Thr Mid anyway
|
// If throttle is LOW - use Thr Mid anyway
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
if (lowThrottle) {
|
||||||
altHoldThrottleRCZero = rcLookupThrottleMid();
|
altHoldThrottleRCZero = rcLookupThrottleMid();
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
@ -198,7 +198,7 @@ void setupMulticopterAltitudeController(void)
|
||||||
|
|
||||||
// Force AH controller to initialize althold integral for pending takeoff on reset
|
// Force AH controller to initialize althold integral for pending takeoff on reset
|
||||||
// Signal for that is low throttle _and_ low actual altitude
|
// 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;
|
prepareForTakeoffOnReset = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -724,13 +724,12 @@ bool isMulticopterLandingDetected(void)
|
||||||
{
|
{
|
||||||
DEBUG_SET(DEBUG_LANDING, 4, 0);
|
DEBUG_SET(DEBUG_LANDING, 4, 0);
|
||||||
static timeUs_t landingDetectorStartedAt;
|
static timeUs_t landingDetectorStartedAt;
|
||||||
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
|
|
||||||
|
|
||||||
/* Basic condition to start looking for landing
|
/* Basic condition to start looking for landing
|
||||||
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
|
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
|
||||||
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|
||||||
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
|
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
|
||||||
|| (!navigationIsFlyingAutonomousMode() && throttleIsLow);
|
|| (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
|
||||||
|
|
||||||
if (!startCondition || posControl.flags.resetLandingDetector) {
|
if (!startCondition || posControl.flags.resetLandingDetector) {
|
||||||
landingDetectorStartedAt = 0;
|
landingDetectorStartedAt = 0;
|
||||||
|
|
|
@ -547,7 +547,6 @@ void currentMeterUpdate(timeUs_t timeDelta)
|
||||||
case CURRENT_SENSOR_VIRTUAL:
|
case CURRENT_SENSOR_VIRTUAL:
|
||||||
amperage = batteryMetersConfig()->current.offset;
|
amperage = batteryMetersConfig()->current.offset;
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
|
||||||
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
|
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
|
||||||
bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE;
|
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));
|
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
|
if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop
|
||||||
throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
||||||
} else {
|
} 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);
|
int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
|
||||||
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
|
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
|
||||||
|
|
|
@ -195,10 +195,10 @@ static void sendThrottleOrBatterySizeAsRpm(void)
|
||||||
{
|
{
|
||||||
sendDataHead(ID_RPM);
|
sendDataHead(ID_RPM);
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
|
|
||||||
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
|
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
|
||||||
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
|
if (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) {
|
||||||
throttleForRPM = 0;
|
throttleForRPM = 0;
|
||||||
|
}
|
||||||
serialize16(throttleForRPM);
|
serialize16(throttleForRPM);
|
||||||
} else {
|
} else {
|
||||||
serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));
|
serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue