From 339b1047e02d376da33e626c55e83ebf3d7ed48a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 18 Feb 2020 17:39:38 +0100 Subject: [PATCH] Add new stateds --- src/main/blackbox/blackbox.c | 8 +-- src/main/fc/fc_core.c | 12 ++-- src/main/fc/fc_msp_box.c | 12 ++-- src/main/fc/rc_controls.c | 2 +- src/main/fc/rc_modes.c | 2 +- src/main/fc/runtime_config.h | 4 +- src/main/flight/failsafe.c | 2 +- src/main/flight/hil.c | 2 +- src/main/flight/imu.c | 8 +-- src/main/flight/mixer.c | 18 +++--- src/main/flight/pid.c | 8 +-- src/main/flight/rth_estimator.c | 4 +- src/main/flight/wind_estimator.c | 2 +- src/main/io/osd.c | 4 +- src/main/navigation/navigation.c | 64 +++++++++---------- .../navigation/navigation_pos_estimator.c | 2 +- src/main/telemetry/crsf.c | 2 +- src/main/telemetry/mavlink.c | 2 +- 18 files changed, 80 insertions(+), 78 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 50a1f4ac61..5dcd4346b5 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -624,14 +624,14 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV: #ifdef USE_NAV - return STATE(AIRPLANE_ROVER_BOAT); + return STATE(FIXED_WING); #else return false; #endif case FLIGHT_LOG_FIELD_CONDITION_MC_NAV: #ifdef USE_NAV - return !STATE(AIRPLANE_ROVER_BOAT); + return !STATE(FIXED_WING); #else return false; #endif @@ -1369,7 +1369,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->magADC[i] = mag.magADC[i]; #endif #ifdef USE_NAV - if (!STATE(AIRPLANE_ROVER_BOAT)) { + if (!STATE(FIXED_WING)) { // log requested velocity in cm/s blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained); @@ -1384,7 +1384,7 @@ static void loadMainState(timeUs_t currentTimeUs) } #ifdef USE_NAV - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { // log requested pitch in decidegrees blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d48906e5a9..5d417f702b 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -532,7 +532,7 @@ void processRx(timeUs_t currentTimeUs) const throttleStatus_e throttleStatus = calculateThrottleStatus(); // When armed and motors aren't spinning, do beeps periodically - if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(AIRPLANE_ROVER_BOAT)) { + if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)) { static bool armedBeeperOn = false; if (throttleStatus == THROTTLE_LOW) { @@ -633,7 +633,7 @@ void processRx(timeUs_t currentTimeUs) #endif // Handle passthrough mode - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough (!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating ENABLE_FLIGHT_MODE(MANUAL_MODE); @@ -649,13 +649,13 @@ void processRx(timeUs_t currentTimeUs) /* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */ pidResetErrorAccumulators(); } - else if (STATE(AIRPLANE_ROVER_BOAT) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { + else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleStatus == THROTTLE_LOW) { if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(); // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs - if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(AIRPLANE_ROVER_BOAT))) { + if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) { ENABLE_STATE(ANTI_WINDUP); } else { @@ -753,7 +753,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; - if (ARMING_FLAG(ARMED) && (!STATE(AIRPLANE_ROVER_BOAT) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { + if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { flightTime += cycleTime; updateAccExtremes(); } @@ -782,7 +782,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) #endif // Apply throttle tilt compensation - if (!STATE(AIRPLANE_ROVER_BOAT)) { + if (!STATE(FIXED_WING)) { int16_t thrTiltCompStrength = 0; if (navigationRequiresThrottleTiltCompensation()) { diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 3f1286d37d..03e7bd29eb 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -187,17 +187,17 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; #ifdef USE_GPS - if (sensors(SENSOR_BARO) || (STATE(AIRPLANE_ROVER_BOAT) && feature(FEATURE_GPS))) { + if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) { activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; activeBoxIds[activeBoxIdCount++] = BOXSURFACE; } - const bool navReadyQuads = !STATE(AIRPLANE_ROVER_BOAT) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navReadyPlanes = STATE(AIRPLANE_ROVER_BOAT) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyQuads = !STATE(FIXED_WING) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) { activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; } } @@ -209,7 +209,7 @@ void initActiveBoxIds(void) if (feature(FEATURE_GPS)) { activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; } } @@ -223,7 +223,7 @@ void initActiveBoxIds(void) #endif - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { activeBoxIds[activeBoxIdCount++] = BOXMANUAL; if (!feature(FEATURE_FW_LAUNCH)) { activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 6c44c1deba..86c41fac6b 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -183,7 +183,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); emergencyArmingUpdate(armingSwitchIsActive); - if (STATE(AIRPLANE_ROVER_BOAT) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { + if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop if (throttleStatus != THROTTLE_LOW) { tryArm(); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index fe31cfd63c..01f84af7f4 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -104,7 +104,7 @@ static void processAirmodeMultirotor(void) { void processAirmode(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { processAirmodeAirplane(); } else { processAirmodeMultirotor(); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 2527bb0bee..1f5772850a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -110,7 +110,9 @@ typedef enum { GPS_FIX = (1 << 1), CALIBRATE_MAG = (1 << 2), SMALL_ANGLE = (1 << 3), - AIRPLANE_ROVER_BOAT = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code + FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code + ROVER = (1 << 4), // It shares the same bitmask as FIXED_WING by purpose + BOAT = (1 << 4), // It shares the same bitmask as FIXED_WING by purpose ANTI_WINDUP = (1 << 5), FLAPERON_AVAILABLE = (1 << 6), NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index e53e2dc525..b03da0541e 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -258,7 +258,7 @@ void failsafeApplyControlInput(void) { // Prepare FAILSAFE_CHANNEL_AUTO values for rcCommand int16_t autoRcCommand[4]; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); diff --git a/src/main/flight/hil.c b/src/main/flight/hil.c index 652d242266..59fe5b47ef 100644 --- a/src/main/flight/hil.c +++ b/src/main/flight/hil.c @@ -57,7 +57,7 @@ void hilUpdateControlState(void) { // FIXME: There must be a cleaner way to to this // If HIL active, store PID outout into hilState and disable motor control - if (FLIGHT_MODE(MANUAL_MODE) || !STATE(AIRPLANE_ROVER_BOAT)) { + if (FLIGHT_MODE(MANUAL_MODE) || !STATE(FIXED_WING)) { hilToSIM.pidCommand[ROLL] = rcCommand[ROLL]; hilToSIM.pidCommand[PITCH] = rcCommand[PITCH]; hilToSIM.pidCommand[YAW] = rcCommand[YAW]; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 1ab543290a..3dfdf29b81 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -481,7 +481,7 @@ static float imuCalculateAccelerometerWeight(const float dT) const float nearness = ABS(100 - (accMagnitudeSq * 100)); const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f; - // Experiment: if rotation rate on a AIRPLANE_ROVER_BOAT is higher than a threshold - centrifugal force messes up too much and we + // Experiment: if rotation rate on a FIXED_WING is higher than a threshold - centrifugal force messes up too much and we // should not use measured accel for AHRS comp // Centrifugal acceleration AccelC = Omega^2 * R = Speed^2 / R // Omega = Speed / R @@ -499,7 +499,7 @@ static float imuCalculateAccelerometerWeight(const float dT) // Default - don't apply rate/ignore scaling float accWeight_RateIgnore = 1.0f; - if (ARMING_FLAG(ARMED) && STATE(AIRPLANE_ROVER_BOAT) && imuConfig()->acc_ignore_rate) { + if (ARMING_FLAG(ARMED) && STATE(FIXED_WING) && imuConfig()->acc_ignore_rate) { const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT); @@ -532,7 +532,7 @@ static void imuCalculateEstimatedAttitude(float dT) bool useCOG = false; #if defined(USE_GPS) - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { bool canUseCOG = isGPSHeadingValid(); // Prefer compass (if available) @@ -670,7 +670,7 @@ bool isImuReady(void) bool isImuHeadingValid(void) { - return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(AIRPLANE_ROVER_BOAT) && gpsHeadingInitialized); + return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING) && gpsHeadingInitialized); } float calculateCosTiltAngle(void) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5f220064d4..eb14b28c1a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -147,14 +147,14 @@ bool mixerIsOutputSaturated(void) void mixerUpdateStateFlags(void) { - if ( - mixerConfig()->platformType == PLATFORM_AIRPLANE || - mixerConfig()->platformType == PLATFORM_BOAT || - mixerConfig()->platformType == PLATFORM_ROVER - ) { - ENABLE_STATE(AIRPLANE_ROVER_BOAT); + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { + ENABLE_STATE(FIXED_WING); + } if (mixerConfig()->platformType == PLATFORM_ROVER) { + ENABLE_STATE(ROVER); + } if (mixerConfig()->platformType == PLATFORM_BOAT) { + ENABLE_STATE(BOAT); } else { - DISABLE_STATE(AIRPLANE_ROVER_BOAT); + DISABLE_STATE(FIXED_WING); } if (mixerConfig()->hasFlaps) { @@ -302,7 +302,7 @@ void FAST_CODE NOINLINE mixTable(const float dT) { int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes - if (STATE(AIRPLANE_ROVER_BOAT) && FLIGHT_MODE(MANUAL_MODE)) { + if (STATE(FIXED_WING) && FLIGHT_MODE(MANUAL_MODE)) { // Direct passthru from RX input[ROLL] = rcCommand[ROLL]; input[PITCH] = rcCommand[PITCH]; @@ -439,7 +439,7 @@ motorStatus_e getMotorStatus(void) } if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) { - if ((STATE(AIRPLANE_ROVER_BOAT) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { + if ((STATE(FIXED_WING) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { return MOTOR_STOPPED_USER; } } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7c3f6ef0de..4dbe03395e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -530,7 +530,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle - if ((axis == FD_PITCH) && STATE(AIRPLANE_ROVER_BOAT) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) + if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); @@ -857,7 +857,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) targetRates.x = 0.0f; targetRates.y = 0.0f; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { if (calculateCosTiltAngle() >= 0.173648f) { // Ideal banked turn follow the equations: // forward_vel^2 / radius = Gravity * tan(roll_angle) @@ -901,7 +901,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f); } else { @@ -1014,7 +1014,7 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex) if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) { return usedPidControllerType; } - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) { return PID_TYPE_NONE; } diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 6195f9e386..5715000714 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -146,10 +146,10 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee // returns Wh static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { // Fixed wing only for now - if (!STATE(AIRPLANE_ROVER_BOAT)) + if (!STATE(FIXED_WING)) return -1; - if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(AIRPLANE_ROVER_BOAT)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid() + if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid() #ifdef USE_WIND_ESTIMATOR && isEstimatedWindSpeedValid() #endif diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index d536e5bf71..dcb49f0080 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -79,7 +79,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) { static timeUs_t lastUpdateUs = 0; - if (!STATE(AIRPLANE_ROVER_BOAT) || + if (!STATE(FIXED_WING) || !isGPSHeadingValid() || !gpsSol.flags.validVelNE || !gpsSol.flags.validVelD) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 57c9a3cd09..a61947d4e3 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -811,7 +811,7 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_LAND_IN_PROGRESS: return OSD_MESSAGE_STR("LANDING"); case MW_NAV_STATE_HOVER_ABOVE_HOME: - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { return OSD_MESSAGE_STR("LOITERING AROUND HOME"); } return OSD_MESSAGE_STR("HOVERING"); @@ -2113,7 +2113,7 @@ static bool osdDrawSingleElement(uint8_t item) if (navStateMessage) { messages[messageCount++] = navStateMessage; } - } else if (STATE(AIRPLANE_ROVER_BOAT) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + } else if (STATE(FIXED_WING) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = "AUTOLAUNCH"; } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5934f346cf..3cfe2459ef 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -841,7 +841,7 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void) static bool navTerrainFollowingRequested(void) { // Terrain following not supported on FIXED WING aircraft yet - return !STATE(AIRPLANE_ROVER_BOAT) && IS_RC_MODE_ACTIVE(BOXSURFACE); + return !STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXSURFACE); } /*************************************************************************************************/ @@ -939,7 +939,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na { const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - if (!STATE(AIRPLANE_ROVER_BOAT)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now + if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now DEBUG_SET(DEBUG_CRUISE, 0, 1); if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration. @@ -1018,7 +1018,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState) { - if (!STATE(AIRPLANE_ROVER_BOAT)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now + if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); @@ -1049,7 +1049,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (STATE(AIRPLANE_ROVER_BOAT) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { + if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -1078,7 +1078,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati else { fpVector3_t targetHoldPos; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { // Airplane - climbout before turning around calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away } else { @@ -1115,14 +1115,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // If we have valid pos sensor OR we are configured to ignore GPS loss if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { - const uint8_t rthClimbMarginPercent = STATE(AIRPLANE_ROVER_BOAT) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT; + const uint8_t rthClimbMarginPercent = STATE(FIXED_WING) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT; const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); } @@ -1130,7 +1130,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n posControl.rthState.rthInitialDistance = posControl.homeDistance; fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); - if (navConfig()->general.flags.rth_tail_first && !STATE(AIRPLANE_ROVER_BOAT)) { + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); } else { @@ -1144,12 +1144,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); /* For multi-rotors execute sanity check during initial ascent as well */ - if (!STATE(AIRPLANE_ROVER_BOAT) && !validateRTHSanityChecker()) { + if (!STATE(FIXED_WING) && !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } // Climb to safe altitude and turn to correct direction - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); } else { @@ -1222,7 +1222,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { // Wait until target heading is reached (with 15 deg margin for error) - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { resetLandingDetector(); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; @@ -1673,7 +1673,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) case RTH_HOME_ENROUTE_PROPORTIONAL: { - float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(AIRPLANE_ROVER_BOAT) ? navConfig()->fw.loiter_radius : 0); + float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0); if (rthTotalDistanceToTravel >= 100) { float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f); posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled)); @@ -2052,7 +2052,7 @@ static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWayp // We consider waypoint reached if within specified radius posControl.wpDistance = calculateDistanceToDestination(pos); - if (STATE(AIRPLANE_ROVER_BOAT) && isWaypointHome) { + if (STATE(FIXED_WING) && isWaypointHome) { // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check return (posControl.wpDistance <= navConfig()->general.waypoint_radius) || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius @@ -2311,7 +2311,7 @@ uint32_t getTotalTravelDistance(void) *-----------------------------------------------------------*/ void calculateInitialHoldPosition(fpVector3_t * pos) { - if (STATE(AIRPLANE_ROVER_BOAT)) { // AIRPLANE_ROVER_BOAT + if (STATE(FIXED_WING)) { // FIXED_WING calculateFixedWingInitialHoldPosition(pos); } else { @@ -2368,7 +2368,7 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc *-----------------------------------------------------------*/ void resetLandingDetector(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { // AIRPLANE_ROVER_BOAT + if (STATE(FIXED_WING)) { // FIXED_WING resetFixedWingLandingDetector(); } else { @@ -2380,7 +2380,7 @@ bool isLandingDetected(void) { bool landingDetected; - if (STATE(AIRPLANE_ROVER_BOAT)) { // AIRPLANE_ROVER_BOAT + if (STATE(FIXED_WING)) { // FIXED_WING landingDetected = isFixedWingLandingDetected(); } else { @@ -2406,7 +2406,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti posControl.desiredState.pos.z = altitudeToUse; } else { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { // Fixed wing climb rate controller is open-loop. We simply move the known altitude target float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); @@ -2429,7 +2429,7 @@ static void resetAltitudeController(bool useTerrainFollowing) // Set terrain following flag posControl.flags.isTerrainFollowEnabled = useTerrainFollowing; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { resetFixedWingAltitudeController(); } else { @@ -2439,7 +2439,7 @@ static void resetAltitudeController(bool useTerrainFollowing) static void setupAltitudeController(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { setupFixedWingAltitudeController(); } else { @@ -2449,7 +2449,7 @@ static void setupAltitudeController(void) static bool adjustAltitudeFromRCInput(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { return adjustFixedWingAltitudeFromRCInput(); } else { @@ -2462,7 +2462,7 @@ static bool adjustAltitudeFromRCInput(void) *-----------------------------------------------------------*/ static void resetHeadingController(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { resetFixedWingHeadingController(); } else { @@ -2472,7 +2472,7 @@ static void resetHeadingController(void) static bool adjustHeadingFromRCInput(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { return adjustFixedWingHeadingFromRCInput(); } else { @@ -2485,7 +2485,7 @@ static bool adjustHeadingFromRCInput(void) *-----------------------------------------------------------*/ static void resetPositionController(void) { - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { resetFixedWingPositionController(); } else { @@ -2498,7 +2498,7 @@ static bool adjustPositionFromRCInput(void) { bool retValue; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { retValue = adjustFixedWingPositionFromRCInput(); } else { @@ -2766,7 +2766,7 @@ static void processNavigationRCAdjustments(void) posControl.flags.isAdjustingPosition = adjustPositionFromRCInput(); } else { - if (!STATE(AIRPLANE_ROVER_BOAT)) { + if (!STATE(FIXED_WING)) { resetMulticopterBrakingMode(); } } @@ -2818,7 +2818,7 @@ void applyWaypointNavigationAndAltitudeHold(void) /* Process controllers */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { applyFixedWingNavigationController(navStateFlags, currentTimeUs); } else { @@ -2891,7 +2891,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) bool canActivateNavigation = canActivateNavigationModes(); // LAUNCH mode has priority over any other NAV mode - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now if (canActivateLaunchMode) { canActivateLaunchMode = false; @@ -2974,7 +2974,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) *-----------------------------------------------------------*/ bool navigationRequiresThrottleTiltCompensation(void) { - return !STATE(AIRPLANE_ROVER_BOAT) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT); + return !STATE(FIXED_WING) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT); } /*----------------------------------------------------------- @@ -2983,7 +2983,7 @@ bool navigationRequiresThrottleTiltCompensation(void) bool navigationRequiresAngleMode(void) { const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState); - return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(AIRPLANE_ROVER_BOAT)); + return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING)); } /*----------------------------------------------------------- @@ -2992,7 +2992,7 @@ bool navigationRequiresAngleMode(void) bool navigationRequiresTurnAssistance(void) { const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState); - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { // For airplanes turn assistant is always required when controlling position return (currentState & (NAV_CTL_POS | NAV_CTL_ALT)); } @@ -3007,7 +3007,7 @@ bool navigationRequiresTurnAssistance(void) int8_t navigationGetHeadingControlState(void) { // For airplanes report as manual heading control - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { return NAV_HEADING_CONTROL_MANUAL; } @@ -3032,7 +3032,7 @@ bool navigationTerrainFollowingEnabled(void) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { - const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(AIRPLANE_ROVER_BOAT) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(AIRPLANE_ROVER_BOAT) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); if (usedBypass) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 875c197efc..d190c0c041 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -581,7 +581,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) return true; } - else if (STATE(AIRPLANE_ROVER_BOAT) && (ctx->newFlags & EST_GPS_Z_VALID)) { + else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) { // If baro is not available - use GPS Z for correction on a plane // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 87f0b47218..449e92f625 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -490,7 +490,7 @@ void initCrsfTelemetry(void) } #endif #if defined(USE_BARO) || defined(USE_GPS) - if (sensors(SENSOR_BARO) || (STATE(AIRPLANE_ROVER_BOAT) && feature(FEATURE_GPS))) { + if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) { crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); } #endif diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ccb5928d87..10901f62d9 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -467,7 +467,7 @@ void mavlinkSendHUDAndHeartbeat(void) flightModeForTelemetry_e flm = getFlightModeForTelemetry(); uint8_t mavCustomMode; - if (STATE(AIRPLANE_ROVER_BOAT)) { + if (STATE(FIXED_WING)) { mavCustomMode = inavToArduPlaneMap[flm]; } else {