From ae8192a59922eda2e629c4b1941c5ffbea0ff24e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 2 Mar 2024 18:56:25 +0100 Subject: [PATCH 01/11] initial commit --- .../IFLIGHT_BLITZ_F722_X1/CMakeLists.txt | 3 + .../target/IFLIGHT_BLITZ_F722_X1/config.c | 27 +++ .../target/IFLIGHT_BLITZ_F722_X1/target.c | 37 ++++ .../target/IFLIGHT_BLITZ_F722_X1/target.h | 161 ++++++++++++++++++ 4 files changed, 228 insertions(+) create mode 100644 src/main/target/IFLIGHT_BLITZ_F722_X1/CMakeLists.txt create mode 100644 src/main/target/IFLIGHT_BLITZ_F722_X1/config.c create mode 100644 src/main/target/IFLIGHT_BLITZ_F722_X1/target.c create mode 100644 src/main/target/IFLIGHT_BLITZ_F722_X1/target.h 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..77d259bbe9 --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c @@ -0,0 +1,27 @@ +/* + * 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" + +void targetConfiguration(void) +{ + +} 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..b5ddefbe48 --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h @@ -0,0 +1,161 @@ +/* + * 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 \ No newline at end of file From ec4a1962b3ec510442e570b893394a060d022fc2 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 28 Feb 2024 20:24:54 +0100 Subject: [PATCH 02/11] fixed led_pin_pwm programming framework functionality --- src/main/programming/logic_condition.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 1fa6cb0c7b..8fcca4dabc 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -476,8 +476,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 { From 9a942a5e9735e5a00f3e12d7a28248623c305e00 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 3 Mar 2024 21:00:21 +0000 Subject: [PATCH 03/11] Update navigation.c --- src/main/navigation/navigation.c | 58 ++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 26 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a4a0224e7d..46090ff24d 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4322,22 +4322,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 @@ -4377,14 +4361,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; } @@ -4398,11 +4404,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; } From e852b74fbdac0f4a035f43c9cdb697e3f4fa9d19 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 5 Mar 2024 12:30:50 +0000 Subject: [PATCH 04/11] Show a timeout for in flight rearming If possible, this will show a countdown time for how long the pilot has to rearm in flight. This was a part of #9688, which wasn't working as expected. This part should work, and give the pilots useful information. Currently not tested. Will test in HITL, then in flight asap. --- src/main/fc/fc_core.c | 16 ++++++++++++-- src/main/fc/fc_core.h | 2 ++ src/main/io/osd.c | 49 ++++++++++++++++++++++++++++++------------- 3 files changed, 51 insertions(+), 16 deletions(-) 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/io/osd.c b/src/main/io/osd.c index 7e67faef01..0fccaffc5e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -28,6 +28,7 @@ #include #include #include +#include #include "platform.h" @@ -4560,6 +4561,17 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } + if (emergInflightRearmEnabled()) { + uint16_t rearmMs = emergencyInFlightRearmTimeMS(); + if (rearmMs > 0) { + 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)); + } + } + if (savingSettings == true) { displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); } else if (notify_settings_saved > 0) { @@ -4861,9 +4873,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)) { @@ -4931,25 +4944,24 @@ 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); } } @@ -5317,6 +5329,15 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } + uint16_t rearmMs = emergencyInFlightRearmTimeMS(); + if (rearmMs > 0) { + 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); + } + if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; if (message == failsafeInfoMessage) { From 792983868ffb349c85f0e54084c25ce08874ca49 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 6 Mar 2024 07:33:52 +0000 Subject: [PATCH 05/11] Only show rearm message when not saving If the save is happening, writing to EEPROM locks up the FC. This change will only show the rearm message if the FC is not currently saving. If rearm is available, the `** SETTINGS SAVED**` message will not appear, to maximise time for rearm. If rearm is not available. The save messages display as normal. --- src/main/io/osd.c | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0fccaffc5e..d8214474bf 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4561,19 +4561,16 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } - if (emergInflightRearmEnabled()) { - uint16_t rearmMs = emergencyInFlightRearmTimeMS(); - if (rearmMs > 0) { - 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)); - } - } + 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; @@ -5318,9 +5315,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; @@ -5329,14 +5333,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } - uint16_t rearmMs = emergencyInFlightRearmTimeMS(); - if (rearmMs > 0) { - 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); - } if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; From 230095b473f089a40821fa5c3c9b25e251d3f3ba Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 6 Mar 2024 09:11:47 +0100 Subject: [PATCH 06/11] Add PINIO support for IFLIGHT_BLITZ_F722_X1 --- src/main/target/IFLIGHT_BLITZ_F722_X1/config.c | 4 +++- src/main/target/IFLIGHT_BLITZ_F722_X1/target.h | 8 +++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c b/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c index 77d259bbe9..b8796a0d2f 100644 --- a/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/config.c @@ -20,8 +20,10 @@ #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.h b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h index b5ddefbe48..0319625442 100644 --- a/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h +++ b/src/main/target/IFLIGHT_BLITZ_F722_X1/target.h @@ -158,4 +158,10 @@ #define MAX_PWM_OUTPUT_PORTS 7 #define USE_DSHOT #define USE_SERIALSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#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 From a287e88b687a64592b50291d9ecad63c8f7fa824 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 7 Mar 2024 11:19:16 +0100 Subject: [PATCH 07/11] Add temp units to DJI mapping --- src/main/io/displayport_msp_bf_compat.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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; /* From bc45aa74ff898b2dabe12d9bd824c9bad907f8cb Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Mon, 11 Mar 2024 11:28:51 +0000 Subject: [PATCH 08/11] fix cygwin/sitl build with cmake 3.28.3 (#9787) (#9788) --- cmake/sitl.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 78697f98a9..ee43aa9a93 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -34,7 +34,7 @@ set(SITL_LINK_OPTIONS -Wl,-L${STM32_LINKER_DIR} ) -if(${WIN32} OR ${CYGWIN}) +if(${CYGWIN}) set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-static-libgcc") endif() @@ -131,7 +131,7 @@ function (target_sitl name) target_link_options(${exe_target} PRIVATE -T${script_path}) endif() - if(${WIN32} OR ${CYGWIN}) + if(${CYGWIN}) set(exe_filename ${CMAKE_BINARY_DIR}/${binary_name}.exe) else() set(exe_filename ${CMAKE_BINARY_DIR}/${binary_name}) From 2fa82a33e794febb575be8981e08885bacb9ec0a Mon Sep 17 00:00:00 2001 From: Scavanger Date: Wed, 13 Mar 2024 17:01:49 -0300 Subject: [PATCH 09/11] 7.1-RC1-Autoland-Fix --- src/main/fc/runtime_config.h | 1 + src/main/flight/failsafe.c | 4 + src/main/flight/pid.c | 2 +- src/main/io/osd.c | 8 +- src/main/io/osd_dji_hd.c | 4 + src/main/navigation/navigation.c | 94 +++++++++++++--------- src/main/navigation/navigation.h | 1 - src/main/navigation/navigation_fixedwing.c | 12 +-- src/main/programming/logic_condition.c | 2 +- src/main/telemetry/crsf.c | 5 ++ src/main/telemetry/ltm.c | 5 ++ 11 files changed, 87 insertions(+), 51 deletions(-) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index ed5caf4954..8772c090e0 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 36f4d86f8e..9f60dd83a3 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -239,6 +239,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/osd.c b/src/main/io/osd.c index d8214474bf..201f3efb7b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -986,7 +986,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); } @@ -2278,7 +2278,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 @@ -5151,7 +5151,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()) { @@ -5192,7 +5192,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) { diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index c040b7b762..8edf8288d0 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1059,6 +1059,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 46090ff24d..9e81024c3a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -64,6 +64,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" @@ -1045,13 +1046,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 = { @@ -1071,8 +1073,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 = { @@ -1092,8 +1094,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 = { @@ -1114,7 +1116,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 = { @@ -1135,7 +1137,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 = { @@ -1148,8 +1150,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 = { @@ -1711,7 +1713,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; } @@ -1721,7 +1723,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 @@ -1730,18 +1732,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; @@ -1813,6 +1820,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 */ @@ -2022,11 +2035,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 @@ -2356,7 +2378,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; } @@ -2400,14 +2422,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; } @@ -2421,7 +2443,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); @@ -3016,7 +3038,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(); } @@ -3138,7 +3160,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; @@ -3243,7 +3265,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) suspendTracking = false; } - if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) { + if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) || !ARMING_FLAG(ARMED) || suspendTracking) { return; } @@ -4061,6 +4083,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(); } @@ -4100,7 +4123,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 && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); + return FLIGHT_MODE(NAV_WP_MODE) + || posControl.navState == NAV_STATE_FW_LANDING_APPROACH + || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); } /*----------------------------------------------------------- @@ -4169,7 +4194,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 @@ -4210,7 +4235,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); } @@ -4899,7 +4924,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; } @@ -4981,6 +5006,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; @@ -5138,15 +5169,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 884f172638..6ebdaeccea 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -693,7 +693,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 da5d5f70c0..2dfebdf8d9 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 8fcca4dabc..2b10896b71 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -746,7 +746,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/telemetry/crsf.c b/src/main/telemetry/crsf.c index af9f278659..aee7e5d303 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -371,6 +371,11 @@ 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 00e99a8047..c8cefd8725 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" @@ -178,6 +179,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 From 56f9a7a000cf79312fdec2c1dfc2a8a17dd77916 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Wed, 13 Mar 2024 17:11:03 -0300 Subject: [PATCH 10/11] Add forgotten ; and add a } --- src/main/telemetry/crsf.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index aee7e5d303..fd46100387 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -373,8 +373,7 @@ static void crsfFrameFlightMode(sbuf_t *dst) } #ifdef USE_FW_AUTOLAND } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { - flightMode = "LAND" - } + flightMode = "LAND"; #endif #ifdef USE_GPS } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { From 7fda8c62fdb60cdb2db878c6cfcfdaa4c281cb34 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 15 Mar 2024 09:37:52 +0100 Subject: [PATCH 11/11] Add ICM42688P driver to the target --- src/main/target/IFLIGHT_BLITZ_ATF435/target.h | 7 +++++++ 1 file changed, 7 insertions(+) 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