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

Add new stateds

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-02-18 17:39:38 +01:00
parent d5d47b3515
commit 339b1047e0
18 changed files with 80 additions and 78 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/blackbox/blackbox.c
View file

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

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

@ -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()) {

12
src/main/fc/fc_msp_box.c
View file

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

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

@ -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();

2
src/main/fc/rc_modes.c
View file

@ -104,7 +104,7 @@ static void processAirmodeMultirotor(void) {
void processAirmode(void) {
if (STATE(AIRPLANE_ROVER_BOAT)) {
if (STATE(FIXED_WING)) {
processAirmodeAirplane();
} else {
processAirmodeMultirotor();

4
src/main/fc/runtime_config.h
View file

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

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

@ -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]);

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

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

8
src/main/flight/imu.c
View file

@ -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)

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

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

8
src/main/flight/pid.c
View file

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

4
src/main/flight/rth_estimator.c
View file

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

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

@ -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) {

4
src/main/io/osd.c
View file

@ -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()) {

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

@ -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) {

2
src/main/navigation/navigation_pos_estimator.c
View file

@ -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)) {

2
src/main/telemetry/crsf.c
View file

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

2
src/main/telemetry/mavlink.c
View file

@ -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 {