mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Merge branch 'iNavFlight:master' into master
This commit is contained in:
Julio Cesar Matias
GitHub
commit
f30edb083b
32 changed files with 431 additions and 262 deletions
2
.vscode/settings.json
vendored
2
.vscode/settings.json
vendored
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -35,11 +35,11 @@ To navigate without GPS fix, we make the following assumptions:
|
|||
|
||||
It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available).
|
||||
|
||||
From estimated heading direction and speed, plane is able to **roughty** estimate it's position.
|
||||
From estimated heading direction and speed, plane is able to **roughly** estimate it's position.
|
||||
|
||||
It is assumed, that plane will fly in roughly estimated direction to home position untill either GPS fix or RC signal is recovered.
|
||||
|
||||
*Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*.
|
||||
*Plane has to acquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*.
|
||||
|
||||
# Estimation without magnethometer
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
BIN
docs/assets/images/KAKUTEH7WING-wiring-diagram.webp
Normal file
BIN
docs/assets/images/KAKUTEH7WING-wiring-diagram.webp
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 286 KiB |
10
docs/boards/KAKUTEH7WING.md
Normal file
10
docs/boards/KAKUTEH7WING.md
Normal file
|
|
@ -0,0 +1,10 @@
|
|||
# Board - [KAKUTEH7WING](https://holybro.com/products/kakute-h743-wing)
|
||||
|
||||
[manufacturer manual](https://cdn.shopify.com/s/files/1/0604/5905/7341/files/Holybro_KakuteH743-Wing_Manual_v1.0_C.pdf?v=1693393206)
|
||||
|
||||
Note that this board has two i2c plugs.
|
||||
The six-pin plug should be used for GPS and compass.
|
||||
The 4-pin plug should be used for other i2c sesors such as i2c pitot (airspeed sensor), rangefinder, and external
|
||||
temperature sensors.
|
||||
|
||||
|
||||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -100,7 +100,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa
|
|||
|
||||
* [msp-tool](https://github.com/fiam/msp-tool)
|
||||
* [mwp](https://github.com/stronnag/mwptools)
|
||||
* [dbg-tool](https://github.com/stronnag/mwptools)
|
||||
* [dbg-tool](https://codeberg.org/stronnag/dbg-tool)
|
||||
* [INAV Configurator](https://github.com/iNavFlight/inav-configurator)
|
||||
|
||||
For example, with the final lines of `src/main/fc/fc_init.c` set to:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -179,11 +179,11 @@ void adcHardwareInit(drv_adc_config_t *init)
|
|||
adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice];
|
||||
// init adc gpio port
|
||||
IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0);
|
||||
IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_OUTPUT_OPEN_DRAIN, GPIO_PULL_NONE));
|
||||
IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, 0, 0));
|
||||
|
||||
adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
|
||||
adcConfig[i].dmaIndex = adc->usedChannelCount++;
|
||||
adcConfig[i].sampleTime = ADC_SAMPLETIME_6_5;
|
||||
adcConfig[i].sampleTime = ADC_SAMPLETIME_640_5;
|
||||
adcConfig[i].enabled = true;
|
||||
|
||||
adc->enabled = true;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -24,6 +24,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
|
||||
extern uint8_t multiFunctionFlags;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
182
src/main/navigation/rth_trackback.c
Normal file
182
src/main/navigation/rth_trackback.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* --------------------------------------------------------------------------------
|
||||
* == 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;
|
||||
}
|
||||
42
src/main/navigation/rth_trackback.h
Normal file
42
src/main/navigation/rth_trackback.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -109,11 +109,11 @@
|
|||
#define MAG_I2C_BUS BUS_I2C1
|
||||
#define USE_MAG_ALL
|
||||
|
||||
#define TEMPERATURE_I2C_BUS BUS_I2C1
|
||||
#define PITOT_I2C_BUS BUS_I2C1
|
||||
#define TEMPERATURE_I2C_BUS BUS_I2C2
|
||||
#define PITOT_I2C_BUS BUS_I2C2
|
||||
|
||||
#define USE_RANGEFINDER
|
||||
#define RANGEFINDER_I2C_BUS BUS_I2C1
|
||||
#define RANGEFINDER_I2C_BUS BUS_I2C2
|
||||
|
||||
// *************** UART *****************************
|
||||
#define USE_VCP
|
||||
|
|
|
|||
|
|
@ -1,2 +1,3 @@
|
|||
target_stm32f722xe(SKYSTARSF722HD)
|
||||
target_stm32f722xe(SKYSTARSF722HDPRO)
|
||||
target_stm32f722xe(SKYSTARSF722MINIHD)
|
||||
|
|
|
|||
|
|
@ -20,6 +20,9 @@
|
|||
#ifdef SKYSTARSF7MINIHD
|
||||
#define TARGET_BOARD_IDENTIFIER "SS7M"
|
||||
#define USBD_PRODUCT_STRING "SkystarsF722MiniHD"
|
||||
#elif defined(SKYSTARSF722HDPRO)
|
||||
#define TARGET_BOARD_IDENTIFIER "SS7P"
|
||||
#define USBD_PRODUCT_STRING "SkystarsF722HDPRO"
|
||||
#else
|
||||
#define TARGET_BOARD_IDENTIFIER "SS7D"
|
||||
#define USBD_PRODUCT_STRING "SkystarsF722HD"
|
||||
|
|
@ -45,6 +48,22 @@
|
|||
|
||||
#define USE_IMU_MPU6000
|
||||
#define IMU_MPU6000_ALIGN CW270_DEG
|
||||
#elif defined(SKYSTARSF722HDPRO)
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_BUS BUS_SPI1
|
||||
#define USE_IMU_MPU6000
|
||||
#define IMU_MPU6000_ALIGN CW270_DEG_FLIP
|
||||
|
||||
#define ICM42605_CS_PIN PA4
|
||||
#define ICM42605_SPI_BUS BUS_SPI1
|
||||
#define USE_IMU_ICM42605
|
||||
#define IMU_ICM42605_ALIGN CW90_DEG_FLIP
|
||||
|
||||
#define BMI270_CS_PIN PA4
|
||||
#define BMI270_SPI_BUS BUS_SPI1
|
||||
#define USE_IMU_BMI270
|
||||
#define IMU_BMI270_ALIGN CW90_DEG_FLIP
|
||||
|
||||
#else
|
||||
#define BMI270_CS_PIN PA4
|
||||
#define BMI270_SPI_BUS BUS_SPI1
|
||||
|
|
@ -81,6 +100,11 @@
|
|||
#define USE_BARO_BMP280
|
||||
#define BMP280_SPI_BUS BUS_SPI2
|
||||
#define BMP280_CS_PIN PB1
|
||||
#ifdef SKYSTARSF722HDPRO
|
||||
#define USE_BARO_SPL06
|
||||
#define SPL06_SPI_BUS BUS_SPI2
|
||||
#define SPL06_CS_PIN PB1
|
||||
#endif
|
||||
|
||||
// *************** UART *****************************
|
||||
#define USE_VCP
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue