diff --git a/.vscode/settings.json b/.vscode/settings.json index 9be3fced22b..ea2289e4321 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,6 +3,8 @@ "chrono": "c", "cmath": "c", "ranges": "c", + "navigation.h": "c", + "rth_trackback.h": "c" "platform.h": "c", "timer.h": "c", "bus.h": "c" diff --git a/cmake/arm-none-eabi-checks.cmake b/cmake/arm-none-eabi-checks.cmake index 497828088a1..200065fdd41 100644 --- a/cmake/arm-none-eabi-checks.cmake +++ b/cmake/arm-none-eabi-checks.cmake @@ -2,21 +2,18 @@ include(gcc) set(arm_none_eabi_triplet "arm-none-eabi") # Keep version in sync with the distribution files below -set(arm_none_eabi_gcc_version "10.3.1") -set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10") +set(arm_none_eabi_gcc_version "13.2.1") +# This is the output directory "pretty" name and URI name prefix +set(base_dir_name "arm-gnu-toolchain-13.2.rel1") +# This is the name inside the archive, which is no longer evincible from URI, alas +set(archive_base_dir_name "arm-gnu-toolchain-13.2.Rel1") +set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/${base_dir_name}") # suffix and checksum -set(arm_none_eabi_win32 "win32.zip" 2bc8f0c4c4659f8259c8176223eeafc1) -set(arm_none_eabi_linux_amd64 "x86_64-linux.tar.bz2" 2383e4eb4ea23f248d33adc70dc3227e) -set(arm_none_eabi_linux_aarch64 "aarch64-linux.tar.bz2" 3fe3d8bb693bd0a6e4615b6569443d0d) -set(arm_none_eabi_gcc_macos "mac.tar.bz2" 7f2a7b7b23797302a9d6182c6e482449) - -function(arm_none_eabi_gcc_distname var) - string(REPLACE "/" ";" url_parts ${arm_none_eabi_base_url}) - list(LENGTH url_parts n) - math(EXPR last "${n} - 1") - list(GET url_parts ${last} basename) - set(${var} ${basename} PARENT_SCOPE) -endfunction() +set(arm_none_eabi_win32 "mingw-w64-i686-arm-none-eabi.zip" 7fd677088038cdf82f33f149e2e943ee) +set(arm_none_eabi_linux_amd64 "x86_64-arm-none-eabi.tar.xz" 791754852f8c18ea04da7139f153a5b7) +set(arm_none_eabi_linux_aarch64 "aarch64-arm-none-eabi.tar.xz" 5a08122e6d4caf97c6ccd1d29e62599c) +set(arm_none_eabi_darwin_amd64 "darwin-x86_64-arm-none-eabi.tar.xz" 41d49840b0fc676d2ae35aab21a58693) +set(arm_none_eabi_darwin_aarch64 "darwin-arm64-arm-none-eabi.tar.xz" 2c43e9d72206c1f81227b0a685df5ea6) function(host_uname_machine var) # We need to call uname -m manually, since at the point @@ -47,7 +44,14 @@ function(arm_none_eabi_gcc_install) message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}") endif() elseif(CMAKE_HOST_SYSTEM_NAME STREQUAL "Darwin") - set(dist ${arm_none_eabi_gcc_macos}) + host_uname_machine(machine) + if(machine STREQUAL "x86_64" OR machine STREQUAL "amd64") + set(dist ${arm_none_eabi_darwin_amd64}) + elseif(machine STREQUAL "aarch64") + set(dist ${arm_none_eabi_darwin_aarch64}) + else() + message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}") + endif() endif() if(dist STREQUAL "") @@ -83,11 +87,27 @@ function(arm_none_eabi_gcc_install) if(NOT status EQUAL 0) message(FATAL_ERROR "error extracting ${basename}: ${status}") endif() + string(REPLACE "." ";" url_parts ${dist_suffix}) + list(GET url_parts 0 host_dir_name) + set(dir_name "${archive_base_dir_name}-${host_dir_name}") + file(REMOVE_RECURSE "${TOOLS_DIR}/${base_dir_name}") + file(RENAME "${TOOLS_DIR}/${dir_name}" "${TOOLS_DIR}/${base_dir_name}") + # This is **somewhat ugly** + # the newlib distributed by ARM generates suprious warnings from re-entrant POSIX functions + # that INAV doesn't use. These "harmless" warnings can be surpressed by removing the + # errant section from the only libnosys used by INAV ... + # So look the other way ... while this is "fixed" + execute_process(COMMAND arm-none-eabi-objcopy -w -R .gnu.warning.* "${TOOLS_DIR}/${base_dir_name}/arm-none-eabi/lib/thumb/v7e-m+fp/hard/libnosys.a" + RESULT_VARIABLE status + WORKING_DIRECTORY ${TOOLS_DIR} + ) + if(NOT status EQUAL 0) + message(FATAL_ERROR "error fixing libnosys.a: ${status}") + endif() endfunction() function(arm_none_eabi_gcc_add_path) - arm_none_eabi_gcc_distname(dist_name) - set(gcc_path "${TOOLS_DIR}/${dist_name}/bin") + set(gcc_path "${TOOLS_DIR}/${base_dir_name}/bin") if(CMAKE_HOST_SYSTEM MATCHES ".*Windows.*") set(sep "\\;") else() @@ -110,7 +130,7 @@ function(arm_none_eabi_gcc_check) message("-- found ${prog} ${version} at ${prog_path}") if(COMPILER_VERSION_CHECK AND NOT arm_none_eabi_gcc_version STREQUAL version) message("-- expecting ${prog} version ${arm_none_eabi_gcc_version}, but got version ${version} instead") - arm_none_eabi_gcc_install() + arm_none_eabi_gcc_install() return() endif() endfunction() diff --git a/cmake/at32.cmake b/cmake/at32.cmake index aa902593e94..54e178deb7b 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -9,7 +9,7 @@ option(SEMIHOSTING "Enable semihosting") message("-- DEBUG_HARDFAULTS: ${DEBUG_HARDFAULTS}, SEMIHOSTING: ${SEMIHOSTING}") set(CMSIS_DIR "${MAIN_LIB_DIR}/lib/main/AT32F43x/Drivers/CMSIS") -set(CMSIS_INCLUDE_DIR "${CMSIS_DIR}/cm4/core_support") +set(CMSIS_INCLUDE_DIR "${CMSIS_DIR}/cm4/core_support") # DSP use common set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP") set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include") @@ -50,8 +50,8 @@ main_sources(AT32_ASYNCFATFS_SRC ) main_sources(AT32_MSC_SRC - msc/at32_msc_diskio.c - msc/emfat.c + msc/at32_msc_diskio.c + msc/emfat.c msc/emfat_file.c ) @@ -92,6 +92,7 @@ set(AT32_LINK_OPTIONS -Wl,--cref -Wl,--no-wchar-size-warning -Wl,--print-memory-usage + -Wl,--no-warn-rwx-segments ) # Get target features macro(get_at32_target_features output_var dir target_name) @@ -264,7 +265,7 @@ function(add_at32_executable) endif() endfunction() -# Main function of AT32 +# Main function of AT32 function(target_at32) if(NOT arm-none-eabi STREQUAL TOOLCHAIN) return() diff --git a/docs/SITL/SITL.md b/docs/SITL/SITL.md index 8dca15e22f3..0c127135781 100644 --- a/docs/SITL/SITL.md +++ b/docs/SITL/SITL.md @@ -79,7 +79,7 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/) For SBUS, the command line arguments of the python script are: ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000``` -### Telemtry +### Telemetry LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP. diff --git a/docs/Settings.md b/docs/Settings.md index bc6dfef4053..5befa0aeffe 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2782,16 +2782,6 @@ Craft name --- -### nav_auto_climb_rate - -Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] - -| Default | Min | Max | -| --- | --- | --- | -| 500 | 10 | 2000 | - ---- - ### nav_auto_disarm_delay Delay before craft disarms when `nav_disarm_on_landing` is set (ms) @@ -2818,7 +2808,7 @@ Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on | Default | Min | Max | | --- | --- | --- | -| 20 | 0 | 120 | +| 60 | 0 | 120 | --- @@ -3112,6 +3102,16 @@ PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] --- +### nav_fw_manual_climb_rate + +Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 300 | 10 | 2500 | + +--- + ### nav_fw_max_thr Maximum throttle for flying wing in GPS assisted modes @@ -3382,16 +3382,6 @@ Allows immediate landing detection based on G bump at touchdown when set to ON. --- -### nav_manual_climb_rate - -Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] - -| Default | Min | Max | -| --- | --- | --- | -| 200 | 10 | 2000 | - ---- - ### nav_manual_speed Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] @@ -3442,6 +3432,16 @@ If set to STICK the FC remembers the throttle stick position when enabling ALTHO --- +### nav_mc_auto_climb_rate + +Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 10 | 2000 | + +--- + ### nav_mc_bank_angle Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude @@ -3552,6 +3552,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx --- +### nav_mc_manual_climb_rate + +Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 10 | 2000 | + +--- + ### nav_mc_pos_deceleration_time Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting diff --git a/docs/boards/SPEEDYBEEF405V3.md b/docs/boards/SPEEDYBEEF405V3.md index 7ec4b1ce944..fe51d3ce658 100644 --- a/docs/boards/SPEEDYBEEF405V3.md +++ b/docs/boards/SPEEDYBEEF405V3.md @@ -1,6 +1,6 @@ # SpeedyBee F405 V3 -> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead +> Notice: DSHOT on SpeedyBe F405 V3 requires INAV 7.0.0 or later. If you are using an older version, use MULTISHOT instead. > Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time. diff --git a/lib/main/MAVLink/common/common.h b/lib/main/MAVLink/common/common.h index 55200c5a84b..ec5fc7218d2 100755 --- a/lib/main/MAVLink/common/common.h +++ b/lib/main/MAVLink/common/common.h @@ -48,7 +48,7 @@ typedef enum FIRMWARE_VERSION_TYPE } FIRMWARE_VERSION_TYPE; #endif -/** @brief Flags to report failure cases over the high latency telemtry. */ +/** @brief Flags to report failure cases over the high latency telemetry. */ #ifndef HAVE_ENUM_HL_FAILURE_FLAG #define HAVE_ENUM_HL_FAILURE_FLAG typedef enum HL_FAILURE_FLAG diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 13ac4a92911..db4b1f57d1b 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -558,6 +558,8 @@ main_sources(COMMON_SRC navigation/navigation_rover_boat.c navigation/sqrt_controller.c navigation/sqrt_controller.h + navigation/rth_trackback.c + navigation/rth_trackback.h sensors/barometer.c sensors/barometer.h diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index a54843afc40..f5b12301028 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -45,8 +45,9 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] = OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED), OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED), OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED), - OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE), - OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE), + OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_MC_AUTO_CLIMB_RATE), + OSD_SETTING_ENTRY("MAX MC AH CLIMB RATE", SETTING_NAV_MC_MANUAL_CLIMB_RATE), + OSD_SETTING_ENTRY("MAX FW AH CLIMB RATE", SETTING_NAV_FW_MANUAL_CLIMB_RATE), OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE), OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE), OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR), diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 1daed3ef6aa..0f5b6d021a7 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -36,7 +36,7 @@ #define RAD (M_PIf / 180.0f) #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) -#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f) +#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f) #define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) #define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) @@ -54,11 +54,11 @@ #define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD) #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) -#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) +#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD) #define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) #define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) -#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) +#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f) #define METERS_TO_CENTIMETERS(m) (m * 100) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 94412e11279..be4d621549c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1310,9 +1310,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_NAV_POSHOLD: sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); sbufWriteU16(dst, navConfig()->general.max_auto_speed); - sbufWriteU16(dst, navConfig()->general.max_auto_climb_rate); + sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_speed); - sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); + sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.althold_throttle_type); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); @@ -2321,9 +2321,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize == 13) { navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.max_auto_speed = sbufReadU16(src); - navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src); + navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); navConfigMutable()->general.max_manual_speed = sbufReadU16(src); - navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); + if (mixerConfig()->platformType != PLATFORM_AIRPLANE) { + navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); + }else{ + navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src); + } navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src); currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src); diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h index b5c1e1d9d69..93265ba1200 100644 --- a/src/main/fc/multifunction.h +++ b/src/main/fc/multifunction.h @@ -24,6 +24,8 @@ #pragma once +#include + #ifdef USE_MULTI_FUNCTIONS extern uint8_t multiFunctionFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7483f145acb..3df7beebb1e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2511,24 +2511,12 @@ groups: field: general.max_auto_speed min: 10 max: 2000 - - name: nav_auto_climb_rate - description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" - default_value: 500 - field: general.max_auto_climb_rate - min: 10 - max: 2000 - name: nav_manual_speed description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" default_value: 500 field: general.max_manual_speed min: 10 max: 2000 - - name: nav_manual_climb_rate - description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" - default_value: 200 - field: general.max_manual_climb_rate - min: 10 - max: 2000 - name: nav_land_minalt_vspd description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]" default_value: 50 @@ -2695,7 +2683,7 @@ groups: type: bool - name: nav_cruise_yaw_rate description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]" - default_value: 20 + default_value: 60 field: general.cruise_yaw_rate min: 0 max: 120 @@ -2705,6 +2693,18 @@ groups: field: mc.max_bank_angle min: 15 max: 45 + - name: nav_mc_auto_climb_rate + description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" + default_value: 500 + field: mc.max_auto_climb_rate + min: 10 + max: 2000 + - name: nav_mc_manual_climb_rate + description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" + default_value: 200 + field: mc.max_manual_climb_rate + min: 10 + max: 2000 - name: nav_auto_disarm_delay description: "Delay before craft disarms when `nav_disarm_on_landing` is set (ms)" default_value: 1000 @@ -2790,6 +2790,12 @@ groups: field: fw.max_bank_angle min: 5 max: 80 + - name: nav_fw_manual_climb_rate + description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" + default_value: 300 + field: fw.max_manual_climb_rate + min: 10 + max: 2500 - name: nav_fw_climb_angle description: "Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit" default_value: 20 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3ed2ac2b3bb..22e11250963 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3742,7 +3742,7 @@ void osdDrawNextElement(void) elementIndex = osdIncElementIndex(elementIndex); } while (!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon + tracking telemtry last + // Draw artificial horizon + tracking telemetry last osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); if (osdConfig()->telemetry>0){ osdDisplayTelemetry(); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9ab373b833c..fc516e97708 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -55,6 +55,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/rth_trackback.h" #include "rx/rx.h" @@ -134,9 +135,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes - .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, - .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s @@ -162,6 +161,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // MC-specific .mc = { .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s + .max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT, #ifdef USE_MR_BRAKING_MODE .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s @@ -183,6 +184,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // Fixed wing .fw = { .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s @@ -258,10 +260,8 @@ static void resetJumpCounter(void); static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); -static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos); void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); -static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); @@ -273,11 +273,6 @@ bool validateRTHSanityChecker(void); void updateHomePosition(void); bool abortLaunchAllowed(void); -static bool rthAltControlStickOverrideCheck(unsigned axis); - -static void updateRthTrackback(bool forceSaveTrackPoint); -static fpVector3_t * rthGetTrackbackPos(void); - /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState); @@ -1257,16 +1252,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING - } - else { - // Switch to RTH trackback - bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || - (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); - - if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) { - updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference + } else { + if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { + rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference posControl.flags.rthTrackbackActive = true; - calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos()); + calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK; } @@ -1381,36 +1371,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (posControl.flags.estPosStatus >= EST_USABLE) { - const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100; -#ifdef USE_MULTI_FUNCTIONS - const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK); -#else - const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL); -#endif - const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || - (overrideTrackback && !posControl.flags.forcedRTHActivated); - - if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) { - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1; - posControl.flags.rthTrackbackActive = false; - return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point - } - - if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { - posControl.activeRthTBPointIndex--; - - if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) { - posControl.activeRthTBPointIndex = NAV_RTH_TRACKBACK_POINTS - 1; - } - calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos()); - - if (posControl.activeRthTBPointIndex - posControl.rthTBWrapAroundCounter == 0) { - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1; - } - } else { - setDesiredPosition(rthGetTrackbackPos(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); - } + if (rthTrackBackSetNewPosition()) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; } return NAV_FSM_EVENT_NONE; @@ -2408,7 +2370,7 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) * Check if waypoint is/was reached. * waypointBearing stores initial bearing to waypoint *-----------------------------------------------------------*/ -static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing) +bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing) { posControl.wpDistance = calculateDistanceToDestination(waypointPos); @@ -2768,12 +2730,13 @@ void updateHomePosition(void) * Climb First override limited to Fixed Wing only * Roll also cancels RTH trackback on Fixed Wing and Multirotor *-----------------------------------------------------------*/ -static bool rthAltControlStickOverrideCheck(unsigned axis) +bool rthAltControlStickOverrideCheck(uint8_t axis) { if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) { return false; } + static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { @@ -2812,110 +2775,6 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) return false; } -/* -------------------------------------------------------------------------------- - * == RTH Trackback == - * Saves track during flight which is used during RTH to back track - * along arrival route rather than immediately heading directly toward home. - * Max desired trackback distance set by user or limited by number of available points. - * Reverts to normal RTH heading direct to home when end of track reached. - * Trackpoints logged with precedence for course/altitude changes. Distance based changes - * only logged if no course/altitude changes logged over an extended distance. - * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold). - * --------------------------------------------------------------------------------- */ - static void updateRthTrackback(bool forceSaveTrackPoint) -{ - static bool suspendTracking = false; - bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE)); - if (!fwLoiterIsActive && suspendTracking) { - suspendTracking = false; - } - - if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) { - return; - } - - // Record trackback points based on significant change in course/altitude until - // points limit reached. Overwrite older points from then on. - if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) { - static int32_t previousTBTripDist; // cm - static int16_t previousTBCourse; // degrees - static int16_t previousTBAltitude; // meters - static uint8_t distanceCounter = 0; - bool saveTrackpoint = forceSaveTrackPoint; - bool GPSCourseIsValid = isGPSHeadingValid(); - - // start recording when some distance from home, 50m seems reasonable. - if (posControl.activeRthTBPointIndex < 0) { - saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50); - - previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog); - previousTBTripDist = posControl.totalTripDistance; - } else { - // Minimum distance increment between course change track points when GPS course valid - set to 10m - const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(10); - - // Altitude change - if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > 10) { // meters - saveTrackpoint = true; - } else if (distanceIncrement && GPSCourseIsValid) { - // Course change - set to 45 degrees - if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) { - saveTrackpoint = true; - } else if (distanceCounter >= 9) { - // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change - // and deviation from projected course path > 20m - float distToPrevPoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]); - - fpVector3_t virtualCoursePoint; - virtualCoursePoint.x = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse)); - virtualCoursePoint.y = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse)); - - saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(20); - } - distanceCounter++; - previousTBTripDist = posControl.totalTripDistance; - } else if (!GPSCourseIsValid) { - // if no reliable course revert to basic distance logging based on direct distance from last point - set to 20m - saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20); - previousTBTripDist = posControl.totalTripDistance; - } - - // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter. - if (distanceCounter && fwLoiterIsActive) { - saveTrackpoint = suspendTracking = true; - } - } - - // when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position - if (saveTrackpoint) { - if (posControl.activeRthTBPointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // wraparound to start - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = 0; - } else { - posControl.activeRthTBPointIndex++; - if (posControl.rthTBWrapAroundCounter > -1) { // track wraparound overwrite position after store first filled - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex; - } - } - posControl.rthTBPointsList[posControl.activeRthTBPointIndex] = posControl.actualState.abs.pos; - - posControl.rthTBLastSavedIndex = posControl.activeRthTBPointIndex; - previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z); - previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse; - distanceCounter = 0; - } - } -} - -static fpVector3_t * rthGetTrackbackPos(void) -{ - // ensure trackback altitude never lower than altitude of start point - if (posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z < posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z) { - posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z = posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z; - } - - return &posControl.rthTBPointsList[posControl.activeRthTBPointIndex]; -} - /*----------------------------------------------------------- * Update flight statistics *-----------------------------------------------------------*/ @@ -3589,7 +3448,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv); } -static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) +void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos) { // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints) if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) { @@ -3682,7 +3541,7 @@ 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.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex); } /*----------------------------------------------------------- @@ -3739,9 +3598,7 @@ void applyWaypointNavigationAndAltitudeHold(void) // ensure WP missions always restart from first waypoint after disarm posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback - posControl.activeRthTBPointIndex = -1; - posControl.flags.rthTrackbackActive = false; - posControl.rthTBWrapAroundCounter = -1; + resetRthTrackBack(); return; } @@ -4273,7 +4130,7 @@ void updateWaypointsAndNavigationMode(void) updateWpMissionPlanner(); // Update RTH trackback - updateRthTrackback(false); + rthTrackBackUpdate(false); //Update Blackbox data navCurrentState = (int16_t)posControl.navPersistentId; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5b9dda12ab7..7a2ff1869cc 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -250,9 +250,7 @@ typedef struct navConfig_s { uint16_t auto_speed; // autonomous navigation speed cm/sec uint8_t min_ground_speed; // Minimum navigation ground speed [m/s] uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec - uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed - uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend @@ -277,6 +275,8 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // multicopter max banking angle (deg) + uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec + uint16_t max_manual_climb_rate; // manual velocity control max vertical speed #ifdef USE_MR_BRAKING_MODE uint16_t braking_speed_threshold; // above this speed braking routine might kick in @@ -297,6 +297,7 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // Fixed wing max banking angle (deg) + uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH @@ -579,6 +580,9 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units // Select absolute or relative altitude based on WP mission flag setting geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag); +void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos); +bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing); + /* Distance/bearing calculation */ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); // NOT USED uint32_t distanceToFirstWP(void); @@ -631,6 +635,7 @@ bool isEstimatedAglTrusted(void); void checkManualEmergencyLandingControl(bool forcedActivation); float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue); +bool rthAltControlStickOverrideCheck(uint8_t axis); int8_t navCheckActiveAngleHoldAxis(void); uint8_t getActiveWpNumber(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 37971c6a5b3..7b81f473d79 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -115,7 +115,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) if (rcAdjustment) { // set velocity proportional to stick movement - float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); + float rcClimbRate = -rcAdjustment * navConfig()->fw.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2264d842014..02e3edbb910 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -77,9 +77,9 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) float vel_max_z = 0.0f; if (posControl.flags.isAdjustingAltitude) { - vel_max_z = navConfig()->general.max_manual_climb_rate; + vel_max_z = navConfig()->mc.max_manual_climb_rate; } else { - vel_max_z = navConfig()->general.max_auto_climb_rate; + vel_max_z = navConfig()->mc.max_auto_climb_rate; } targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); @@ -151,11 +151,11 @@ bool adjustMulticopterAltitudeFromRCInput(void) // Make sure we can satisfy max_manual_climb_rate in both up and down directions if (rcThrottleAdjustment > 0) { // Scaling from altHoldThrottleRCZero to maxthrottle - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); } else { // Scaling from minthrottle to altHoldThrottleRCZero - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); } updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); @@ -221,11 +221,11 @@ void resetMulticopterAltitudeController(void) const float maxSpeed = getActiveSpeed(); nav_speed_up = maxSpeed; nav_accel_z = maxSpeed; - nav_speed_down = navConfig()->general.max_auto_climb_rate; + nav_speed_down = navConfig()->mc.max_auto_climb_rate; } else { nav_speed_up = navConfig()->general.max_manual_speed; nav_accel_z = navConfig()->general.max_manual_speed; - nav_speed_down = navConfig()->general.max_manual_climb_rate; + nav_speed_down = navConfig()->mc.max_manual_climb_rate; } sqrtControllerInit( @@ -252,8 +252,8 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) if (prepareForTakeoffOnReset) { const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); - posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate; - posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); + posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate; + posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->mc.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); posControl.pids.vel[Z].integrator = -500.0f; pt1FilterReset(&altholdThrottleFilterState, -500.0f); prepareForTakeoffOnReset = false; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2bd72d7ae13..4c919d84de2 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -45,8 +45,6 @@ #define MC_LAND_DESCEND_THROTTLE 40 // RC pwm units (us) #define MC_LAND_SAFE_SURFACE 5.0f // cm -#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points - #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); @@ -428,12 +426,6 @@ typedef struct { timeMs_t wpReachedTime; // Time the waypoint was reached bool wpAltitudeReached; // WP altitude achieved - /* RTH Trackback */ - fpVector3_t rthTBPointsList[NAV_RTH_TRACKBACK_POINTS]; - int8_t rthTBLastSavedIndex; // last trackback point index saved - int8_t activeRthTBPointIndex; - int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position - /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c new file mode 100644 index 00000000000..37467d9b385 --- /dev/null +++ b/src/main/navigation/rth_trackback.c @@ -0,0 +1,182 @@ +/* + * 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 . + */ + +/* -------------------------------------------------------------------------------- + * == RTH Trackback == + * Saves track during flight which is used during RTH to back track + * along arrival route rather than immediately heading directly toward home. + * Max desired trackback distance set by user or limited by number of available points. + * Reverts to normal RTH heading direct to home when end of track reached. + * Trackpoints logged with precedence for course/altitude changes. Distance based changes + * only logged if no course/altitude changes logged over an extended distance. + * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold). + * --------------------------------------------------------------------------------- */ + +#include "platform.h" + +#include "fc/multifunction.h" +#include "fc/rc_controls.h" + +#include "navigation/rth_trackback.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +rth_trackback_t rth_trackback; + +bool rthTrackBackIsActive(void) +{ + return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); +} + +void rthTrackBackUpdate(bool forceSaveTrackPoint) +{ + static bool suspendTracking = false; + bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE)); + + if (!fwLoiterIsActive && suspendTracking) { + suspendTracking = false; + } + + if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) { + return; + } + + // Record trackback points based on significant change in course/altitude until points limit reached. Overwrite older points from then on. + if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) { + static int32_t previousTBTripDist; // cm + static int16_t previousTBCourse; // degrees + static int16_t previousTBAltitude; // meters + static uint8_t distanceCounter = 0; + bool saveTrackpoint = forceSaveTrackPoint; + bool GPSCourseIsValid = isGPSHeadingValid(); + + // Start recording when some distance from home + if (rth_trackback.activePointIndex < 0) { + saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_DIST_TO_START); + previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog); + previousTBTripDist = posControl.totalTripDistance; + } else { + // Minimum distance increment between course change track points when GPS course valid + const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE); + + // Altitude change + if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE) { + saveTrackpoint = true; + } else if (distanceIncrement && GPSCourseIsValid) { + // Course change - set to 45 degrees + if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) { + saveTrackpoint = true; + } else if (distanceCounter >= 9) { + // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change and deviation from projected course path > 20m + float distToPrevPoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]); + + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = rth_trackback.pointsList[rth_trackback.activePointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse)); + virtualCoursePoint.y = rth_trackback.pointsList[rth_trackback.activePointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse)); + + saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE); + } + distanceCounter++; + previousTBTripDist = posControl.totalTripDistance; + } else if (!GPSCourseIsValid) { + // If no reliable course revert to basic distance logging based on direct distance from last point + saveTrackpoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE); + previousTBTripDist = posControl.totalTripDistance; + } + + // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter. + if (distanceCounter && fwLoiterIsActive) { + saveTrackpoint = suspendTracking = true; + } + } + + // When trackpoint store full, overwrite from start of store using 'WrapAroundCounter' to track overwrite position + if (saveTrackpoint) { + if (rth_trackback.activePointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // Wraparound to start + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = 0; + } else { + rth_trackback.activePointIndex++; + if (rth_trackback.WrapAroundCounter > -1) { // Track wraparound overwrite position after store first filled + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex; + } + } + + rth_trackback.pointsList[rth_trackback.activePointIndex] = posControl.actualState.abs.pos; + rth_trackback.lastSavedIndex = rth_trackback.activePointIndex; + previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z); + previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse; + distanceCounter = 0; + } + } +} + +bool rthTrackBackSetNewPosition(void) +{ + if (posControl.flags.estPosStatus == EST_NONE) { + return false; + } + + const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex])); + +#ifdef USE_MULTI_FUNCTIONS + const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK); +#else + const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL); +#endif + const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || (overrideTrackback && !posControl.flags.forcedRTHActivated); + + if (rth_trackback.activePointIndex < 0 || cancelTrackback) { + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; + posControl.flags.rthTrackbackActive = false; + return true; // Procede to home after final trackback point + } + + if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { + rth_trackback.activePointIndex--; + + if (rth_trackback.WrapAroundCounter > -1 && rth_trackback.activePointIndex < 0) { + rth_trackback.activePointIndex = NAV_RTH_TRACKBACK_POINTS - 1; + } + + calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); + + if (rth_trackback.activePointIndex - rth_trackback.WrapAroundCounter == 0) { + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; + } + } else { + setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + return false; +} + +fpVector3_t *getRthTrackBackPosition(void) +{ + // Ensure trackback altitude never lower than altitude of start point + if (rth_trackback.pointsList[rth_trackback.activePointIndex].z < rth_trackback.pointsList[rth_trackback.lastSavedIndex].z) { + rth_trackback.pointsList[rth_trackback.activePointIndex].z = rth_trackback.pointsList[rth_trackback.lastSavedIndex].z; + } + + return &rth_trackback.pointsList[rth_trackback.activePointIndex]; +} + +void resetRthTrackBack(void) +{ + rth_trackback.activePointIndex = -1; + posControl.flags.rthTrackbackActive = false; + rth_trackback.WrapAroundCounter = -1; +} \ No newline at end of file diff --git a/src/main/navigation/rth_trackback.h b/src/main/navigation/rth_trackback.h new file mode 100644 index 00000000000..e3c71d0d45e --- /dev/null +++ b/src/main/navigation/rth_trackback.h @@ -0,0 +1,42 @@ +/* + * 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 + +#include "common/vector.h" + +#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points +#define NAV_RTH_TRACKBACK_MIN_DIST_TO_START 50 // start recording when some distance from home (meters) +#define NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE 20 // minimum XY distance between two points to store in the buffer (meters) +#define NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE 10 // minimum Z distance between two points to store in the buffer (meters) +#define NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE 10 // minimum trip distance between two points to store in the buffer (meters) + +typedef struct +{ + fpVector3_t pointsList[NAV_RTH_TRACKBACK_POINTS]; // buffer of points stored + int16_t lastSavedIndex; // last trackback point index saved + int16_t activePointIndex; // trackback points counter + int16_t WrapAroundCounter; // stores trackpoint array overwrite index position +} rth_trackback_t; + +extern rth_trackback_t rth_trackback; + +bool rthTrackBackIsActive(void); +bool rthTrackBackSetNewPosition(void); +void rthTrackBackUpdate(bool forceSaveTrackPoint); +fpVector3_t *getRthTrackBackPosition(void); +void resetRthTrackBack(void); \ No newline at end of file diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 7d8a67081eb..b31b4e305d8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -287,6 +287,13 @@ static int logicConditionCompute( return true; break; +#ifdef USE_MAG + case LOGIC_CONDITION_RESET_MAG_CALIBRATION: + + ENABLE_STATE(CALIBRATE_MAG); + return true; + break; +#endif case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: #if defined(USE_VTX_CONTROL) #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index b46aeb44340..29cf8353af2 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -83,7 +83,8 @@ typedef enum { LOGIC_CONDITION_APPROX_EQUAL = 51, LOGIC_CONDITION_LED_PIN_PWM = 52, LOGIC_CONDITION_DISABLE_GPS_FIX = 53, - LOGIC_CONDITION_LAST = 54, + LOGIC_CONDITION_RESET_MAG_CALIBRATION = 54, + LOGIC_CONDITION_LAST = 55, } logicOperation_e; typedef enum logicOperandType_s { diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 23d45ed7879..9c7ba99d9f9 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -485,4 +485,5 @@ void compassUpdate(timeUs_t currentTimeUs) magUpdatedAtLeastOnce = true; } + #endif diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 13b9fb54255..baf7bed0a61 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -275,15 +275,15 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = 0.0f; } -#ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; - } -#endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); } +#endif +#ifdef USE_SIMULATOR + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + pitot.airSpeed = simulatorData.airSpeed; + } #endif }