mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-13 19:40:27 +03:00
Merge branch 'release_7.1.0' into master_7.1.0
This commit is contained in:
Pawel Spychalski (DzikuVx)
commit
ae5869c0bc
19 changed files with 413 additions and 97 deletions
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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++;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <math.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#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;
|
||||
|
|
@ -4974,6 +4983,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
|
|||
|
||||
bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS);
|
||||
static uint8_t statsCurrentPage = 0;
|
||||
static timeMs_t statsRefreshTime = 0;
|
||||
static bool statsDisplayed = false;
|
||||
static bool statsAutoPagingEnabled = true;
|
||||
|
||||
|
|
@ -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);
|
||||
if (manualPageUpRequested)
|
||||
statsCurrentPage = 1;
|
||||
} else if (manualPageDownRequested) {
|
||||
osdShowStats(statsSinglePageCompatible, 0);
|
||||
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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -702,7 +702,6 @@ uint8_t getActiveWpNumber(void);
|
|||
int32_t navigationGetHomeHeading(void);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
bool isFwLandInProgess(void);
|
||||
bool canFwLandCanceld(void);
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
3
src/main/target/IFLIGHT_BLITZ_F722_X1/CMakeLists.txt
Normal file
3
src/main/target/IFLIGHT_BLITZ_F722_X1/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
target_stm32f722xe(IFLIGHT_BLITZ_F722_X1)
|
||||
target_stm32f722xe(IFLIGHT_BLITZ_F722_X1_OSD)
|
||||
|
||||
29
src/main/target/IFLIGHT_BLITZ_F722_X1/config.c
Normal file
29
src/main/target/IFLIGHT_BLITZ_F722_X1/config.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#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;
|
||||
}
|
||||
37
src/main/target/IFLIGHT_BLITZ_F722_X1/target.c
Normal file
37
src/main/target/IFLIGHT_BLITZ_F722_X1/target.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#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]);
|
||||
167
src/main/target/IFLIGHT_BLITZ_F722_X1/target.h
Normal file
167
src/main/target/IFLIGHT_BLITZ_F722_X1/target.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue