diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0d0863760d..c51519927b 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -507,6 +507,18 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm) return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT; } +uint16_t emergencyInFlightRearmTimeMS(void) +{ + uint16_t rearmMS = 0; + + if (STATE(IN_FLIGHT_EMERG_REARM)) { + timeMs_t currentTimeMs = millis(); + rearmMS = (uint16_t)((US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS) - currentTimeMs); + } + + return rearmMS; +} + bool emergInflightRearmEnabled(void) { /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */ @@ -880,7 +892,6 @@ static void applyThrottleTiltCompensation(void) void taskMainPidLoop(timeUs_t currentTimeUs) { - cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; @@ -899,7 +910,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } } - if (armTime > 1 * USECS_PER_SEC) { // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose + if (armTime > 1 * USECS_PER_SEC) { + // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose DISABLE_STATE(IN_FLIGHT_EMERG_REARM); } diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index c66a0050ba..455e5b3849 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -42,7 +42,9 @@ timeUs_t getLastDisarmTimeUs(void); void tryArm(void); disarmReason_t getDisarmReason(void); +uint16_t emergencyInFlightRearmTimeMS(void); bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm); +bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 5b6c624139..bda9321b6b 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -105,6 +105,7 @@ typedef enum { TURTLE_MODE = (1 << 15), SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), + NAV_FW_AUTOLAND = (1 << 18), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 780de21b0a..2a32dbbd12 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -242,6 +242,10 @@ static void failsafeActivate(failsafePhase_e newPhase) ENABLE_FLIGHT_MODE(FAILSAFE_MODE); failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; +#ifdef USE_FW_AUTOLAND + posControl.fwLandState.landAborted = false; +#endif + failsafeState.events++; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d913802d55..3da150e39d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -594,7 +594,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) { // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle #ifdef USE_FW_AUTOLAND - if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) { + if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { #else if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { #endif diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index dd120a2295..7067ac140f 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -293,13 +293,12 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_THR: return BF_SYM_THR; -/* case SYM_TEMP_F: - return BF_SYM_TEMP_F; + return BF_SYM_F; case SYM_TEMP_C: - return BF_SYM_TEMP_C; -*/ + return BF_SYM_C; + case SYM_BLANK: return BF_SYM_BLANK; /* diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e792d87b6d..3436de93ed 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -28,6 +28,7 @@ #include #include #include +#include #include "platform.h" @@ -986,7 +987,7 @@ static const char * osdFailsafePhaseMessage(void) static const char * osdFailsafeInfoMessage(void) { - if (failsafeIsReceivingRxData()) { + if (failsafeIsReceivingRxData() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { // User must move sticks to exit FS mode return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); } @@ -2362,7 +2363,7 @@ static bool osdDrawSingleElement(uint8_t item) { char *p = "ACRO"; #ifdef USE_FW_AUTOLAND - if (isFwLandInProgess()) + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else #endif @@ -4672,8 +4673,16 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } + uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0; + if (savingSettings == true) { displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); + } else if (rearmMs > 0) { // Show rearming time if settings not actively being saved. Ignore the settings saved message if rearm available. + char emReArmMsg[23]; + tfp_sprintf(emReArmMsg, "** REARM PERIOD: "); + tfp_sprintf(emReArmMsg + strlen(emReArmMsg), "%02d", (uint8_t)MS2S(rearmMs)); + strcat(emReArmMsg, " **\0"); + displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(emReArmMsg)); } else if (notify_settings_saved > 0) { if (millis() > notify_settings_saved) { notify_settings_saved = 0; @@ -4973,9 +4982,10 @@ static void osdRefresh(timeUs_t currentTimeUs) } bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS); - static uint8_t statsCurrentPage = 0; - static bool statsDisplayed = false; - static bool statsAutoPagingEnabled = true; + static uint8_t statsCurrentPage = 0; + static timeMs_t statsRefreshTime = 0; + static bool statsDisplayed = false; + static bool statsAutoPagingEnabled = true; // Detect arm/disarm if (armState != ARMING_FLAG(ARMED)) { @@ -5043,25 +5053,25 @@ static void osdRefresh(timeUs_t currentTimeUs) // Alternate screens for multi-page stats. // Also, refreshes screen at swap interval for single-page stats. if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { - if (statsCurrentPage == 0) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); + if (statsCurrentPage == 0) statsCurrentPage = 1; - } } else { - if (statsCurrentPage == 1) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); + if (statsCurrentPage == 1) statsCurrentPage = 0; - } } } else { // Process manual page change events for multi-page stats. - if (manualPageUpRequested) { - osdShowStats(statsSinglePageCompatible, 1); - statsCurrentPage = 1; - } else if (manualPageDownRequested) { - osdShowStats(statsSinglePageCompatible, 0); + if (manualPageUpRequested) + statsCurrentPage = 1; + else if (manualPageDownRequested) statsCurrentPage = 0; } + + // Only refresh the stats every 1/4 of a second. + if (statsRefreshTime <= millis()) { + statsRefreshTime = millis() + 250; + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + } } } @@ -5254,7 +5264,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (ARMING_FLAG(ARMED)) { #ifdef USE_FW_AUTOLAND - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || FLIGHT_MODE(NAV_FW_AUTOLAND)) { if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) { #else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { @@ -5295,7 +5305,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter #ifdef USE_FW_AUTOLAND if (canFwLandCanceld()) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); - } else if (!isFwLandInProgess()) { + } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { #endif const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { @@ -5418,9 +5428,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } /* Messages that are shown regardless of Arming state */ + uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0; if (savingSettings == true) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS); + } else if (rearmMs > 0) { // Show rearming time if settings not actively being saved. Ignore the settings saved message if rearm available. + char emReArmMsg[23]; + tfp_sprintf(emReArmMsg, "** REARM PERIOD: "); + tfp_sprintf(emReArmMsg + strlen(emReArmMsg), "%02d", (uint8_t)MS2S(rearmMs)); + strcat(emReArmMsg, " **\0"); + messages[messageCount++] = OSD_MESSAGE_STR(emReArmMsg); } else if (notify_settings_saved > 0) { if (millis() > notify_settings_saved) { notify_settings_saved = 0; @@ -5429,6 +5446,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } + if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; if (message == failsafeInfoMessage) { diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 38db724c8e..78edf45370 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1063,6 +1063,10 @@ static bool djiFormatMessages(char *buff) if (FLIGHT_MODE(MANUAL_MODE)) { messages[messageCount++] = "(MANUAL)"; } + + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + messages[messageCount++] = "(LAND)"; + } } } // Pick one of the available messages. Each message lasts diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f9ddc1f255..4bc4cece47 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -65,6 +65,7 @@ #include "sensors/boardalignment.h" #include "sensors/battery.h" #include "sensors/gyro.h" +#include "sensors/diagnostics.h" #include "programming/global_variables.h" #include "sensors/rangefinder.h" @@ -1044,13 +1045,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, +/** Advanced Fixed Wing Autoland **/ #ifdef USE_FW_AUTOLAND [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1070,8 +1072,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1091,8 +1093,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_WP, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1113,7 +1115,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1134,7 +1136,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FLARE, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1147,8 +1149,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1677,7 +1679,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF /* If position sensors unavailable - land immediately (wait for timeout on GPS) * Continue to check for RTH sanity during landing */ - if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1687,7 +1689,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF #ifdef USE_FW_AUTOLAND if (STATE(AIRPLANE)) { - int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8; + int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1; #ifdef USE_MULTI_MISSION missionIdx = posControl.loadedMultiMissionIndex - 1; #endif @@ -1696,18 +1698,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF shIdx = posControl.safehomeState.index; missionFwLandConfigStartIdx = MAX_SAFE_HOMES; #endif - if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) { + if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) { + approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; + } else if (shIdx >= 0) { + approachSettingIdx = shIdx; + } + + if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) { if (previousState == NAV_STATE_WAYPOINT_REACHED) { posControl.fwLandState.landPos = posControl.activeWaypoint.pos; - posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; posControl.fwLandState.landWp = true; } else { posControl.fwLandState.landPos = posControl.safehomeState.nearestSafeHome; - posControl.fwLandState.approachSettingIdx = shIdx; posControl.fwLandState.landWp = false; } + posControl.fwLandState.approachSettingIdx = approachSettingIdx; posControl.fwLandState.landAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt; posControl.fwLandState.landAproachAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; @@ -1779,6 +1786,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL +#ifdef USE_FW_AUTOLAND + if (previousState != NAV_STATE_FW_LANDING_ABORT) { + posControl.fwLandState.landAborted = false; + } +#endif + if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) { /* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps Using p3 minimises the risk of saving an invalid counter if a mission is aborted */ @@ -1988,11 +2001,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig { UNUSED(previousState); +#ifdef USE_FW_AUTOLAND + if (posControl.fwLandState.landAborted) { + return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; + } +#endif + const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); +#ifdef USE_FW_AUTOLAND if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; - } else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { + } else +#endif + if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; } else if (landEvent == NAV_FSM_EVENT_SUCCESS) { // Landing controller returned success - invoke RTH finishing state and finish the waypoint @@ -2322,7 +2344,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav if (isLandingDetected()) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; - disarm(DISARM_LANDING); + //disarm(DISARM_LANDING); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -2366,14 +2388,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; } - if (getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) { + if (getHwRangefinderStatus() == HW_SENSOR_OK && getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE; return NAV_FSM_EVENT_SUCCESS; } if (isLandingDetected()) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; - disarm(DISARM_LANDING); + //disarm(DISARM_LANDING); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -2387,7 +2409,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga if (isLandingDetected()) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; - disarm(DISARM_LANDING); + //disarm(DISARM_LANDING); return NAV_FSM_EVENT_SUCCESS; } setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); @@ -2993,7 +3015,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla updateDesiredRTHAltitude(); // Reset RTH sanity checker for new home position if RTH active - if (FLIGHT_MODE(NAV_RTH_MODE)) { + if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) ) { initializeRTHSanityChecker(); } @@ -3115,7 +3137,7 @@ void updateHomePosition(void) static bool isHomeResetAllowed = false; // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { - if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { + if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { homeUpdateFlags = 0; homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setHome = true; @@ -3935,6 +3957,7 @@ bool isNavHoldPositionActive(void) // Also hold position during emergency landing if position valid return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE) || + (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || navigationIsExecutingAnEmergencyLanding(); } @@ -3974,7 +3997,9 @@ bool isWaypointNavTrackingActive(void) // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP. // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called) - return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex); + return FLIGHT_MODE(NAV_WP_MODE) + || posControl.navState == NAV_STATE_FW_LANDING_APPROACH + || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); } /*----------------------------------------------------------- @@ -4039,7 +4064,7 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.flags.verticalPositionDataConsumed = false; #ifdef USE_FW_AUTOLAND - if (!isFwLandInProgess()) { + if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; } #endif @@ -4080,7 +4105,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4192,22 +4217,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } posControl.rthSanityChecker.rthSanityOK = true; - /* WP mission activation control: - * canActivateWaypoint & waypointWasActivated are used to prevent WP mission - * auto restarting after interruption by Manual or RTH modes. - * WP mode must be deselected before it can be reactivated again. */ - static bool waypointWasActivated = false; - const bool isWpMissionLoaded = isWaypointMissionValid(); - bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner - - if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { - canActivateWaypoint = false; - if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { - canActivateWaypoint = true; - waypointWasActivated = false; - } - } - /* Airplane specific modes */ if (STATE(AIRPLANE)) { // LAUNCH mode has priority over any other NAV mode @@ -4247,14 +4256,36 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } - /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded. - * WP prevented from falling back to RTH if WP mission planner is active */ - const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; + /* WP mission activation control: + * canActivateWaypoint & waypointWasActivated are used to prevent WP mission + * auto restarting after interruption by Manual or RTH modes. + * WP mode must be deselected before it can be reactivated again + * WP Mode also inhibited when Mission Planner is active */ + static bool waypointWasActivated = false; + bool canActivateWaypoint = isWaypointMissionValid(); + bool wpRthFallbackIsActive = false; + + if (IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive) { + canActivateWaypoint = false; + } else { + if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { + canActivateWaypoint = false; + + if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { + canActivateWaypoint = true; + waypointWasActivated = false; + } + } + + wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint; + } + + /* Pilot-triggered RTH, also fall-back for WP if no mission is loaded. + * Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss + * Without this loss of any of the canActivateNavigation && canActivateAltHold + * will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back + * logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) */ if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { - // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - // Without this loss of any of the canActivateNavigation && canActivateAltHold - // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back - // logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; } @@ -4268,11 +4299,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded. // Also check multimission mission change status before activating WP mode. #ifdef USE_MULTI_MISSION - if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { + if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP)) { #else - if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { + if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { #endif - if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { + if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { waypointWasActivated = true; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; } @@ -4767,7 +4798,7 @@ void abortForcedRTH(void) rthState_e getStateOfForcedRTH(void) { /* If forced RTH activated and in AUTO_RTH or EMERG state */ - if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) { + if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) { if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { return RTH_HAS_LANDED; } @@ -4849,6 +4880,12 @@ bool navigationIsFlyingAutonomousMode(void) bool navigationRTHAllowsLanding(void) { +#ifdef USE_FW_AUTOLAND + if (posControl.fwLandState.landAborted) { + return false; + } +#endif + // WP mission RTH landing setting if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0; @@ -5006,15 +5043,6 @@ void resetFwAutolandApproach(int8_t idx) } } -bool isFwLandInProgess(void) -{ - return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER - || posControl.navState == NAV_STATE_FW_LANDING_LOITER - || posControl.navState == NAV_STATE_FW_LANDING_APPROACH - || posControl.navState == NAV_STATE_FW_LANDING_GLIDE - || posControl.navState == NAV_STATE_FW_LANDING_FLARE; -} - bool canFwLandCanceld(void) { return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 84e5390d43..3c2155ebc6 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -702,7 +702,6 @@ uint8_t getActiveWpNumber(void); int32_t navigationGetHomeHeading(void); #ifdef USE_FW_AUTOLAND -bool isFwLandInProgess(void); bool canFwLandCanceld(void); #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index db4b3229d5..e6209f460b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -271,7 +271,7 @@ static int8_t loiterDirection(void) { static void calculateVirtualPositionTarget_FW(float trackingPeriod) { - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { return; } @@ -405,7 +405,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta static bool forceTurnDirection = false; int32_t virtualTargetBearing; - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { virtualTargetBearing = posControl.desiredState.yaw; } else { // We have virtual position target, calculate heading error @@ -643,11 +643,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle); // Manual throttle increase -#ifdef USE_FW_AUTOLAND - if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) { -#else - if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { -#endif + if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); } else { @@ -665,7 +661,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // Advanced autoland if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) { // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled - rcCommand[THROTTLE] = getThrottleIdleValue(); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) { @@ -727,6 +722,7 @@ bool isFixedWingLandingDetected(void) // Basic condition to start looking for landing bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || FLIGHT_MODE(FAILSAFE_MODE) + || FLIGHT_MODE(NAV_FW_AUTOLAND) || (!navigationIsControllingThrottle() && throttleStickIsLow()); if (!startCondition || posControl.flags.resetLandingDetector) { diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index ad1cb21c6c..343db2989f 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -483,8 +483,9 @@ static int logicConditionCompute( } break; -#ifdef LED_PIN +#ifdef USE_LED_STRIP case LOGIC_CONDITION_LED_PIN_PWM: + if (operandA >=0 && operandA <= 100) { ledPinStartPWM((uint8_t)operandA); } else { @@ -768,7 +769,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 #ifdef USE_FW_AUTOLAND - return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0; + return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0; #else return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0; #endif diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/target.h b/src/main/target/IFLIGHT_BLITZ_ATF435/target.h index 7518bbe13e..b29112d525 100644 --- a/src/main/target/IFLIGHT_BLITZ_ATF435/target.h +++ b/src/main/target/IFLIGHT_BLITZ_ATF435/target.h @@ -69,6 +69,13 @@ #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PA4 +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + + // Other sensors #define USE_BARO diff --git a/src/main/target/IFLIGHT_BLITZ_F722_X1/CMakeLists.txt b/src/main/target/IFLIGHT_BLITZ_F722_X1/CMakeLists.txt new file mode 100644 index 0000000000..2d52bdb52e --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/CMakeLists.txt @@ -0,0 +1,3 @@ +target_stm32f722xe(IFLIGHT_BLITZ_F722_X1) +target_stm32f722xe(IFLIGHT_BLITZ_F722_X1_OSD) + diff --git a/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c b/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c new file mode 100644 index 0000000000..b8796a0d2f --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include "platform.h" + +#include "config/config_master.h" +#include "config/feature.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/IFLIGHT_BLITZ_F722_X1/target.c b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.c new file mode 100644 index 0000000000..5a20c88ff2 --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.c @@ -0,0 +1,37 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DMA1_S2_CH5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 DMA1_S7_CH5 + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 DMA2_S4_CH7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 DMA2_S7_CH7 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DMA1_S0_CH2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA1_S3_CH2 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // WS2812B +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h new file mode 100644 index 0000000000..0319625442 --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h @@ -0,0 +1,167 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "I7X1" +#define USBD_PRODUCT_STRING "IFLIGHT_BLITZ_F722_X1" + +#define LED0 PC15 +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG_FLIP +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_BUS BUS_SPI1 + +// ICM42605 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 + +#define PITOT_I2C_BUS BUS_I2C2 + +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// *************** SPI2 OSD*********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_OSD + +#ifdef IFLIGHT_BLITZ_F722_X1_OSD + #define USE_MAX7456 + #define MAX7456_SPI_BUS BUS_SPI2 + #define MAX7456_CS_PIN PB12 +#endif + +// *************** SPI3 Flash *********************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_FLASHFS + +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP +#define USB_DETECT_PIN PB2 +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) +#define CURRENT_METER_SCALE 200 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 7 +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC0 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED +#define PINIO2_PIN PC14 \ No newline at end of file diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index af9f278659..fd46100387 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -371,6 +371,10 @@ static void crsfFrameFlightMode(sbuf_t *dst) } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) { flightMode = "ANGH"; } +#ifdef USE_FW_AUTOLAND + } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + flightMode = "LAND"; +#endif #ifdef USE_GPS } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { flightMode = "WAIT"; // Waiting for GPS lock diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 84dccddb86..45ab37deec 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -71,6 +71,7 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" #include "sensors/pitotmeter.h" +#include "sensors/diagnostics.h" #include "telemetry/ltm.h" #include "telemetry/telemetry.h" @@ -182,6 +183,10 @@ void ltm_sframe(sbuf_t *dst) lt_flightmode = LTM_MODE_ANGLE; else if (FLIGHT_MODE(HORIZON_MODE)) lt_flightmode = LTM_MODE_HORIZON; +#ifdef USE_FW_AUTOLAND + else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) + lt_flightmode = LTM_MODE_LAND; +#endif else lt_flightmode = LTM_MODE_RATE; // Rate mode