Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/master' into sh_vtol_nav

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2023-12-28 12:07:58 +01:00
commit 859911599f
265 changed files with 6533 additions and 1672 deletions
Download patch file Download diff file Expand all files Collapse all files

7
.vscode/settings.json vendored
View file

@ -2,7 +2,12 @@
"files.associations": {
"chrono": "c",
"cmath": "c",
"ranges": "c"
"ranges": "c",
"navigation.h": "c",
"rth_trackback.h": "c"
"platform.h": "c",
"timer.h": "c",
"bus.h": "c"
},
"editor.tabSize": 4,
"editor.insertSpaces": true,

2
CMakeLists.txt
View file

@ -51,7 +51,7 @@ else()
endif()
endif()
project(INAV VERSION 7.0.0)
project(INAV VERSION 8.0.0)
enable_language(ASM)

54
cmake/arm-none-eabi-checks.cmake
View file

@ -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()

1
cmake/at32.cmake
View file

@ -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)

3
docs/Boards.md
View file

@ -12,6 +12,9 @@ These boards are well tested with INAV and are known to be of good quality and r
| [Holybro Kakute H7](https://inavflight.com/shop/s/bg/1914066) | H7 | KAKUTEH7 | All | All | All | All | All | SERIAL, SD |
It's possible to find more supported and tested boards [here](https://github.com/iNavFlight/inav/wiki/Welcome-to-INAV,-useful-links-and-products)
There is also a [full list of all supported boards](https://github.com/iNavFlight/inav/wiki/Boards,-Targets-and-PWM-allocations).
### Boards documentation
See the [docs/boards](https://github.com/iNavFlight/inav/tree/master/docs/boards) folder for additional information regards to many targets in INAV, to example help in finding pinout and features. _Feel free to help improve the docs._

132
docs/GPS_fix_estimation.md Normal file
View file

@ -0,0 +1,132 @@
# GPS Fix estimation (dead reconing, RTH without GPS) for fixed wing
Video demonstration
[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/wzvgRpXCS4U/0.jpg)](https://www.youtube.com/watch?v=wzvgRpXCS4U)
There is possibility to allow plane to estimate it's position when GPS fix is lost.
The main purpose is RTH without GPS.
It works for fixed wing only.
Plane should have the following sensors:
- acceleromenter, gyroscope
- barometer
- GPS
- magnethometer (optional, highly recommended)
- pitot (optional)
By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost.
GPS fix estimation allows to recover plane using magnetometer and baromener only.
GPS Fix is also estimated on GPS Sensor timeouts (hardware failures).
Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated.
# How it works ?
In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints.
Without GPS fix, plane has nose heading from magnetometer and height from barometer only.
To navigate without GPS fix, we make the following assumptions:
- plane is flying in the direction where nose is pointing
- (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings
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.
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*.
# Estimation without magnethometer
Without magnethometer, navigation accuracy is very poor. The problem is heading drift.
The longer plane flies without magnethometer or GPS, the bigger is course estimation error.
After few minutes and few turns, "North" direction estimation can be completely broken.
In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with occasional GPS outages.
![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da)
(purple line - estimated position, black line - real position).
It is recommened to use GPS fix estimation without magnethometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing.
It is up to user to estimate the risk of fly-away.
# Settings
GPS Fix estimation is enabled with CLI command:
```set inav_allow_gps_fix_estimation=ON```
Also you have to specify cruise airspeed of the plane.
To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed.
Cruise airspeed is specified in cm/s.
To convert km/h to m/s, multiply by 27.77.
Example: 100 km/h = 100 * 27.77 = 2777 cm/s
```set fw_reference_airspeed=2777```
*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle" - do not use it without GPS Fix.*
*If pitot is available, pitot sensor data will be used instead of constant. It is not necessary to specify fw_reference_airspeed. However, it is still adviced to specify for the case of pitot failure.*
*Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.*
**After entering CLI command, make sure that settings are saved:**
```save```
# Disabling GPS sensor from RC controller
![](Screenshots/programming_disable_gps_sensor_fix.png)
For testing purposes, it is possible to disable GPS sensor fix from RC controller in programming tab:
*GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.*
# Allowing wp missions with GPS Fix estimation
```failsafe_gps_fix_estimation_delay```
Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. RTH is trigerred regradless of failsafe procedure selected in configurator.
# Expected error (mag + baro)
Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen.
To dicrease drift:
- fly one large circle with GPS available to get good wind estimation
- use airspeed sensor. If airspeed sensor is not installed, fly in cruise mode without throttle override.
- do smooth, large turns
- make sure compass is pointing in nose direction precicely
- calibrate compass correctly
This video shows real world test where GPS was disabled occasionally. Wind is 10km/h south-west:
https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592
Purple line shows estimated position. Black line shows real position. "EST ERR" sensor shows estimation error in metters. Estimation is running when satellite icon displays "ES". Estimated position snaps to real position when GPS fix is reaquired.
# Is it possible to implement this for multirotor ?
There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing.
# Links
INAV HITL https://github.com/RomanLut/INAV-X-Plane-HITL

90
docs/LED pin PWM.md Normal file
View file

@ -0,0 +1,90 @@
# LED pin PWM
Normally LED pin is used to drive WS2812 led strip. LED pin is held low, and every 10ms or 20ms a set of pulses is sent to change color of the 32 LEDs:
![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets")
![alt text](/docs/assets/images/ws2811_data.png "ws2811 data")
As alternative function, it is possible to generate PWM signal with specified duty ratio on the LED pin.
Feature can be used to drive external devices. It is also used to simulate [OSD joystick](OSD%20Joystick.md) to control cameras.
PWM frequency is fixed to 24kHz with duty ratio between 0 and 100%:
![alt text](/docs/assets/images/led_pin_pwm.png "led pin pwm")
There are four modes of operation:
- low
- high
- shared_low
- shared_high
Mode is configured using ```led_pin_pwm_mode``` setting: ```LOW```, ```HIGH```, ```SHARED_LOW```, ```SHARED_HIGH```
*Note that in any mode, there will be ~2 seconds LOW pulse on boot.*
## LOW
LED Pin is initialized to output low level by default and can be used to generate PWM signal.
ws2812 strip can not be controlled.
## HIGH
LED Pin is initialized to output high level by default and can be used to generate PWM signal.
ws2812 strip can not be controlled.
## SHARED_LOW (default)
LED Pin is used to drive WS2812 strip. Pauses between pulses are low:
![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets")
It is possible to generate PWM signal with duty ratio >0...100%.
While PWM signal is generated, ws2811 strip is not updated.
When PWM generation is disabled, LED pin is used to drive ws2812 strip.
Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio < ~10%.
## SHARED_HIGH
LED Pin is used to drive WS2812 strip. Pauses between pulses are high. ws2812 pulses are prefixed with 50us low 'reset' pulse:
![alt text](/docs/assets/images/ws2811_packets_high.png "ws2811 packets_high")
![alt text](/docs/assets/images/ws2811_data_high.png "ws2811 data_high")
It is possible to generate PWM signal with duty ratio 0...<100%.
While PWM signal is generated, ws2811 strip is not updated.
When PWM generation is disabled, LED pin is used to drive ws2812 strip. Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio > ~90%.
After sending ws2812 protocol pulses for 32 LEDS, we held line high for 9ms, then send 50us low 'reset' pulse. Datasheet for ws2812 protocol does not describe behavior for long high pulse, but in practice it works the same as 'reset' pulse. To be safe, we also send correct low 'reset' pulse before starting next LEDs update sequence.
This mode is used to simulate OSD joystick. It is Ok that effectively voltage level is held >90% while driving LEDs, because OSD joystick keypress voltages are below 90%.
See [OSD Joystick](OSD%20Joystick.md) for more information.
# Generating PWM signal with programming framework
See "LED Pin PWM" operation in [Programming Framework](Programming%20Framework.md)
# Generating PWM signal from CLI
```ledpinpwm <value>``` - value = 0...100 - enable PWM generation with specified duty cycle
```ledpinpwm``` - disable PWM generation ( disable to allow ws2812 LEDs updates in shared modes )
# Example of driving LED
It is possible to drive single color LED with brightness control. Current consumption should not be greater then 1-2ma, thus LED can be used for indication only.
![alt text](/docs/assets/images/ledpinpwmled.png "led pin pwm led")
# Example of driving powerfull white LED
To drive power LED with brightness control, Mosfet should be used:
![alt text](/docs/assets/images/ledpinpwmpowerled.png "led pin pwm power_led")

143
docs/MixerProfile.md
View file

@ -1,80 +1,30 @@
# MixerProfile
A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings to enable VTOL transitions.
A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings. It is designed for experienced inav users.
Currently two profiles are supported on targets other than F411 and F722 (due to resource constraints on these FC). i.e VTOL transition is not available on F411 and F722 targets.
### For a tutorial of vtol setup, Read https://github.com/iNavFlight/inav/blob/master/docs/VTOL.md
By default, switching between profiles requires reboot to take affect. However, when all conditions are met, and a suitable [configuration](#configuration) has been applied, `mixer_profile` also allows in-flight profile [switching](#rc-mode-settings) to allow things like VTOL operation. This is the recommended operating mode.
Not limited to VTOL. air/land/sea mixed vehicle is also achievable with this feature. Model behaves according to current mixer_profile's platform_type and configured custom motor/servo mixer
Currently two profiles are supported on targets other than F411(due to resource constraints on F411). i.e VTOL transition is not available on F411.
For VTOL setup. one mixer_profile is used for multi-rotor(MR) and the other is used for fixed-wing(FW)
By default, switching between profiles requires reboot to take affect. However, using the RC mode: `MIXER PROFILE 2` will allow in flight switching for things like VTOL operation
. And will re-initialize pid and navigation controllers for current MC or FW flying mode.
Please note that this is an emerging / experimental capability that will require some effort by the pilot to implement.
## Setup for VTOL
- A VTOL specific FC target or `timer_output_mode` overrides was required in the early stage of the development, But since unified mapping introduced in INAV 7.0 It is not needed anymore.
- ~~For mixer profile switching it is necessary to keep motor and servo PWM mapping consistent between Fixed-Wing (FW) and Multi-rotor (MR) profiles~~
- ~~Traditionally, FW and MR have had different enumerations to define the PWM mappings. For VTOL operation it is necessary to set the `timer_output_mode` overrides to allow a consistent enumeration and thus mapping between MR and FW modes.~~
- ~~In operation, it is necessary to set the `mixer_profile` and the `pid_profile` separately and to set a [RC mode](#rc-mode-settings) to switch between them.~~
## Configuration
### Timer overrides
Set the timer overrides for the outputs that you are intended to use.
For SITL builds, is not necessary to set timer overrides.
Please note that there are some display issues on the configurator that will show wrong mapping on the mixer_profile which has less motor/servo compared with the another
### Profile Switch
Setup the FW mode and MR mode separately in two different mixer profiles:
In this example, FW mode is `mixer_profile` 1 and MR mode is `mixer_profile` 2.
Currently, the INAV Configurator does not fully support `mixer_profile`, so some of the settings have to be done in CLI.
Add `set mixer_pid_profile_linking = ON` in order to enable `pid_profile` auto handling. It will change the `pid profile` index according to the `mixer_profile` index on FC boot and allow `mixer_profile` hot switching (this is recommended usage).
The following 2 `mixer_profile` sections are added in the CLI:
```
#switch to mixer_profile by cli
mixer_profile 1
set platform_type = AIRPLANE
set model_preview_type = 26
# motor stop feature have been moved to here
set motorstop_on_low = ON
# enable pid_profile auto handling (recommended).
set mixer_pid_profile_linking = ON
save
```
Then finish the aeroplane setting on mixer_profile 1
```
mixer_profile 2
set platform_type = TRICOPTER
set model_preview_type = 1
# also enable pid_profile auto handling
set mixer_pid_profile_linking = ON
save
```
Then finish the multi-rotor setting on `mixer_profile` 2.
Note that default profile is profile `1`.
You can use `MAX` servo input to set a fixed input for the tilting servo. Speed setting for `MAX` input is available in the CLI.
It is recommended to have some amount of control surface (elevon / elevator) mapped for stabilization even in MR mode to get improved authority when airspeed is high.
**Double check all settings in CLI with the `diff all` command**; make sure you have set the correct settings. Also check what will change with `mixer_profile`. For example servo output min / max range will not change. But `smix` and `mmix` will change.
### Mixer Transition input
## Mixer Transition input
Typically, 'transition input' will be useful in MR mode to gain airspeed.
Both the servo mixer and motor mixer can accept transition mode as an input.
The associated motor or servo will then move accordingly when transition mode is activated.
Transition input is disabled when navigation mode is activate
The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended
#### Servo
## Servo
38 is the input source for transition input; use this to tilt motor to gain airspeed.
`Mixer Transition` is the input source for transition input; use this to tilt motor to gain airspeed.
Example: Increase servo 1 output by +45 with speed of 150 when transition mode is activated for tilted motor setup:
@ -82,15 +32,14 @@ Example: Increase servo 1 output by +45 with speed of 150 when transition mode i
# rule no; servo index; input source; rate; speed; activate logic function number
smix 6 1 38 45 150 -1
```
Please note there will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. More forward tilting servo position on transition input(you can use 'speed' in servo rules to slowly move to this position), A faster tilting servo speed on `MAX` servo input will reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect.
#### Motor
## Motor
The default `mmix` throttle value is 0.0, It will not show in `diff` command when throttle value is 0.0 (unused); this causes the motor to stop.
The default `mmix` throttle value is 0.0, It will not show in `diff` command when throttle value is 0.0 (unused);
- 0.0<throttle<=1.0 : normal mapping
- -1.0<throttle<=0.0 : motor stop, default value 0
- -2.0<throttle<-1.0 : spin regardless of the radio's throttle position at speed `abs(throttle)-1` when Mixer Transition is activated.
- -1.0<throttle<=0.0 : motor stop, default value 0, set to -1 to use a place holder for subsequent motor rules
- -2.0<throttle<-1.0 : spin regardless of throttle position at speed `abs(throttle)-1` when Mixer Transition is activated.
Example: This will spin motor number 5 (counting from 1) at 20%, in transition mode only, to gain speed for a "4 rotor 1 pusher" setup:
@ -99,14 +48,13 @@ Example: This will spin motor number 5 (counting from 1) at 20%, in transition m
mmix 4 -1.200 0.000 0.000 0.000
```
### RC mode settings
## RC mode settings
It is recommend that the pilot uses a RC mode switch to activate modes or switch profiles.
Profile files Switching is not available until the runtime sensor calibration is done. Switching is NOT available when navigation mode is activate or position controller is activate, including altitude hold.
Profile files Switching is not available until the runtime sensor calibration is done. Switching is NOT available when navigation mode is activate.
`mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. Once successfully set, you can see the profiles / model preview etc. will switch accordingly when you view the relevant INAV Configurator tabs. Checking these tabs in the INAV Configurator will help make the setup easier.
Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. Here is sample of using the `MIXER TRANSITION` mode:
`mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated.
Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. Here is sample of using these RC modes:
![Alt text](Screenshots/mixer_profile.png)
@ -116,29 +64,50 @@ Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input
It is also possible to set it as 4 state switch by adding FW(profile1) with transition on.
### Automated Transition
## Automated Transition
This feature is mainly for RTH in a failsafe event. When set properly, model will use the FW mode to fly home efficiently, And land in the MC mode for easier landing.
Set `mixer_automated_switch` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode, for example, 50 ds. Finally set `mixer_automated_switch` to `ON` in mixer_profile for FW mode to let the model land in MC mode.
```
mixer_profile 2
set mixer_automated_switch = ON
set mixer_switch_trans_timer = 50
mixer_profile 1
set mixer_automated_switch = ON
save
```
`ON` for a mixer_profile\`s `mixer_automated_switch` means to schedule a Automated Transition when RTH head home(applies for MC mixer_profile) or RTH Land(applies for FW mixer_profile) is requested by navigation controller.
Set `mixer_automated_switch` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode.
When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all.
## TailSitter (planned for INAV 7.1)
TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing set `tailsitter_orientation_offset = ON ` to apply orientation offset. orientation offset will also add a 45deg orientation offset.
## Parameter list (Partial List)
#### Please be aware of what parameter is shared among FW/MC modes and what isn't.
### Shared Parameters
- **Timer Overrides**
- **Outputs [Servo]:**
- Servo min-point, mid-point, max-point settings
- **Motor Configuration:**
- motor_pwm_protocol
- motor_poles
- **Servo Configuration:**
- servo_protocol
- servo_pwm_rate
- **Board Alignment**
- ·······
### Profile-Specific Parameters in VTOL
- **Mixer Profile**
- **Mixer Configuration:**
- platform_type
- motor_stop_on_low
- tailsitter_orientation_offset
- motor_direction_inverted, and more·······
- **Motor Mixing (mmix)**
- **Servo Mixing (smix)**
- **PID Profile**
- PIDs for Roll, Pitch, Yaw
- PIDs for Navigation Modes
- TPA (Throttle PID Attenuation) Settings
- Rate Settings
- ·······
## Happy flying
Remember that this is currently an emerging capability:
* Test every thing on bench first.
* Remove the props and try `MIXER PROFILE 2`, `MIXER TRANSITION` RC modes while arming.
* Then try MR or FW mode separately see if there are any problems.
* Try it somewhere you can recover your model in case of fail-safe. Fail-safe behavior is unknown at the current stage of development.
* Try MR or FW mode separately see if there are any problems.
* Use the INAV Discord for help and setup questions; use the Github Issues for reporting bugs and unexpected behaviors. For reporting on Github, a CLI `diff all`, a DVR and a Blackbox log of the incident will assist investigation.

94
docs/OSD Joystick.md Normal file
View file

@ -0,0 +1,94 @@
# OSD joystick
LED pin can be used to emulate 5key OSD joystick for OSD camera pin, while still driving ws2812 LEDs (shared functionality).
See [LED pin PWM](LED%20pin%20PWM.md) for more details.
Note that for cameras which support RuncamDevice protocol, there is alternative functionality using serial communication: [Runcam device](Runcam%20device.md)
Also special adapters exist to convert RuncamDevice protocol to OSD Joystick: [Runcam control adapter](https://www.runcam.com/download/runcam_control_adapter_manual.pdf)
# OSD Joystick schematics
![alt text](/docs/assets/images/osd_joystick_keys.png "osd jystick keys")
Camera internal resistance seems to be 47kOhm or 9kOhm depending on camera model.
Each key effectively turns on voltage divider. Voltage is sensed by the camera and is compared to the list of keys voltages with some threshold.
Key voltage has to be held for at least 200ms.
To simulate 5key joystick, it is sufficient to generate correct voltage on camera OSD pin.
# Enabling OSD Joystick emulation
```set led_pin_pwm_mode=shared_high```
```set osd_joystick_enabled=on```
Also enable "Multi-color RGB LED Strip support" in Configuration tab.
# Connection diagram
We use LED pin PWM functionality with RC filter to generate voltage:
![alt text](/docs/assets/images/ledpinpwmfilter.png "led pin pwm filter")
# Example PCB layout (SMD components)
RC Filter can be soldered on a small piece of PCB:
![alt text](/docs/assets/images/osd_joystick.jpg "osd joystick")
# Configuring keys voltages
If default voltages does not work with your camera model, then you have to measure voltages and find out corresponding PWM duty ratios.
1. Connect 5keys joystick to camera.
2. Measure voltages on OSD pin while each key is pressed.
3. Connect camera to FC throught RC filter as shown on schematix above.
4. Enable OSD Joystick emulation (see "Enabling OSD Joystick emulation" above)
4. Use cli command ```led_pin_pwm <value>```, value = 0...100 to find out PWM values for each voltage.
5. Specify PWM values in configuration and save:
```set osd_joystick_down=0```
```set osd_joystick_up=48```
```set osd_joystick_left=63```
```set osd_joystick_right=28```
```set osd_joystick_enter=75```
```save```
# Entering OSD Joystick emulation mode
Emulation can be enabled in unarmed state only.
OSD Joystick emulation mode is enabled using the following stick combination:
```Throttle:CENTER Yaw:RIGHT```
Than camera OSD can be navigated using right stick. See [Controls](Controls.md) for all stick combinations.
*Note that the same stick combination is used to enable 5keys joystick emulation with RuncamDevice protocol.*
Mode is exited using stick combination:
```Throttle:CENTER Yaw:LEFT```
# RC Box
There are 3 RC Boxes which can be used in armed and unarmed state:
- Camera 1 - Enter
- Camera 2 - Up
- Camera 3 - Down
Other keys can be emulated using Programming framework ( see [LED pin PWM](LED%20pin%20PWM.md) for more details ).
# Behavior on boot
There is ~2 seconds LOW pulse during boot sequence, which corresponds to DOWN key. Fortunately, cameras seem to ignore any key events few seconds after statup.

31
docs/OSD.md
View file

@ -6,7 +6,7 @@ The On Screen Display, or OSD, is a feature that overlays flight data over the v
Not all OSDs are created equally. This table shows the differences between the different systems available.
| OSD System | Character grid | Character | Canvas | MSP DisplayPort | All elements supported |
|---------------|-------------------|-----------|-----------|-------------------|---------------------------|
|---------------|----------------|-----------|--------|-----------------|-------------------------|
| Analogue PAL | 30 x 16 | X | | | YES |
| Analogue NTSC | 30 x 13 | X | | | YES |
| PixelOSD | As PAL or NTSC | | X | | YES |
@ -20,7 +20,7 @@ Not all OSDs are created equally. This table shows the differences between the d
Here are the OSD Elements provided by INAV.
| ID | Element | Added |
|-------|-----------------------------------------------|-------|
|-----|--------------------------------------------------|--------|
| 0 | OSD_RSSI_VALUE | 1.0.0 |
| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 |
| 2 | OSD_CROSSHAIRS | 1.0.0 |
@ -165,3 +165,30 @@ Here are the OSD Elements provided by INAV.
| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 |
| 142 | OSD_PILOT_NAME | 6.0.0 |
| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 |
| 144 | OSD_MULTI_FUNCTION | 7.0.0 |
| 145 | OSD_ODOMETER | 7.0.0 |
| 146 | OSD_PILOT_LOGO | 7.0.0 |
# Pilot Logos
From INAV 7.0.0, pilots can add their own logos to the OSD. These can appear in 2 places: the power on/arming screens or as an element on the standard OSD. Please note that the power on/arming screen large pilot logos are only available on HD systems.
To use the pilot logos, you will need to make a custom font for your OSD system. Base fonts and information can be found in the [OSD folder](https://github.com/iNavFlight/inav-configurator/tree/master/resources/osd) in the Configurator resources. Each system will need a specific method to create the font image files. So they will not be covered here. There are two pilot logos.
<img alt="Default small INAV Pilot logo" src="https://github.com/iNavFlight/inav-configurator/raw/master/resources/osd/digital/default/24x36/469_471.png" align="right" />The small pilot logo appears on standard OSD layouts, when you add the elemement to the OSD screen. This is a 3 character wide symbol (characters 469-471).
<img alt="Default large INAV Pilot logo" src="https://github.com/iNavFlight/inav-configurator/raw/master/resources/osd/digital/default/24x36/472_511.png" align="right" />The large pilot logo appears on the power on and arming screens, when you enable the feature in the CLI. To do this, set the `osd_use_pilot_logo` parameter to `on`. This is a 10 character wide, 4 character high symbol (characters 472-511).
## Settings
* `osd_arm_screen_display_time` The amount of time the arming screen is displayed.
* `osd_inav_to_pilot_logo_spacing` The spacing between two logos. This can be set to `0`, so the original INAV logo and Pilot Logo can be combined in to a larger logo. Any non-0 value will be adjusted to best align the logos. For example, the Avatar system has an odd number of columns. If you set the spacing to 8, the logos would look misaligned. So the even number will be changed to an odd number for better alignment.
* `osd_use_pilot_logo` Enable to use the large pilot logo.
## Examples
This is an example of the arming screen with the pilot logo enabled. This is using the default settings.
![Arming screen example using default settings with osd_use_pilot_logo set to on](https://user-images.githubusercontent.com/17590174/271817487-eb18da4d-0911-44b2-b670-ea5940f79176.png)
This is an example of setting the `osd_inav_to_pilot_logo_spacing` to 0. This will allow a larger, single logo.
![Power on screen example with 0 spacing between logos](https://user-images.githubusercontent.com/17590174/271817352-6206402c-9da4-4682-9d83-790cc2396b00.png)

3
docs/Programming Framework.md
View file

@ -83,7 +83,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
@ -97,6 +97,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 49 | TIMER | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
| 50 | DELTA | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
| 51 | APPROX_EQUAL | `true` if `Operand B` is within 1% of `Operand A`. |
| 52 | LED_PIN_PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)|
### Operands

32
docs/Runcam device.md Normal file
View file

@ -0,0 +1,32 @@
# Runcam device
Cameras which support [Runcam device protocol](https://support.runcam.com/hc/en-us/articles/360014537794-RunCam-Device-Protocol), can be configured using sticks.
Note that for cameras which has OSD pin, there is alternative functionality: [OSD Joystick](OSD%20Joystick.md).
Camera's RX/TX should be connected to FC's UART, which has "Runcam device" option selected.
# Entering Joystick emulation mode
Emulation can be enabled in unarmed state only.
Joystick emulation mode is enabled using the following stick combination:
```RIGHT CENTER```
Than camera OSD can be navigated using right stick. See [Controls](Controls.md) for all stick combinations.
*Note that the same stick combination is used to enable [OSD Joystick](OSD%20Joystick.md).*
Mode is exited using stick combination:
```LEFT CENTER```
# RC Box
There are 3 RC Boxes which can be used in armed and unarmed state:
- Camera 1 - Simulate Wifi button
- Camera 2 - Simulate POWER button
- Camera 3 - Simulate Change Mode button.

2
docs/SITL/SITL.md
View file

@ -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.

BIN
docs/Screenshots/mixerprofile_4puls1_mix.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 21 KiB

BIN
docs/Screenshots/mixerprofile_flow.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 24 KiB

BIN
docs/Screenshots/mixerprofile_fw_mixer.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 314 KiB

BIN
docs/Screenshots/mixerprofile_mc_mixer.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 394 KiB

BIN
docs/Screenshots/mixerprofile_servo_transition_mix.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 19 KiB

BIN
docs/Screenshots/programming_disable_gps_sensor_fix.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 15 KiB

160
docs/Settings.md
View file

@ -242,6 +242,16 @@ Inertial Measurement Unit KP Gain for compass measurements
---
### ahrs_gps_yaw_weight
Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass
| Default | Min | Max |
| --- | --- | --- |
| 100 | 0 | 500 |
---
### ahrs_gps_yaw_windcomp
Wind compensation in heading estimation from gps groundcourse(fixed wing only)
@ -1002,6 +1012,16 @@ Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active
---
### failsafe_gps_fix_estimation_delay
Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation.
| Default | Min | Max |
| --- | --- | --- |
| 7 | -1 | 600 |
---
### failsafe_lights
Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF].
@ -1474,11 +1494,11 @@ Enable automatic configuration of UBlox GPS receivers.
### gps_dyn_model
GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying.
GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying.
| Default | Min | Max |
| --- | --- | --- |
| AIR_1G | | |
| AIR_2G | | |
---
@ -1762,6 +1782,16 @@ Defines if INAV will dead-reckon over short GPS outages. May also be useful for
---
### inav_allow_gps_fix_estimation
Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### inav_auto_mag_decl
Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored.
@ -1912,33 +1942,23 @@ Decay coefficient for estimated velocity when GPS reference for position is lost
---
### inav_w_xyz_acc_p
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 1.0 | 0 | 1 |
---
### inav_w_z_baro_p
Weight of barometer measurements in estimated altitude and climb rate
Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.
| Default | Min | Max |
| --- | --- | --- |
| 0.35 | 0 | 10 |
| 0.4 | 0 | 10 |
---
### inav_w_z_gps_p
Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes
Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
| Default | Min | Max |
| --- | --- | --- |
| 0.2 | 0 | 10 |
| 0.4 | 0 | 10 |
---
@ -1948,7 +1968,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
| Default | Min | Max |
| --- | --- | --- |
| 0.1 | 0 | 10 |
| 0.8 | 0 | 10 |
---
@ -2012,6 +2032,16 @@ Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened w
---
### led_pin_pwm_mode
PWM mode of LED pin.
| Default | Min | Max |
| --- | --- | --- |
| SHARED_LOW | | |
---
### ledstrip_visual_beeper
_// TODO_
@ -2434,7 +2464,7 @@ This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK
### mc_cd_lpf_hz
Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky
Cutoff frequency for Control Derivative. This controls the cutoff for the LPF that is applied to the CD (Feed Forward) signal to the PID controller. Lower value will produce a smoother CD gain to the controller, but it will be more delayed. Higher values will produce CD gain that may have more noise in the signal depending on your RC link but wil be less delayed.
| Default | Min | Max |
| --- | --- | --- |
@ -2444,7 +2474,7 @@ Cutoff frequency for Control Derivative. Lower value smoother reaction on fast s
### mc_cd_pitch
Multicopter Control Derivative gain for PITCH
Multicopter Control Derivative gain for PITCH (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough.
| Default | Min | Max |
| --- | --- | --- |
@ -2454,7 +2484,7 @@ Multicopter Control Derivative gain for PITCH
### mc_cd_roll
Multicopter Control Derivative gain for ROLL
Multicopter Control Derivative gain for ROLL (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough.
| Default | Min | Max |
| --- | --- | --- |
@ -2464,7 +2494,7 @@ Multicopter Control Derivative gain for ROLL
### mc_cd_yaw
Multicopter Control Derivative gain for YAW
Multicopter Control Derivative gain for YAW (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough.
| Default | Min | Max |
| --- | --- | --- |
@ -2778,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 |
---
@ -3692,6 +3722,16 @@ When ON, NAV engine will slow down when switching to the next waypoint. This pri
---
### nav_min_ground_speed
Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s.
| Default | Min | Max |
| --- | --- | --- |
| 7 | 6 | 50 |
---
### nav_min_rth_distance
Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase.
@ -3822,6 +3862,16 @@ If set to ON, aircraft will execute initial climb regardless of position sensor
---
### nav_rth_fs_landing_delay
If landing is active on Failsafe and this is above 0. The aircraft will hover or loiter for X seconds before performing the landing. If the battery enters the warning or critical levels, the land will proceed. Default = 0 [seconds]
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 1800 |
---
### nav_rth_home_altitude
Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm]
@ -4452,6 +4502,66 @@ The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. Thi
---
### osd_joystick_down
PWM value for DOWN key
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 100 |
---
### osd_joystick_enabled
Enable OSD Joystick emulation
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### osd_joystick_enter
PWM value for ENTER key
| Default | Min | Max |
| --- | --- | --- |
| 75 | 0 | 100 |
---
### osd_joystick_left
PWM value for LEFT key
| Default | Min | Max |
| --- | --- | --- |
| 63 | 0 | 100 |
---
### osd_joystick_right
PWM value for RIGHT key
| Default | Min | Max |
| --- | --- | --- |
| 28 | 0 | 100 |
---
### osd_joystick_up
PWM value for UP key
| Default | Min | Max |
| --- | --- | --- |
| 48 | 0 | 100 |
---
### osd_left_sidebar_scroll
_// TODO_
@ -4998,7 +5108,7 @@ Selection of pitot hardware.
| Default | Min | Max |
| --- | --- | --- |
| VIRTUAL | | |
| NONE | | |
---
@ -5934,11 +6044,11 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units,
### vtx_band
Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race.
Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race.
| Default | Min | Max |
| --- | --- | --- |
| 1 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND |
| 1 | VTX_SETTINGS_MIN_BAND | VTX_SETTINGS_MAX_BAND |
---

236
docs/VTOL.md Normal file
View file

@ -0,0 +1,236 @@
# Welcome to INAV VTOL
Thank you for trying the INAV VTOL. Read every line in this tutorial. Your patience can save both time and potential repair costs for the model.
## Who Should Use This Tutorial?
This tutorial is designed for individuals who
- have prior experience with **both INAV multi-rotor and INAV fixed-wing configurations/operations.**
- know how to create a custom mixer for their model.
- know basic physics of vtol operation
## Firmware Status
The firmware is in a flyable state, but it hasn't undergone extensive testing yet. This means there may be potential issues that have not yet been discovered.
## Future Changes
Please be aware that both the setup procedure and firmware may change in response to user feedback and testing results.
## Your Feedback Matters
We highly value your feedback as it plays a crucial role in the development and refinement of INAV VTOL capabilities. Please share your experiences, suggestions, and any issues you encounter during testing. Your insights are invaluable in making INAV VTOL better for everyone.
# VTOL Configuration Steps
### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated PID profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/PID profiles are shared among two modes
![Alt text](Screenshots/mixerprofile_flow.png)
0. **Find a DIFF ALL file for your model and start from there if possible**
- Be aware that `MIXER PROFILE 2` RC mode setting introduced by diff file can get your stuck in a mixer_profile. remove or change channel to proceed
1. **Setup Profile 1:**
- Configure it as a normal fixed-wing/multi-copter.
2. **Setup Profile 2:**
- Configure it as a normal multi-copter/fixed-wing.
3. **Mode Tab Settings:**
- Set up switching in the mode tab.
4. *(Recommended)* **Transition Mixing (Multi-Rotor Profile):**
- Configure transition mixing to gain airspeed in the multi-rotor profile.
5. *(Optional)* **Automated Switching (RTH):**
- Optionally, set up automated switching in case of failsafe.
# STEP0: Load parameter preset/templates
Find a working diff file if you can and start from there. If not, select keep current settings and apply following parameter in cli but read description about which one to apply.
```
set small_angle = 180
set gyro_main_lpf_hz = 80
set dynamic_gyro_notch_min_hz = 50
set dynamic_gyro_notch_mode = 3D
set motor_pwm_protocol = DSHOT300 #Try dshot first and see if it works
set airmode_type = STICK_CENTER_ONCE
set nav_disarm_on_landing = OFF #band-aid for false landing detection in NAV landing of multi-copter
set nav_rth_allow_landing = FS_ONLY
set nav_wp_max_safe_distance = 500
set nav_fw_control_smoothness = 2
set nav_fw_launch_max_altitude = 5000
set servo_pwm_rate = 160 #If model using servo for stabilization in MC mode and servo can tolerate it
set servo_lpf_hz = 30 #If model using servo for stabilization in MC mode
## profile 1 as airplane and profile 2 as multi rotor
mixer_profile 1
set platform_type = AIRPLANE
set model_preview_type = 26
set motorstop_on_low = ON
set mixer_pid_profile_linking = ON
mixer_profile 2
set platform_type = TRICOPTER
set model_preview_type = 1
set mixer_pid_profile_linking = ON
profile 1 #pid profile
set dterm_lpf_hz = 10
set d_boost_min = 1.000
set d_boost_max = 1.000
set fw_level_pitch_trim = 5.000
set roll_rate = 18
set pitch_rate = 9
set yaw_rate = 3
set fw_turn_assist_pitch_gain = 0.4
set max_angle_inclination_rll = 450
set fw_ff_pitch = 80
set fw_ff_roll = 50
set fw_p_pitch = 15
set fw_p_roll = 15
profile 2
set dterm_lpf_hz = 60
set dterm_lpf_type = PT3
set d_boost_min = 0.800
set d_boost_max = 1.200
set d_boost_gyro_delta_lpf_hz = 60
set antigravity_gain = 2.000
set antigravity_accelerator = 5.000
set smith_predictor_delay = 1.500
set tpa_rate = 20
set tpa_breakpoint = 1200
set tpa_on_yaw = ON #If model using control surface/tilt mechanism for stabilization in MC mode
set roll_rate = 18
set pitch_rate = 18
set yaw_rate = 9
set mc_iterm_relax = RPY
save
```
# STEP1&2: Configuring as a Normal fixed-wing/Multi-Copter in two profiles separately
1. **Select the fisrt Mixer Profile and PID Profile:**
- In the CLI, switch to the mixer_profile and pid_profile you wish to set first. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded.
```
mixer_profile 1 #in this example, we set profile 1 first
set mixer_pid_profile_linking = ON # Let the mixer_profile handle the pid_profile switch on this mixer_profile
set platform_type = AIRPLANE
save
```
2. **Configure the fixed-wing/Multi-Copter:**
- Configure your fixed-wing/Multi-Copter as you normally would, or you can copy and paste default settings to expedite the process.
- Dshot esc protocol availability might be limited depends on outputs and fc board you are using. change the motor wiring or use oneshot/multishot esc protocol and calibrate throttle range.
- You can use throttle = -1 as a placeholder for the motor you wish to stop if the motor isn't the last motor
- Consider conducting a test flight to ensure that everything operates as expected. And tune the settings, trim the servos.
![Alt text](Screenshots/mixerprofile_fw_mixer.png)
3. **Switch to Another Mixer Profile with PID Profile:**
- In the CLI, switch to another mixer_profile along with the appropriate pid_profile. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded.
```
mixer_profile 2
set mixer_pid_profile_linking = ON
set platform_type = MULTIROTOR/TRICOPTER
save
```
4. **Configure the Multi-Copter/fixed-wing:**
- Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and pid_profile 2.
- Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint.
- At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings.
- you can set -1 in motor mixer throttle as a place holder: disable that motor but will load following motor rules
- compass is required to enable navigation modes for multi-rotor profile.
- Consider conducting a test flight to ensure that everything operates as expected. And tune the settings.
- It is advisable to have a certain degree of control surface (elevon / elevator) mapping for stabilization even in multi-copter mode. This helps improve control authority when airspeed is high. It might be unable to recover from a dive without them.
![Alt text](Screenshots/mixerprofile_mc_mixer.png)
5. **Tailsitters:planned for INAV 7.1**
- Configure the fixed-wing mode/profile sets normally. Use MultiCopter platform type for tail_sitting flying mode/profile sets.
- The baseline board aliment is FW mode(ROLL axis is the trust axis). So set `tailsitter_orientation_offset = ON ` in the tail_sitting MC mode.
- Configure mixer ROLL/YAW mixing according to tail_sitting orientation in the tail_sitting MC mode. YAW axis is the trust axis.
- Conduct a bench test and see the orientation of the model changes in inav-configurator setup tab
# STEP3: Mode Tab Settings:
### We recommend using an 3-pos switch on you radio to activate these modes, So pilot can jump in or bell out at any moment.
### Here is a example, in the bottom of inav-configurator Modes tab:
![Alt text](Screenshots/mixer_profile.png)
| 1000~1300 | 1300~1700 | 1700~2000 |
| :-- | :-- | :-- |
| Profile1(FW) with transition off | Profile2(MC) with transition on | Profile2(MC) with transition off |
- Profile file switching becomes available after completing the runtime sensor calibration(15-30s after booting). And It is **not available** when a navigation mode or position hold is active.
- By default, `mixer_profile 1` is used. `mixer_profile 2` is used when the `MIXER PROFILE 2` mode is activate. Once configured successfully, you will notice that the profiles and model preview changes accordingly when you refresh the relevant INAV Configurator tabs.
- Use the `MIXER TRANSITION` mode to gain airspeed in MC profile, set `MIXER TRANSITION` accordingly.
Conduct a bench test on the model (without props attached). The model can now switch between fixed-wing and multi-copter modes while armed. Furthermore, it is capable of mid-air switching, resulting in an immediate stall upon entering fixed-wing profile
# STEP4: Transition Mixing (Multi-Rotor Profile)(Recommended)
### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing.
Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling.
## Servo 'Transition Mixing': Tilting rotor configuration.
Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weight/rate according to your desired angle. This will allow tilting the motor for tilting rotor model.
![Alt text](Screenshots/mixerprofile_servo_transition_mix.png)
## Motor 'Transition Mixing': Dedicated forward motor configuration
In motor mixer set:
- -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated.
![Alt text](Screenshots/mixerprofile_4puls1_mix.png)
## TailSitter 'Transition Mixing':
No additional settings needed, 45deg off set will be added to target pitch angle for angle mode in the firmware.
### With aforementioned settings, your model should be able to enter fixed-wing profile without stalling.
# Automated Switching (RTH) (Optional):
### This is one of the least tested features. This feature is primarily designed for Return to Home (RTH) in the event of a failsafe.
When configured correctly, the model will use the Fixed-Wing (FW) mode to efficiently return home and then transition to Multi-Copter (MC) mode for easier landing.
To enable this feature, type following command in cli
1. In your MC mode mixer profile (e.g., mixer_profile 2), set `mixer_automated_switch` to `ON`. leave it to `OFF` if burning remaining battery capacity on the way home is acceptable.
```
mixer_profile 2or1
set mixer_automated_switch= ON
```
2. Set `mixer_switch_trans_timer` ds in cli in the MC mode mixer profile to specify the time required for your model to gain sufficient airspeed before transitioning to FW mode.
```
mixer_profile 2or1
set mixer_switch_trans_timer = 30 # 3s, 3000ms
```
3. In your FW mode mixer profile (e.g., mixer_profile 1), also set `mixer_automated_switch` to `ON`. leave it to `OFF` if automated landing in fixed-wing is acceptable.
```
mixer_profile 1or2
set mixer_automated_switch = ON
```
4. Save your settings. type `save` in cli.
If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default setting), the model will not perform automated transitions. You can always enable navigation modes after performing a manual transition.
# Notes and Experiences
## General
- VTOL model operating in multi-copter (MC) mode may encounter challenges in windy conditions. Please exercise caution when testing in such conditions.
- Make sure you can recover from a complete stall before trying the mid air transition
- It will be much safer if you can understand every line in diff all, read your diff all before maiden
## Tilting-rotor
- In some tilting motor models, you may experience roll/yaw coupled oscillations when `MIXER TRANSITION` is activated. To address this issue, you can try the following:
1. Use prop blade meets at top/rear prop direction for tilting motors to balance the effects of torque and P-factor.
2. In addition to 1. Add a little yaw mixing(about 0.2) in tilt motors.
- There will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. Use the max speed or faster speed in tiling servo to reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect.
## Dedicated forward motor
- Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight

BIN
docs/assets/images/led_pin_pwm.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 5 KiB

BIN
docs/assets/images/ledpinpwmfilter.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 3.3 KiB

BIN
docs/assets/images/ledpinpwmled.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 2.1 KiB

BIN
docs/assets/images/ledpinpwmpowerled.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 3.4 KiB

BIN
docs/assets/images/osd_joystick.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 26 KiB

BIN
docs/assets/images/osd_joystick_keys.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 5.6 KiB

BIN
docs/assets/images/ws2811_data.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 4.2 KiB

BIN
docs/assets/images/ws2811_data_high.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 4 KiB

BIN
docs/assets/images/ws2811_packets.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 6.4 KiB

BIN
docs/assets/images/ws2811_packets_high.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 5 KiB

5
docs/boards/MAMBAH743_2022B.md Normal file
View file

@ -0,0 +1,5 @@
# VTX Power SWITCH
Contrary to what the documentation suggests, VTX power is actually on USER2.

2
docs/boards/SPEEDYBEEF405V3.md
View file

@ -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.

4
docs/development/Building in Windows 10 or 11 with Linux Subsystem.md
View file

@ -36,7 +36,9 @@ To run `cmake` in the latest version you will need to update from Ubuntu `18_04
Mount MS windows C drive and clone INAV
1. `cd /mnt/c`
1. `git clone https://github.com/iNavFlight/inav.git`
2. `git clone https://github.com/iNavFlight/inav.git`
3. `git checkout 6.1.1` (to switch to a specific release tag, for this example INAV version 6.1.1)
4. `git checkout -b my-branch` (to create own branch)
You are ready!
You now have a folder called inav in the root of C drive that you can edit in windows

2
lib/main/MAVLink/common/common.h
View file

@ -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

93
lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c
View file

@ -184,8 +184,8 @@ __ALIGN_BEGIN uint8_t APP_Rx_Buffer [APP_RX_DATA_SIZE] __ALIGN_END ;
#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
__ALIGN_BEGIN uint8_t CmdBuff[CDC_CMD_PACKET_SZE] __ALIGN_END ;
uint32_t APP_Rx_ptr_in = 0;
uint32_t APP_Rx_ptr_out = 0;
volatile uint32_t APP_Rx_ptr_in = 0;
volatile uint32_t APP_Rx_ptr_out = 0;
uint32_t APP_Rx_length = 0;
uint8_t USB_Tx_State = USB_CDC_IDLE;
@ -619,7 +619,6 @@ uint8_t usbd_cdc_DataIn (void *pdev, uint8_t epnum)
{
(void)pdev;
(void)epnum;
uint16_t USB_Tx_ptr;
uint16_t USB_Tx_length;
if (USB_Tx_State == USB_CDC_BUSY)
@ -627,46 +626,32 @@ uint8_t usbd_cdc_DataIn (void *pdev, uint8_t epnum)
if (APP_Rx_length == 0)
{
USB_Tx_State = USB_CDC_IDLE;
}
else
{
} else {
if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) {
USB_Tx_ptr = APP_Rx_ptr_out;
USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE;
APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE;
}
else
{
USB_Tx_ptr = APP_Rx_ptr_out;
} else {
USB_Tx_length = APP_Rx_length;
APP_Rx_ptr_out += APP_Rx_length;
APP_Rx_length = 0;
if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE)
{
if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) {
USB_Tx_State = USB_CDC_ZLP;
}
}
/* Prepare the available data buffer to be sent on IN endpoint */
DCD_EP_Tx (pdev,
CDC_IN_EP,
(uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr],
USB_Tx_length);
DCD_EP_Tx(pdev, CDC_IN_EP, (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], USB_Tx_length);
// Advance the out pointer
APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE;
APP_Rx_length -= USB_Tx_length;
return USBD_OK;
}
}
/* Avoid any asynchronous transfer during ZLP */
if (USB_Tx_State == USB_CDC_ZLP)
{
if (USB_Tx_State == USB_CDC_ZLP) {
/*Send ZLP to indicate the end of the current transfer */
DCD_EP_Tx (pdev,
CDC_IN_EP,
NULL,
0);
DCD_EP_Tx(pdev, CDC_IN_EP, NULL, 0);
USB_Tx_State = USB_CDC_IDLE;
}
@ -731,66 +716,48 @@ uint8_t usbd_cdc_SOF (void *pdev)
*/
static void Handle_USBAsynchXfer (void *pdev)
{
uint16_t USB_Tx_ptr;
uint16_t USB_Tx_length;
if(USB_Tx_State == USB_CDC_IDLE)
{
if (APP_Rx_ptr_out == APP_RX_DATA_SIZE)
{
APP_Rx_ptr_out = 0;
}
if(APP_Rx_ptr_out == APP_Rx_ptr_in)
{
USB_Tx_State = USB_CDC_IDLE;
if (USB_Tx_State == USB_CDC_IDLE) {
if (APP_Rx_ptr_out == APP_Rx_ptr_in) {
// Ring buffer is empty
return;
}
if(APP_Rx_ptr_out > APP_Rx_ptr_in) /* rollback */
{
if (APP_Rx_ptr_out > APP_Rx_ptr_in) {
// Transfer bytes up to the end of the ring buffer
APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out;
}
else
{
} else {
// Transfer all bytes in ring buffer
APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out;
}
#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED
// Only transfer whole 32 bit words of data
APP_Rx_length &= ~0x03;
#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE)
{
USB_Tx_ptr = APP_Rx_ptr_out;
if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) {
USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE;
APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE;
USB_Tx_State = USB_CDC_BUSY;
}
else
{
USB_Tx_ptr = APP_Rx_ptr_out;
} else {
USB_Tx_length = APP_Rx_length;
APP_Rx_ptr_out += APP_Rx_length;
APP_Rx_length = 0;
if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE)
{
if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) {
USB_Tx_State = USB_CDC_ZLP;
}
else
{
} else {
USB_Tx_State = USB_CDC_BUSY;
}
}
DCD_EP_Tx (pdev,
CDC_IN_EP,
(uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr],
(uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out],
USB_Tx_length);
APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE;
APP_Rx_length -= USB_Tx_length;
}
}

6
readme.md
View file

@ -1,5 +1,11 @@
# INAV - navigation capable flight controller
# PSA
> INAV no longer accepts targets based on STM32 F411 MCU.
> INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards.
![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
# INAV Community

4
src/main/CMakeLists.txt
View file

@ -525,6 +525,8 @@ main_sources(COMMON_SRC
io/osd_grid.h
io/osd_hud.c
io/osd_hud.h
io/osd_joystick.c
io/osd_joystick.h
io/smartport_master.c
io/smartport_master.h
io/vtx.c
@ -556,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

9
src/main/blackbox/blackbox.c
View file

@ -310,6 +310,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"accVib", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
@ -436,6 +437,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)},
#endif
{"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
{"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
};
typedef enum BlackboxState {
@ -487,6 +489,7 @@ typedef struct blackboxMainState_s {
int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
int16_t accADC[XYZ_AXIS_COUNT];
int16_t accVib;
int16_t attitude[XYZ_AXIS_COUNT];
int32_t debug[DEBUG32_VALUE_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS];
@ -554,6 +557,7 @@ typedef struct blackboxSlowState_s {
int8_t escTemperature;
#endif
uint16_t rxUpdateRate;
uint8_t activeWpNumber;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From rc_controls.c
@ -917,6 +921,7 @@ static void writeIntraframe(void)
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
blackboxWriteUnsignedVB(blackboxCurrent->accVib);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@ -1182,6 +1187,7 @@ static void writeInterframe(void)
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@ -1291,6 +1297,7 @@ static void writeSlowFrame(void)
blackboxWriteSignedVB(slowHistory.escTemperature);
#endif
blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
blackboxSlowFrameIterationTimer = 0;
}
@ -1368,6 +1375,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
#endif
slow->rxUpdateRate = getRcUpdateFrequency();
slow->activeWpNumber = getActiveWpNumber();
}
/**
@ -1601,6 +1609,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
}
}
blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
if (STATE(FIXED_WING_LEGACY)) {

7
src/main/common/fp_pid.c
View file

@ -92,6 +92,11 @@ float navPidApply3(
/* Pre-calculate output and limit it if actuator is saturating */
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
const float outValConstrained = constrainf(outVal, outMin, outMax);
float backCalc = outValConstrained - outVal;//back-calculation anti-windup
if (SIGN(backCalc) == SIGN(pid->integrator)) {
//back calculation anti-windup can only shrink integrator, will not push it to the opposite direction
backCalc = 0.0f;
}
pid->proportional = newProportional;
pid->integral = pid->integrator;
@ -104,7 +109,7 @@ float navPidApply3(
!(pidFlags & PID_ZERO_INTEGRATOR) &&
!(pidFlags & PID_FREEZE_INTEGRATOR)
) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt);
if (pidFlags & PID_SHRINK_INTEGRATOR) {
// Only allow integrator to shrink

7
src/main/common/maths.h
View file

@ -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)
@ -91,6 +91,7 @@
)
#define MIN(a, b) _CHOOSE(<, a, b)
#define MAX(a, b) _CHOOSE(>, a, b)
#define SIGN(a) ((a >= 0) ? 1 : -1)
#define _ABS_II(x, var) \
( __extension__ ({ \

5
src/main/config/parameter_group_ids.h
View file

@ -121,7 +121,10 @@
#define PG_OSD_COMMON_CONFIG 1031
#define PG_TIMER_OVERRIDE_CONFIG 1032
#define PG_EZ_TUNE 1033
#define PG_INAV_END PG_EZ_TUNE
#define PG_LEDPIN_CONFIG 1034
#define PG_OSD_JOYSTICK_CONFIG 1035
#define PG_INAV_END PG_OSD_JOYSTICK_CONFIG
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

4
src/main/drivers/adc_at32f43x.c
View file

@ -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;

2
src/main/drivers/compass/compass_fake.c
View file

@ -37,7 +37,7 @@ static bool fakeMagInit(magDev_t *magDev)
{
UNUSED(magDev);
// initially point north
fakeMagData[X] = 4096;
fakeMagData[X] = 1024;
fakeMagData[Y] = 0;
fakeMagData[Z] = 0;
return true;

88
src/main/drivers/light_ws2811strip.c
View file

@ -43,15 +43,26 @@
#include "drivers/timer.h"
#include "drivers/light_ws2811strip.h"
#include "config/parameter_group_ids.h"
#include "fc/settings.h"
#include "fc/runtime_config.h"
#define WS2811_PERIOD (WS2811_TIMER_HZ / WS2811_CARRIER_HZ)
#define WS2811_BIT_COMPARE_1 ((WS2811_PERIOD * 2) / 3)
#define WS2811_BIT_COMPARE_0 (WS2811_PERIOD / 3)
PG_REGISTER_WITH_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig, PG_LEDPIN_CONFIG, 0);
PG_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig,
.led_pin_pwm_mode = SETTING_LED_PIN_PWM_MODE_DEFAULT
);
static DMA_RAM timerDMASafeType_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
static IO_t ws2811IO = IO_NONE;
static TCH_t * ws2811TCH = NULL;
static bool ws2811Initialised = false;
static bool pwmMode = false;
static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH];
@ -91,6 +102,24 @@ void setStripColors(const hsvColor_t *colors)
}
}
bool ledConfigureDMA(void) {
/* Compute the prescaler value */
uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ;
timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ);
timerPWMConfigChannel(ws2811TCH, 0);
return timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE);
}
void ledConfigurePWM(void) {
timerConfigBase(ws2811TCH, 100, WS2811_TIMER_HZ );
timerPWMConfigChannel(ws2811TCH, 0);
timerPWMStart(ws2811TCH);
timerEnable(ws2811TCH);
pwmMode = true;
}
void ws2811LedStripInit(void)
{
const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
@ -104,28 +133,33 @@ void ws2811LedStripInit(void)
return;
}
/* Compute the prescaler value */
uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ;
ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0);
IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction);
timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ);
timerPWMConfigChannel(ws2811TCH, 0);
if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) {
ledConfigurePWM();
*timerCCR(ws2811TCH) = 0;
} else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) {
ledConfigurePWM();
*timerCCR(ws2811TCH) = 100;
} else {
if (!ledConfigureDMA()) {
// If DMA failed - abort
if (!timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE)) {
ws2811Initialised = false;
return;
}
// Zero out DMA buffer
memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer));
if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_SHARED_HIGH ) {
ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE-1] = 255;
}
ws2811Initialised = true;
ws2811UpdateStrip();
}
}
bool isWS2811LedStripReady(void)
{
@ -140,7 +174,7 @@ STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color)
uint32_t grb = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b);
for (int8_t index = 23; index >= 0; index--) {
ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0;
ledStripDMABuffer[WS2811_DELAY_BUFFER_LENGTH + dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0;
}
}
@ -153,7 +187,7 @@ void ws2811UpdateStrip(void)
static rgbColor24bpp_t *rgb24;
// don't wait - risk of infinite block, just get an update next time round
if (timerPWMDMAInProgress(ws2811TCH)) {
if (pwmMode || timerPWMDMAInProgress(ws2811TCH)) {
return;
}
@ -178,4 +212,40 @@ void ws2811UpdateStrip(void)
timerPWMStartDMA(ws2811TCH);
}
//value
void ledPinStartPWM(uint16_t value) {
if (ws2811TCH == NULL) {
return;
}
if ( !pwmMode ) {
timerPWMStopDMA(ws2811TCH);
//FIXME: implement method to release DMA
ws2811TCH->dma->owner = OWNER_FREE;
ledConfigurePWM();
}
*timerCCR(ws2811TCH) = value;
}
void ledPinStopPWM(void) {
if (ws2811TCH == NULL || !pwmMode ) {
return;
}
if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) {
*timerCCR(ws2811TCH) = 100;
return;
} else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) {
*timerCCR(ws2811TCH) = 0;
return;
}
pwmMode = false;
if (!ledConfigureDMA()) {
ws2811Initialised = false;
}
}
#endif

20
src/main/drivers/light_ws2811strip.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "common/color.h"
#include "config/parameter_group.h"
#define WS2811_LED_STRIP_LENGTH 32
#define WS2811_BITS_PER_LED 24
@ -24,16 +25,33 @@
#define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH)
#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
#define WS2811_DMA_BUFFER_SIZE (WS2811_DELAY_BUFFER_LENGTH + WS2811_DATA_BUFFER_SIZE + 1) // leading bytes (reset low 302us) + data bytes LEDS*3 + 1 byte(keep line high optionally)
#define WS2811_TIMER_HZ 2400000
#define WS2811_CARRIER_HZ 800000
typedef enum {
LED_PIN_PWM_MODE_SHARED_LOW = 0,
LED_PIN_PWM_MODE_SHARED_HIGH = 1,
LED_PIN_PWM_MODE_LOW = 2,
LED_PIN_PWM_MODE_HIGH = 3
} led_pin_pwm_mode_e;
typedef struct ledPinConfig_s {
uint8_t led_pin_pwm_mode; //led_pin_pwm_mode_e
} ledPinConfig_t;
PG_DECLARE(ledPinConfig_t, ledPinConfig);
void ws2811LedStripInit(void);
void ws2811LedStripHardwareInit(void);
void ws2811LedStripDMAEnable(void);
bool ws2811LedStripDMAInProgress(void);
//value 0...100
void ledPinStartPWM(uint16_t value);
void ledPinStopPWM(void);
void ws2811UpdateStrip(void);
void setLedHsv(uint16_t index, const hsvColor_t *color);

7
src/main/drivers/serial_tcp.c
View file

@ -265,13 +265,8 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
{
tcpPort_t *port = (tcpPort_t*)instance;
if (port->isClientConnected) {
UNUSED(instance);
return TCP_MAX_PACKET_SIZE;
} else {
return 0;
}
}
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)

22
src/main/drivers/timer_impl_hal.c
View file

@ -81,13 +81,16 @@ void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz)
TIM_HandleTypeDef * timHandle = tch->timCtx->timHandle;
TIM_TypeDef * timer = tch->timCtx->timDef->tim;
if (timHandle->Instance == timer) {
uint16_t period1 = (period - 1) & 0xffff;
uint16_t prescaler1 = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1;
if (timHandle->Instance == timer && timHandle->Init.Prescaler == prescaler1 && timHandle->Init.Period == period1) {
return;
}
timHandle->Instance = timer;
timHandle->Init.Prescaler = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1;
timHandle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
timHandle->Init.Prescaler = prescaler1;
timHandle->Init.Period = period1; // AKA TIMx_ARR
timHandle->Init.RepetitionCounter = 0;
timHandle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
timHandle->Init.CounterMode = TIM_COUNTERMODE_UP;
@ -565,6 +568,15 @@ void impl_timerPWMStartDMA(TCH_t * tch)
void impl_timerPWMStopDMA(TCH_t * tch)
{
(void)tch;
// FIXME
const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
DMA_TypeDef *dmaBase = tch->dma->dma;
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
LL_DMA_DisableStream(dmaBase, streamLL);
DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
}
tch->dmaState = TCH_DMA_IDLE;
HAL_TIM_Base_Start(tch->timCtx->timHandle);
}

1
src/main/drivers/timer_impl_stdperiph.c
View file

@ -516,5 +516,6 @@ void impl_timerPWMStopDMA(TCH_t * tch)
{
DMA_Cmd(tch->dma->ref, DISABLE);
TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE);
tch->dmaState = TCH_DMA_IDLE;
TIM_Cmd(tch->timHw->tim, ENABLE);
}

2
src/main/drivers/timer_impl_stdperiph_at32.c
View file

@ -404,6 +404,6 @@ void impl_timerPWMStopDMA(TCH_t * tch)
{
dma_channel_enable(tch->dma->ref,FALSE);
tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE);
tch->dmaState = TCH_DMA_IDLE;
tmr_counter_enable(tch->timHw->tim, TRUE);
}

4
src/main/drivers/vtx_common.h
View file

@ -19,7 +19,6 @@
#include "common/time.h"
#define VTX_SETTINGS_NO_BAND 0 // used for custom frequency selection mode
#define VTX_SETTINGS_MIN_BAND 1
#define VTX_SETTINGS_MAX_BAND 5
#define VTX_SETTINGS_MIN_CHANNEL 1
@ -33,9 +32,6 @@
#define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL 1
#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0
#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting
#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
#define VTX_SETTINGS_POWER_COUNT 5

16
src/main/fc/cli.c
View file

@ -68,6 +68,7 @@ bool cliMode = false;
#include "drivers/time.h"
#include "drivers/usb_msc.h"
#include "drivers/vtx_common.h"
#include "drivers/light_ws2811strip.h"
#include "fc/fc_core.h"
#include "fc/cli.h"
@ -1688,6 +1689,20 @@ static void cliModeColor(char *cmdline)
cliPrintLinef("mode_color %u %u %u", modeIdx, funIdx, color);
}
}
static void cliLedPinPWM(char *cmdline)
{
int i;
if (isEmpty(cmdline)) {
ledPinStopPWM();
cliPrintLine("PWM stopped");
} else {
i = fastA2I(cmdline);
ledPinStartPWM(i);
cliPrintLinef("PWM started: %d%%",i);
}
}
#endif
static void cliDelay(char* cmdLine) {
@ -4035,6 +4050,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
#ifdef USE_LED_STRIP
CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed),
CLI_COMMAND_DEF("ledpinpwm", "start/stop PWM on LED pin, 0..100 duty ratio", "[<value>]\r\n", cliLedPinPWM),
#endif
CLI_COMMAND_DEF("map", "configure rc channel order", "[<map>]", cliMap),
CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory),

41
src/main/fc/fc_core.c Executable file → Normal file
View file

@ -548,7 +548,7 @@ void tryArm(void)
if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
ENABLE_ARMING_FLAG(ARMED);
enableFlightMode(TURTLE_MODE);
ENABLE_FLIGHT_MODE(TURTLE_MODE);
return;
}
#endif
@ -678,28 +678,21 @@ void processRx(timeUs_t currentTimeUs)
// Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
bool canUseHorizonMode = true;
if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
/* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD
* MANUAL mode has priority over these modes except when ANGLE auto enabled */
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HORIZON_MODE);
DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) {
if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
} else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
} else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
}
}
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
@ -710,18 +703,14 @@ void processRx(timeUs_t currentTimeUs)
/* Flaperon mode */
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
if (!FLIGHT_MODE(FLAPERON)) {
ENABLE_FLIGHT_MODE(FLAPERON);
}
} else {
DISABLE_FLIGHT_MODE(FLAPERON);
}
/* Turn assistant mode */
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
if (!FLIGHT_MODE(TURN_ASSISTANT)) {
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
}
} else {
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
}
@ -740,9 +729,7 @@ void processRx(timeUs_t currentTimeUs)
#if defined(USE_MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
@ -849,6 +836,8 @@ void processRx(timeUs_t currentTimeUs)
}
}
#endif
// Sound a beeper if the flight mode state has changed
updateFlightModeChangeBeeper();
}
// Function for loop trigger

69
src/main/fc/fc_msp.c
View file

@ -462,9 +462,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, packSensorStatus());
sbufWriteU16(dst, averageSystemLoadPercent);
sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile());
sbufWriteU8(dst, getConfigMixerProfile());
sbufWriteU32(dst, armingFlags);
sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
sbufWriteU8(dst, getConfigMixerProfile());
}
break;
@ -478,7 +478,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
for (int i = 0; i < 3; i++) {
#ifdef USE_MAG
sbufWriteU16(dst, mag.magADC[i]);
sbufWriteU16(dst, lrintf(mag.magADC[i]));
#else
sbufWriteU16(dst, 0);
#endif
@ -2585,6 +2585,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (sbufBytesRemaining(src) > 0) {
vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
}
// //////////////////////////////////////////////////////////
// this code is taken from BF, it's hack for HDZERO VTX MSP frame
// API version 1.42 - this parameter kept separate since clients may already be supplying
if (sbufBytesRemaining(src) >= 2) {
sbufReadU16(src); //skip pitModeFreq
}
// API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
if (sbufBytesRemaining(src) >= 4) {
uint8_t newBand = sbufReadU8(src);
const uint8_t newChannel = sbufReadU8(src);
vtxSettingsConfigMutable()->band = newBand;
vtxSettingsConfigMutable()->channel = newChannel;
}
/* if (sbufBytesRemaining(src) >= 4) {
sbufRead8(src); // freq_l
sbufRead8(src); // freq_h
sbufRead8(src); // band count
sbufRead8(src); // channel count
}*/
// //////////////////////////////////////////////////////////
}
}
}
@ -3680,36 +3703,34 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (dataSize >= 14) {
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
gpsSol.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true;
gpsSol.numSat = sbufReadU8(src);
gpsSolDRV.fixType = sbufReadU8(src);
gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSolDRV.numSat = sbufReadU8(src);
if (gpsSol.fixType != GPS_NO_FIX) {
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = true;
gpsSol.flags.validEPE = true;
gpsSol.flags.validTime = false;
if (gpsSolDRV.fixType != GPS_NO_FIX) {
gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true;
gpsSolDRV.flags.validEPE = true;
gpsSolDRV.flags.validTime = false;
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU32(src);
gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
gpsSol.groundCourse = (int16_t)sbufReadU16(src);
gpsSolDRV.llh.lat = sbufReadU32(src);
gpsSolDRV.llh.lon = sbufReadU32(src);
gpsSolDRV.llh.alt = sbufReadU32(src);
gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);
gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);
gpsSol.eph = 100;
gpsSol.epv = 100;
ENABLE_STATE(GPS_FIX);
gpsSolDRV.eph = 100;
gpsSolDRV.epv = 100;
} else {
sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
}
// Feed data to navigation
gpsProcessNewSolutionData();
gpsProcessNewDriverData();
gpsProcessNewSolutionData(false);
} else {
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
}

5
src/main/fc/fc_msp_box.c
View file

@ -102,6 +102,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 },
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 },
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 },
{ .boxId = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
};
@ -279,6 +280,9 @@ void initActiveBoxIds(void)
if (sensors(SENSOR_BARO)) {
ADD_ACTIVE_BOX(BOXAUTOLEVEL);
}
if (sensors(SENSOR_ACC)) {
ADD_ACTIVE_BOX(BOXANGLEHOLD);
}
}
/*
@ -432,6 +436,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
#endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD);
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
if (activeBoxes[activeBoxIds[i]]) {

5
src/main/fc/fc_tasks.c
View file

@ -62,6 +62,7 @@
#include "io/osd.h"
#include "io/serial.h"
#include "io/rcdevice_cam.h"
#include "io/osd_joystick.h"
#include "io/smartport_master.h"
#include "io/vtx.h"
#include "io/vtx_msp.h"
@ -393,8 +394,12 @@ void fcTasksInit(void)
#endif
#endif
#ifdef USE_RCDEVICE
#ifdef USE_LED_STRIP
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled() || osdJoystickEnabled());
#else
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
#endif
#endif
#ifdef USE_PROGRAMMING_FRAMEWORK
setTaskEnabled(TASK_PROGRAMMING_FRAMEWORK, true);
#endif

2
src/main/fc/multifunction.h
View file

@ -24,6 +24,8 @@
#pragma once
#include <stdbool.h>
#ifdef USE_MULTI_FUNCTIONS
extern uint8_t multiFunctionFlags;

3
src/main/fc/rc_controls.c
View file

@ -119,8 +119,7 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e
value = rcCommand[THROTTLE];
}
const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
bool midThrottle = value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband);
bool midThrottle = value > (reversibleMotorsConfig()->deadband_low) && value < (reversibleMotorsConfig()->deadband_high);
if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) {
return THROTTLE_LOW;
}

1
src/main/fc/rc_modes.h
View file

@ -81,6 +81,7 @@ typedef enum {
BOXMULTIFUNCTION = 52,
BOXMIXERPROFILE = 53,
BOXMIXERTRANSITION = 54,
BOXANGLEHOLD = 55,
CHECKBOX_ITEM_COUNT
} boxId_e;

27
src/main/fc/runtime_config.c
View file

@ -91,31 +91,16 @@ armingFlag_e isArmingDisabledReason(void)
}
/**
* Enables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.
* Called at Rx update rate. Beeper sounded if flight mode state has changed.
*/
uint32_t enableFlightMode(flightModeFlags_e mask)
void updateFlightModeChangeBeeper(void)
{
uint32_t oldVal = flightModeFlags;
static uint32_t previousFlightModeFlags = 0;
flightModeFlags |= (mask);
if (flightModeFlags != oldVal)
if (flightModeFlags != previousFlightModeFlags) {
beeperConfirmationBeeps(1);
return flightModeFlags;
}
/**
* Disables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.
*/
uint32_t disableFlightMode(flightModeFlags_e mask)
{
uint32_t oldVal = flightModeFlags;
flightModeFlags &= ~(mask);
if (flightModeFlags != oldVal)
beeperConfirmationBeeps(1);
return flightModeFlags;
previousFlightModeFlags = flightModeFlags;
}
bool sensors(uint32_t mask)
@ -173,6 +158,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
if (FLIGHT_MODE(HORIZON_MODE))
return FLM_HORIZON;
if (FLIGHT_MODE(ANGLEHOLD_MODE))
return FLM_ANGLEHOLD;
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
}

12
src/main/fc/runtime_config.h
View file

@ -104,12 +104,13 @@ typedef enum {
TURN_ASSISTANT = (1 << 14),
TURTLE_MODE = (1 << 15),
SOARING_MODE = (1 << 16),
ANGLEHOLD_MODE = (1 << 17),
} flightModeFlags_e;
extern uint32_t flightModeFlags;
#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask)
#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask))
#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= (mask))
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
typedef enum {
@ -123,6 +124,9 @@ typedef enum {
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED = (1 << 9),
#ifdef USE_GPS_FIX_ESTIMATION
GPS_ESTIMATED_FIX = (1 << 10),
#endif
NAV_CRUISE_BRAKING = (1 << 11),
NAV_CRUISE_BRAKING_BOOST = (1 << 12),
NAV_CRUISE_BRAKING_LOCKED = (1 << 13),
@ -162,6 +166,7 @@ typedef enum {
FLM_CRUISE,
FLM_LAUNCH,
FLM_FAILSAFE,
FLM_ANGLEHOLD,
FLM_COUNT
} flightModeForTelemetry_e;
@ -200,8 +205,7 @@ extern simulatorData_t simulatorData;
#endif
uint32_t enableFlightMode(flightModeFlags_e mask);
uint32_t disableFlightMode(flightModeFlags_e mask);
void updateFlightModeChangeBeeper(void);
bool sensors(uint32_t mask);
void sensorsSet(uint32_t mask);

122
src/main/fc/settings.yaml Executable file → Normal file
View file

@ -50,7 +50,7 @@ tables:
values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]
enum: sbasMode_e
- name: gps_dyn_model
values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"]
values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"]
enum: gpsDynModel_e
- name: reset_type
values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
@ -193,6 +193,9 @@ tables:
- name: nav_mc_althold_throttle
values: ["STICK", "MID_STICK", "HOVER"]
enum: navMcAltHoldThrottle_e
- name: led_pin_pwm_mode
values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
enum: led_pin_pwm_mode_e
constants:
RPYL_PID_MIN: 0
@ -585,7 +588,7 @@ groups:
members:
- name: pitot_hardware
description: "Selection of pitot hardware."
default_value: "VIRTUAL"
default_value: "NONE"
table: pitot_hardware
- name: pitot_lpf_milli_hz
min: 0
@ -835,6 +838,12 @@ groups:
default_value: 0
min: -1
max: 600
- name: failsafe_gps_fix_estimation_delay
description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation."
condition: USE_GPS_FIX_ESTIMATION
default_value: 7
min: -1
max: 600
- name: PG_LIGHTS_CONFIG
type: lightsConfig_t
@ -1460,6 +1469,12 @@ groups:
default_value: ADAPTIVE
field: inertia_comp_method
table: imu_inertia_comp_method
- name: ahrs_gps_yaw_weight
description: "Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass"
default_value: 100
field: gps_yaw_weight
min: 0
max: 500
- name: PG_ARMING_CONFIG
type: armingConfig_t
@ -1605,8 +1620,8 @@ groups:
table: gps_sbas_mode
type: uint8_t
- name: gps_dyn_model
description: "GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying."
default_value: "AIR_1G"
description: "GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying."
default_value: "AIR_2G"
field: dynModel
table: gps_dyn_model
type: uint8_t
@ -1727,7 +1742,7 @@ groups:
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_cd_pitch
description: "Multicopter Control Derivative gain for PITCH"
description: "Multicopter Control Derivative gain for PITCH (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough."
default_value: 60
field: bank_mc.pid[PID_PITCH].FF
min: RPYL_PID_MIN
@ -1751,7 +1766,7 @@ groups:
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_cd_roll
description: "Multicopter Control Derivative gain for ROLL"
description: "Multicopter Control Derivative gain for ROLL (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough."
default_value: 60
field: bank_mc.pid[PID_ROLL].FF
min: RPYL_PID_MIN
@ -1775,7 +1790,7 @@ groups:
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_cd_yaw
description: "Multicopter Control Derivative gain for YAW"
description: "Multicopter Control Derivative gain for YAW (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough."
default_value: 60
field: bank_mc.pid[PID_YAW].FF
min: RPYL_PID_MIN
@ -2192,7 +2207,7 @@ groups:
table: pidTypeTable
default_value: AUTO
- name: mc_cd_lpf_hz
description: "Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky"
description: "Cutoff frequency for Control Derivative. This controls the cutoff for the LPF that is applied to the CD (Feed Forward) signal to the PID controller. Lower value will produce a smoother CD gain to the controller, but it will be more delayed. Higher values will produce CD gain that may have more noise in the signal depending on your RC link but wil be less delayed."
default_value: 30
field: controlDerivativeLpfHz
min: 0
@ -2282,6 +2297,12 @@ groups:
default_value: OFF
field: allow_dead_reckoning
type: bool
- name: inav_allow_gps_fix_estimation
description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay."
condition: USE_GPS_FIX_ESTIMATION
default_value: OFF
field: allow_gps_fix_estimation
type: bool
- name: inav_reset_altitude
description: "Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming)"
default_value: "FIRST_ARM"
@ -2319,23 +2340,23 @@ groups:
max: 100
default_value: 2.0
- name: inav_w_z_baro_p
description: "Weight of barometer measurements in estimated altitude and climb rate"
description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors."
field: w_z_baro_p
min: 0
max: 10
default_value: 0.35
default_value: 0.4
- name: inav_w_z_gps_p
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes"
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
field: w_z_gps_p
min: 0
max: 10
default_value: 0.2
default_value: 0.4
- name: inav_w_z_gps_v
description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
field: w_z_gps_v
min: 0
max: 10
default_value: 0.1
default_value: 0.8
- name: inav_w_xy_gps_p
description: "Weight of GPS coordinates in estimated UAV position and speed."
default_value: 1.0
@ -2360,11 +2381,6 @@ groups:
field: w_xy_res_v
min: 0
max: 10
- name: inav_w_xyz_acc_p
field: w_xyz_acc_p
min: 0
max: 1
default_value: 1.0
- name: inav_w_acc_bias
description: "Weight for accelerometer drift estimation"
default_value: 0.01
@ -2483,6 +2499,12 @@ groups:
field: general.auto_speed
min: 10
max: 2000
- name: nav_min_ground_speed
description: "Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s."
default_value: 7
field: general.min_ground_speed
min: 6
max: 50
- name: nav_max_auto_speed
description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]"
default_value: 1000
@ -2577,6 +2599,12 @@ groups:
default_value: "ALWAYS"
field: general.flags.rth_allow_landing
table: nav_rth_allow_landing
- name: nav_rth_fs_landing_delay
description: "If landing is active on Failsafe and this is above 0. The aircraft will hover or loiter for X seconds before performing the landing. If the battery enters the warning or critical levels, the land will proceed. Default = 0 [seconds]"
default_value: 0
min: 0
max: 1800
field: general.rth_fs_landing_delay
- name: nav_rth_alt_mode
description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details"
default_value: "AT_LEAST"
@ -2655,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
@ -3813,10 +3841,10 @@ groups:
condition: USE_VTX_SMARTAUDIO || USE_VTX_TRAMP
members:
- name: vtx_band
description: "Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race."
description: "Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race."
default_value: 1
field: band
min: VTX_SETTINGS_NO_BAND
min: VTX_SETTINGS_MIN_BAND
max: VTX_SETTINGS_MAX_BAND
- name: vtx_channel
description: "Channel to use within the configured `vtx_band`. Valid values are [1, 8]."
@ -4025,3 +4053,55 @@ groups:
default_value: 1.2
field: attnFilterCutoff
max: 100
- name: PG_LEDPIN_CONFIG
type: ledPinConfig_t
headers: ["drivers/light_ws2811strip.h"]
members:
- name: led_pin_pwm_mode
condition: USE_LED_STRIP
description: "PWM mode of LED pin."
default_value: "SHARED_LOW"
field: led_pin_pwm_mode
table: led_pin_pwm_mode
- name: PG_OSD_JOYSTICK_CONFIG
type: osdJoystickConfig_t
headers: ["io/osd_joystick.h"]
condition: USE_RCDEVICE && USE_LED_STRIP
members:
- name: osd_joystick_enabled
description: "Enable OSD Joystick emulation"
default_value: OFF
field: osd_joystick_enabled
type: bool
- name: osd_joystick_down
description: "PWM value for DOWN key"
default_value: 0
field: osd_joystick_down
min: 0
max: 100
- name: osd_joystick_up
description: "PWM value for UP key"
default_value: 48
field: osd_joystick_up
min: 0
max: 100
- name: osd_joystick_left
description: "PWM value for LEFT key"
default_value: 63
field: osd_joystick_left
min: 0
max: 100
- name: osd_joystick_right
description: "PWM value for RIGHT key"
default_value: 28
field: osd_joystick_right
min: 0
max: 100
- name: osd_joystick_enter
description: "PWM value for ENTER key"
default_value: 75
field: osd_joystick_enter
min: 0
max: 100

47
src/main/flight/failsafe.c
View file

@ -82,6 +82,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default
.failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
.failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s)
#ifdef USE_GPS_FIX_ESTIMATION
.failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
#endif
);
typedef enum {
@ -350,7 +353,13 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 && sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
if (failsafeConfig()->failsafe_min_distance > 0 &&
((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME)) {
// get the distance to the original arming point
uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition);
if (distance < failsafeConfig()->failsafe_min_distance) {
@ -362,6 +371,28 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
return failsafeConfig()->failsafe_procedure;
}
#ifdef USE_GPS_FIX_ESTIMATION
bool checkGPSFixFailsafe(void)
{
if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) {
if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) {
failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis();
} else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) {
if ( !posControl.flags.forcedRTHActivated ) {
failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH);
failsafeActivate(FAILSAFE_RETURN_TO_HOME);
activateForcedRTH();
return true;
}
}
} else {
failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0;
}
return false;
}
#endif
void failsafeUpdateState(void)
{
if (!failsafeIsMonitoring() || failsafeIsSuspended()) {
@ -390,6 +421,12 @@ void failsafeUpdateState(void)
if (!throttleStickIsLow()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
#ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) {
reprocessState = true;
} else
#endif
if (!receivingRxDataAndNotFailsafeMode) {
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
@ -459,6 +496,14 @@ void failsafeUpdateState(void)
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
}
#ifdef USE_GPS_FIX_ESTIMATION
else {
if ( checkGPSFixFailsafe() ) {
reprocessState = true;
}
}
#endif
break;
case FAILSAFE_RETURN_TO_HOME:

6
src/main/flight/failsafe.h
View file

@ -43,6 +43,9 @@ typedef struct failsafeConfig_s {
uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s)
#ifdef USE_GPS_FIX_ESTIMATION
int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
#endif
} failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig);
@ -149,6 +152,9 @@ typedef struct failsafeState_s {
timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData
timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time
#ifdef USE_GPS_FIX_ESTIMATION
timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation
#endif
failsafeProcedure_e activeProcedure;
failsafePhase_e phase;
failsafeRxLinkState_e rxLinkState;

90
src/main/flight/imu.c
View file

@ -49,6 +49,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/mixer_profile.h"
#include "flight/pid.h"
#if defined(USE_WIND_ESTIMATOR)
#include "flight/wind_estimator.h"
@ -77,6 +78,9 @@
#define SPIN_RATE_LIMIT 20
#define MAX_ACC_NEARNESS 0.2 // 20% or G error soft-accepted (0.8-1.2G)
#define MAX_MAG_NEARNESS 0.25 // 25% or magnetic field error soft-accepted (0.75-1.25)
#define COS10DEG 0.985f
#define COS20DEG 0.940f
#define IMU_ROTATION_LPF 3 // Hz
FASTRAM fpVector3_t imuMeasuredAccelBF;
FASTRAM fpVector3_t imuMeasuredRotationBF;
@ -111,6 +115,8 @@ FASTRAM bool gpsHeadingInitialized;
FASTRAM bool imuUpdated = false;
static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF);
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
@ -122,7 +128,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
.acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
.gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
.inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
.inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT,
.gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT
);
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@ -323,7 +330,20 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
bool isGPSTrustworthy(void)
{
return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
;
}
static float imuCalculateMcCogWeight(void)
{
float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f;
wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
return wCoG;
}
static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
@ -339,11 +359,15 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
/* Step 1: Yaw correction */
// Use measured magnetic field vector
if (magBF || useCOG) {
static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
float wMag = 1.0f;
float wCoG = 1.0f;
if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;}
fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
if (magBF && vectorNormSquared(magBF) > 0.01f) {
wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS);
fpVector3_t vMag;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
@ -369,13 +393,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
// Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth);
// Rotate error back into body frame
quaternionRotateVector(&vErr, &vErr, &orientation);
quaternionRotateVector(&vMagErr, &vMagErr, &orientation);
}
}
else if (useCOG) {
if (useCOG) {
fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
//vForward as trust vector
if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
vForward.z = 1.0f;
}else{
vForward.x = 1.0f;
}
fpVector3_t vHeadingEF;
// Use raw heading error (from GPS or whatever else)
@ -386,6 +417,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
// (Rxx; Ryx) - measured (estimated) heading vector (EF)
// (-cos(COG), sin(COG)) - reference heading vector (EF)
float airSpeed = gpsSol.groundSpeed;
// Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards.
fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
#if defined(USE_WIND_ESTIMATOR)
@ -395,12 +427,21 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed);
vCoG.x += getEstimatedWindSpeed(X);
vCoG.y -= getEstimatedWindSpeed(Y);
airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG));
vectorNormalize(&vCoG, &vCoG);
}
#endif
wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
// Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
if (STATE(MULTIROTOR)){
//when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
wCoG *= imuCalculateMcCogWeight();
//scale accroading to multirotor`s tilt angle
wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f);
//for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed
}
vHeadingEF.z = 0.0f;
// We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero
@ -409,13 +450,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
vectorNormalize(&vHeadingEF, &vHeadingEF);
// error is cross product between reference heading and estimated heading (calculated in EF)
vectorCrossProduct(&vErr, &vCoG, &vHeadingEF);
vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF);
// Rotate error back into body frame
quaternionRotateVector(&vErr, &vErr, &orientation);
quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation);
}
}
fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
vectorScale(&vMagErr, &vMagErr, wMag);
vectorScale(&vCoGErr, &vCoGErr, wCoG);
vectorAdd(&vErr, &vMagErr, &vCoGErr);
// Compute and apply integral feedback if enabled
if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
// Stop integrating if spinning beyond the certain limit
@ -535,10 +579,10 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
}
}
static float imuCalculateAccelerometerWeightNearness(void)
static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF)
{
fpVector3_t accBFNorm;
vectorScale(&accBFNorm, &compansatedGravityBF, 1.0f / GRAVITY_CMSS);
vectorScale(&accBFNorm, accBF, 1.0f / GRAVITY_CMSS);
const float accMagnitudeSq = vectorNormSquared(&accBFNorm);
const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
return accWeight_Nearness;
@ -672,20 +716,20 @@ static void imuCalculateEstimatedAttitude(float dT)
bool useMag = false;
bool useCOG = false;
#if defined(USE_GPS)
if (STATE(FIXED_WING_LEGACY)) {
bool canUseCOG = isGPSHeadingValid();
// Prefer compass (if available)
// Use compass (if available)
if (canUseMAG) {
useMag = true;
gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails
}
else if (canUseCOG) {
// Use GPS (if available)
if (canUseCOG) {
if (gpsHeadingInitialized) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
}
else {
else if (!canUseMAG) {
// Re-initialize quaternion from known Roll, Pitch and GPS heading
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
gpsHeadingInitialized = true;
@ -696,13 +740,6 @@ static void imuCalculateEstimatedAttitude(float dT)
} else if (!ARMING_FLAG(ARMED)) {
gpsHeadingInitialized = false;
}
}
else {
// Multicopters don't use GPS heading
if (canUseMAG) {
useMag = true;
}
}
imuCalculateFilters(dT);
// centrifugal force compensation
@ -751,7 +788,7 @@ static void imuCalculateEstimatedAttitude(float dT)
}
compansatedGravityBF = imuMeasuredAccelBF
#endif
float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness();
float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
const bool useAcc = (accWeight > 0.001f);
@ -807,6 +844,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
}
}
bool isImuReady(void)
{
return sensors(SENSOR_ACC) && STATE(ACCELEROMETER_CALIBRATED) && gyroIsCalibrationComplete();
@ -814,7 +852,7 @@ bool isImuReady(void)
bool isImuHeadingValid(void)
{
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized;
}
float calculateCosTiltAngle(void)

1
src/main/flight/imu.h
View file

@ -54,6 +54,7 @@ typedef struct imuConfig_s {
uint8_t acc_ignore_slope;
uint8_t gps_yaw_windcomp;
uint8_t inertia_comp_method;
uint16_t gps_yaw_weight;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);

75
src/main/flight/pid.c
View file

@ -159,6 +159,8 @@ typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
#define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f
@ -1064,9 +1066,62 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
}
bool isAngleHoldLevel(void)
{
return angleHoldIsLevel;
}
void updateAngleHold(float *angleTarget, uint8_t axis)
{
int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
if (!restartAngleHoldMode) { // set restart flag when anglehold is inactive
restartAngleHoldMode = !FLIGHT_MODE(ANGLEHOLD_MODE) && navAngleHoldAxis == -1;
}
if ((FLIGHT_MODE(ANGLEHOLD_MODE) || axis == navAngleHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) {
/* angleHoldTarget stores attitude values using a zero datum when level.
* This requires angleHoldTarget pitch to be corrected for fixedWingLevelTrim so it is 0
* when the craft is level even though attitude pitch is non zero in this case.
* angleTarget pitch is corrected back to fixedWingLevelTrim datum on return from function */
static int16_t angleHoldTarget[2];
if (restartAngleHoldMode) { // set target attitude to current attitude on activation
angleHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
angleHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
restartAngleHoldMode = false;
}
// set flag indicating anglehold is level
if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
angleHoldIsLevel = angleHoldTarget[FD_ROLL] == 0 && angleHoldTarget[FD_PITCH] == 0;
} else {
angleHoldIsLevel = angleHoldTarget[navAngleHoldAxis] == 0;
}
uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
// use Nav bank angle limits if Nav active
if (navAngleHoldAxis == FD_ROLL) {
bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle);
} else if (navAngleHoldAxis == FD_PITCH) {
bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
}
int16_t levelTrim = axis == FD_PITCH ? DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : 0;
if (calculateRollPitchCenterStatus() == CENTERED) {
angleHoldTarget[axis] = ABS(angleHoldTarget[axis]) < 30 ? 0 : angleHoldTarget[axis]; // snap to level when within 3 degs of level
*angleTarget = constrain(angleHoldTarget[axis] - levelTrim, -bankLimit, bankLimit);
} else {
*angleTarget = constrain(attitude.raw[axis] + *angleTarget + levelTrim, -bankLimit, bankLimit);
angleHoldTarget[axis] = attitude.raw[axis] + levelTrim;
}
}
}
void FAST_CODE pidController(float dT)
{
const float dT_inv = 1.0f / dT;
if (!pidFiltersConfigured) {
@ -1115,21 +1170,30 @@ void FAST_CODE pidController(float dT)
#endif
}
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
const float horizonRateMagnitude = calcHorizonRateMagnitude();
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
levelingEnabled = false;
angleHoldIsLevel = false;
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
// If axis angle override, get the correct angle from Logic Conditions
float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
if (STATE(AIRPLANE)) { // update anglehold mode
updateAngleHold(&angleTarget, axis);
}
// Apply the Level PID controller
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
} else {
restartAngleHoldMode = true;
}
}
// Apply Turn Assistance
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
@ -1137,6 +1201,7 @@ void FAST_CODE pidController(float dT)
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT
}
// Apply FPV camera mix
if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) {
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
}
@ -1272,7 +1337,7 @@ bool isFixedWingLevelTrimActive(void)
return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
!FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
!navigationIsControllingAltitude();
!navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
}
void updateFixedWingLevelTrim(timeUs_t currentTimeUs)

1
src/main/flight/pid.h
View file

@ -220,3 +220,4 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex);
bool isFixedWingLevelTrimActive(void);
void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
float getFixedWingLevelTrim(void);
bool isAngleHoldLevel(void);

28
src/main/flight/servos.c
View file

@ -385,20 +385,6 @@ void servoMixer(float dT)
servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100;
}
/*
* When not armed, apply servo low position to all outputs that include a throttle or stabilized throttle in the mix
*/
if (!ARMING_FLAG(ARMED)) {
for (int i = 0; i < servoRuleCount; i++) {
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t from = currentServoMixer[i].inputSource;
if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) {
servo[target] = motorConfig()->mincommand;
}
}
}
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
/*
@ -429,6 +415,20 @@ void servoMixer(float dT)
*/
servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max);
}
/*
* When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix
*/
if (!ARMING_FLAG(ARMED)) {
for (int i = 0; i < servoRuleCount; i++) {
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t from = currentServoMixer[i].inputSource;
if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) {
servo[target] = motorConfig()->mincommand;
}
}
}
}
#define SERVO_AUTOTRIM_TIMER_MS 2000

15
src/main/flight/wind_estimator.c
View file

@ -52,7 +52,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT];
bool isEstimatedWindSpeedValid(void)
{
return hasValidWindEstimate;
return hasValidWindEstimate
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX) //use any wind estimate with GPS fix estimation.
#endif
;
}
float getEstimatedWindSpeed(int axis)
@ -83,15 +87,18 @@ void updateWindEstimator(timeUs_t currentTimeUs)
static float lastValidEstimateAltitude = 0.0f;
float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT)
{
if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) {
hasValidWindEstimate = false;
}
if (!STATE(FIXED_WING_LEGACY) ||
!isGPSHeadingValid() ||
!gpsSol.flags.validVelNE ||
!gpsSol.flags.validVelD) {
!gpsSol.flags.validVelD
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
return;
}

250
src/main/io/gps.c
View file

@ -45,8 +45,12 @@
#include "fc/runtime_config.h"
#endif
#include "fc/rc_modes.h"
#include "sensors/sensors.h"
#include "sensors/compass.h"
#include "sensors/barometer.h"
#include "sensors/pitotmeter.h"
#include "io/serial.h"
#include "io/gps.h"
@ -54,6 +58,7 @@
#include "io/gps_ublox.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "config/feature.h"
@ -61,6 +66,13 @@
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/imu.h"
#include "flight/wind_estimator.h"
#include "flight/pid.h"
#include "flight/mixer.h"
#include "programming/logic_condition.h"
typedef struct {
bool isDriverBased;
portMode_t portMode; // Port mode RX/TX (only for serial based)
@ -71,7 +83,13 @@ typedef struct {
// GPS public data
gpsReceiverData_t gpsState;
gpsStatistics_t gpsStats;
gpsSolutionData_t gpsSol;
//it is not safe to access gpsSolDRV which is filled in gps thread by driver.
//gpsSolDRV can be accessed only after driver signalled that data is ready
//we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features
//and use it in the rest of code.
gpsSolutionData_t gpsSolDRV; //filled by driver
gpsSolutionData_t gpsSol; //used in the rest of the code
// Map gpsBaudRate_e index to baudRate_e
baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 };
@ -203,8 +221,158 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
gpsState.timeoutMs = timeoutMs;
}
void gpsProcessNewSolutionData(void)
#ifdef USE_GPS_FIX_ESTIMATION
bool canEstimateGPSFix(void)
{
#if defined(USE_GPS) && defined(USE_BARO)
//we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because:
//1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once
//2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix
return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) &&
sensors(SENSOR_BARO) && baroIsHealthy() &&
ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME);
#else
return false;
#endif
}
#endif
#ifdef USE_GPS_FIX_ESTIMATION
void processDisableGPSFix(void)
{
static int32_t last_lat = 0;
static int32_t last_lon = 0;
static int32_t last_alt = 0;
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) {
gpsSol.fixType = GPS_NO_FIX;
gpsSol.hdop = 9999;
gpsSol.numSat = 0;
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSol.flags.validEPE = false;
gpsSol.flags.validTime = false;
//freeze coordinates
gpsSol.llh.lat = last_lat;
gpsSol.llh.lon = last_lon;
gpsSol.llh.alt = last_alt;
} else {
last_lat = gpsSol.llh.lat;
last_lon = gpsSol.llh.lon;
last_alt = gpsSol.llh.alt;
}
}
#endif
#ifdef USE_GPS_FIX_ESTIMATION
//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition"
void updateEstimatedGPSFix(void)
{
static uint32_t lastUpdateMs = 0;
static int32_t estimated_lat = 0;
static int32_t estimated_lon = 0;
static int32_t estimated_alt = 0;
uint32_t t = millis();
int32_t dt = t - lastUpdateMs;
lastUpdateMs = t;
bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats;
if (sensorHasFix || !canEstimateGPSFix()) {
estimated_lat = gpsSol.llh.lat;
estimated_lon = gpsSol.llh.lon;
estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
return;
}
gpsSol.fixType = GPS_FIX_3D;
gpsSol.hdop = 99;
gpsSol.numSat = 99;
gpsSol.eph = 100;
gpsSol.epv = 100;
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = false; //do not provide velocity.z
gpsSol.flags.validEPE = true;
gpsSol.flags.validTime = false;
float speed = pidProfile()->fixedWingReferenceAirspeed;
#ifdef USE_PITOT
if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
speed = getAirspeedEstimate();
}
#endif
float velX = rMat[0][0] * speed;
float velY = -rMat[1][0] * speed;
// here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame
if (isEstimatedWindSpeedValid()) {
velX += getEstimatedWindSpeed(X);
velY += getEstimatedWindSpeed(Y);
}
// here (velX, velY) is estimated horizontal speed with wind influence = ground speed
if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent(false) == 0))) {
velX = 0;
velY = 0;
}
estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) );
estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) );
estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
gpsSol.llh.lat = estimated_lat;
gpsSol.llh.lon = estimated_lon;
gpsSol.llh.alt = estimated_alt;
gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY);
float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction
if (groundCourse < 0) {
groundCourse += 2 * M_PIf;
}
gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse);
gpsSol.velNED[X] = (int16_t)(velX);
gpsSol.velNED[Y] = (int16_t)(velY);
gpsSol.velNED[Z] = 0;
}
#endif
void gpsProcessNewDriverData(void)
{
gpsSol = gpsSolDRV;
#ifdef USE_GPS_FIX_ESTIMATION
processDisableGPSFix();
updateEstimatedGPSFix();
#endif
}
//called after:
//1)driver copies gpsSolDRV to gpsSol
//2)gpsSol is processed by "Disable GPS logical switch"
//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix()
//On GPS sensor timeout - called after updateEstimatedGPSFix()
void gpsProcessNewSolutionData(bool timeout)
{
#ifdef USE_GPS_FIX_ESTIMATION
if ( gpsSol.numSat == 99 ) {
ENABLE_STATE(GPS_ESTIMATED_FIX);
DISABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_ESTIMATED_FIX);
#endif
// Set GPS fix flag only if we have 3D fix
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
ENABLE_STATE(GPS_FIX);
@ -216,9 +384,14 @@ void gpsProcessNewSolutionData(void)
gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX);
}
#ifdef USE_GPS_FIX_ESTIMATION
}
#endif
// Set sensor as ready and available
if (!timeout) {
// Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix)
sensorsSet(SENSOR_GPS);
}
// Pass on GPS update to NAV and IMU
onNewGPSData();
@ -237,20 +410,35 @@ void gpsProcessNewSolutionData(void)
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
}
static void gpsResetSolution(void)
static void gpsResetSolution(gpsSolutionData_t* gpsSol)
{
gpsSol.eph = 9999;
gpsSol.epv = 9999;
gpsSol.numSat = 0;
gpsSol.hdop = 9999;
gpsSol->eph = 9999;
gpsSol->epv = 9999;
gpsSol->numSat = 0;
gpsSol->hdop = 9999;
gpsSol.fixType = GPS_NO_FIX;
gpsSol->fixType = GPS_NO_FIX;
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSol.flags.validMag = false;
gpsSol.flags.validEPE = false;
gpsSol.flags.validTime = false;
gpsSol->flags.validVelNE = false;
gpsSol->flags.validVelD = false;
gpsSol->flags.validEPE = false;
gpsSol->flags.validTime = false;
}
void gpsTryEstimateOnTimeout(void)
{
gpsResetSolution(&gpsSol);
DISABLE_STATE(GPS_FIX);
#ifdef USE_GPS_FIX_ESTIMATION
if ( canEstimateGPSFix() ) {
updateEstimatedGPSFix();
if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in
gpsProcessNewSolutionData(true);
}
}
#endif
}
void gpsPreInit(void)
@ -269,7 +457,8 @@ void gpsInit(void)
gpsStats.timeouts = 0;
// Reset solution, timeout and prepare to start
gpsResetSolution();
gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
gpsSetState(GPS_UNKNOWN);
@ -345,27 +534,21 @@ bool gpsUpdate(void)
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT))
{
if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) {
gpsSetState(GPS_LOST_COMMUNICATION);
sensorsClear(SENSOR_GPS);
gpsStats.timeouts = 5;
return false;
}
else
{
gpsTryEstimateOnTimeout();
} else {
gpsSetState(GPS_RUNNING);
sensorsSet(SENSOR_GPS);
}
bool res = gpsSol.flags.hasNewData;
gpsSol.flags.hasNewData = false;
return res;
}
}
#endif
// Assume that we don't have new data this run
gpsSol.flags.hasNewData = false;
switch (gpsState.state) {
default:
case GPS_INITIALIZING:
@ -373,10 +556,9 @@ bool gpsUpdate(void)
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
// Reset internals
DISABLE_STATE(GPS_FIX);
gpsSol.fixType = GPS_NO_FIX;
// Reset solution
gpsResetSolution();
gpsResetSolution(&gpsSolDRV);
// Call GPS protocol reset handler
gpsProviders[gpsState.gpsConfig->provider].restart();
@ -406,7 +588,13 @@ bool gpsUpdate(void)
break;
}
return gpsSol.flags.hasNewData;
if ( !sensors(SENSOR_GPS) ) {
gpsTryEstimateOnTimeout();
}
bool res = gpsSol.flags.hasNewData;
gpsSol.flags.hasNewData = false;
return res;
}
void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
@ -453,7 +641,11 @@ bool isGPSHealthy(void)
bool isGPSHeadingValid(void)
{
return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
return ((STATE(GPS_FIX) && gpsSol.numSat >= 6)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed >= 300;
}
#endif

4
src/main/io/gps.h
View file

@ -76,7 +76,9 @@ typedef enum {
typedef enum {
GPS_DYNMODEL_PEDESTRIAN = 0,
GPS_DYNMODEL_AUTOMOTIVE,
GPS_DYNMODEL_AIR_1G,
GPS_DYNMODEL_AIR_2G,
GPS_DYNMODEL_AIR_4G,
} gpsDynModel_e;
@ -124,7 +126,6 @@ typedef struct gpsSolutionData_s {
bool gpsHeartbeat; // Toggle each update
bool validVelNE;
bool validVelD;
bool validMag;
bool validEPE; // EPH/EPV values are valid - actual accuracy
bool validTime;
} flags;
@ -133,7 +134,6 @@ typedef struct gpsSolutionData_s {
uint8_t numSat;
gpsLocation_t llh;
int16_t magData[3];
int16_t velNED[3];
int16_t groundSpeed;

54
src/main/io/gps_fake.c
View file

@ -31,6 +31,7 @@
#include "platform.h"
#include "build/build_config.h"
#include "common/log.h"
#if defined(USE_GPS_FAKE)
@ -46,7 +47,7 @@ void gpsFakeRestart(void)
void gpsFakeHandle(void)
{
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
}
void gpsFakeSet(
@ -62,37 +63,38 @@ void gpsFakeSet(
int16_t velNED_Z,
time_t time)
{
gpsSol.fixType = fixType;
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.numSat = numSat;
gpsSolDRV.fixType = fixType;
gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSolDRV.numSat = numSat;
gpsSol.llh.lat = lat;
gpsSol.llh.lon = lon;
gpsSol.llh.alt = alt;
gpsSol.groundSpeed = groundSpeed;
gpsSol.groundCourse = groundCourse;
gpsSol.velNED[X] = velNED_X;
gpsSol.velNED[Y] = velNED_Y;
gpsSol.velNED[Z] = velNED_Z;
gpsSol.eph = 100;
gpsSol.epv = 100;
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = true;
gpsSol.flags.validEPE = true;
gpsSol.flags.hasNewData = true;
gpsSolDRV.llh.lat = lat;
gpsSolDRV.llh.lon = lon;
gpsSolDRV.llh.alt = alt;
gpsSolDRV.groundSpeed = groundSpeed;
gpsSolDRV.groundCourse = groundCourse;
gpsSolDRV.velNED[X] = velNED_X;
gpsSolDRV.velNED[Y] = velNED_Y;
gpsSolDRV.velNED[Z] = velNED_Z;
gpsSolDRV.eph = 100;
gpsSolDRV.epv = 100;
gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true;
gpsSolDRV.flags.validEPE = true;
if (time) {
struct tm* gTime = gmtime(&time);
gpsSol.time.year = (uint16_t)(gTime->tm_year + 1900);
gpsSol.time.month = (uint16_t)(gTime->tm_mon + 1);
gpsSol.time.day = (uint8_t)gTime->tm_mday;
gpsSol.time.hours = (uint8_t)gTime->tm_hour;
gpsSol.time.minutes = (uint8_t)gTime->tm_min;
gpsSol.time.seconds = (uint8_t)gTime->tm_sec;
gpsSol.time.millis = 0;
gpsSol.flags.validTime = gpsSol.fixType >= 3;
gpsSolDRV.time.year = (uint16_t)(gTime->tm_year + 1900);
gpsSolDRV.time.month = (uint16_t)(gTime->tm_mon + 1);
gpsSolDRV.time.day = (uint8_t)gTime->tm_mday;
gpsSolDRV.time.hours = (uint8_t)gTime->tm_hour;
gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min;
gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec;
gpsSolDRV.time.millis = 0;
gpsSolDRV.flags.validTime = gpsSol.fixType >= 3;
}
gpsProcessNewDriverData();
}
#endif

51
src/main/io/gps_msp.c
View file

@ -63,7 +63,7 @@ void gpsRestartMSP(void)
void gpsHandleMSP(void)
{
if (newDataReady) {
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
newDataReady = false;
}
}
@ -81,33 +81,34 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr)
{
const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
gpsSol.fixType = gpsMapFixType(pkt->fixType);
gpsSol.numSat = pkt->satellitesInView;
gpsSol.llh.lon = pkt->longitude;
gpsSol.llh.lat = pkt->latitude;
gpsSol.llh.alt = pkt->mslAltitude;
gpsSol.velNED[X] = pkt->nedVelNorth;
gpsSol.velNED[Y] = pkt->nedVelEast;
gpsSol.velNED[Z] = pkt->nedVelDown;
gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10
gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
gpsSol.hdop = gpsConstrainHDOP(pkt->hdop);
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = true;
gpsSol.flags.validEPE = true;
gpsSolDRV.fixType = gpsMapFixType(pkt->fixType);
gpsSolDRV.numSat = pkt->satellitesInView;
gpsSolDRV.llh.lon = pkt->longitude;
gpsSolDRV.llh.lat = pkt->latitude;
gpsSolDRV.llh.alt = pkt->mslAltitude;
gpsSolDRV.velNED[X] = pkt->nedVelNorth;
gpsSolDRV.velNED[Y] = pkt->nedVelEast;
gpsSolDRV.velNED[Z] = pkt->nedVelDown;
gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
gpsSolDRV.groundCourse = pkt->groundCourse / 10; // in deg * 10
gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop);
gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true;
gpsSolDRV.flags.validEPE = true;
gpsSol.time.year = pkt->year;
gpsSol.time.month = pkt->month;
gpsSol.time.day = pkt->day;
gpsSol.time.hours = pkt->hour;
gpsSol.time.minutes = pkt->min;
gpsSol.time.seconds = pkt->sec;
gpsSol.time.millis = 0;
gpsSolDRV.time.year = pkt->year;
gpsSolDRV.time.month = pkt->month;
gpsSolDRV.time.day = pkt->day;
gpsSolDRV.time.hours = pkt->hour;
gpsSolDRV.time.minutes = pkt->min;
gpsSolDRV.time.seconds = pkt->sec;
gpsSolDRV.time.millis = 0;
gpsSol.flags.validTime = (pkt->fixType >= 3);
gpsSolDRV.flags.validTime = (pkt->fixType >= 3);
gpsProcessNewDriverData();
newDataReady = true;
}
#endif

4
src/main/io/gps_private.h
View file

@ -61,6 +61,7 @@ typedef struct {
} gpsReceiverData_t;
extern gpsReceiverData_t gpsState;
extern gpsSolutionData_t gpsSolDRV;
extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT];
@ -70,7 +71,8 @@ extern void gpsFinalizeChangeBaud(void);
extern uint16_t gpsConstrainEPE(uint32_t epe);
extern uint16_t gpsConstrainHDOP(uint32_t hdop);
void gpsProcessNewSolutionData(void);
void gpsProcessNewDriverData(void);
void gpsProcessNewSolutionData(bool);
void gpsSetProtocolTimeout(timeMs_t timeoutMs);
extern void gpsRestartUBLOX(void);

122
src/main/io/gps_ublox.c
View file

@ -538,84 +538,84 @@ static bool gpsParseFrameUBLOX(void)
{
switch (_msg_id) {
case MSG_POSLLH:
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
gpsSol.flags.validEPE = true;
gpsSolDRV.llh.lon = _buffer.posllh.longitude;
gpsSolDRV.llh.lat = _buffer.posllh.latitude;
gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
gpsSolDRV.flags.validEPE = true;
if (next_fix_type != GPS_NO_FIX)
gpsSol.fixType = next_fix_type;
gpsSolDRV.fixType = next_fix_type;
_new_position = true;
break;
case MSG_STATUS:
next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
if (next_fix_type == GPS_NO_FIX)
gpsSol.fixType = GPS_NO_FIX;
gpsSolDRV.fixType = GPS_NO_FIX;
break;
case MSG_SOL:
next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
if (next_fix_type == GPS_NO_FIX)
gpsSol.fixType = GPS_NO_FIX;
gpsSol.numSat = _buffer.solution.satellites;
gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
gpsSolDRV.fixType = GPS_NO_FIX;
gpsSolDRV.numSat = _buffer.solution.satellites;
gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
break;
case MSG_VELNED:
gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.velNED[X] = _buffer.velned.ned_north;
gpsSol.velNED[Y] = _buffer.velned.ned_east;
gpsSol.velNED[Z] = _buffer.velned.ned_down;
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = true;
gpsSolDRV.groundSpeed = _buffer.velned.speed_2d; // cm/s
gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSolDRV.velNED[X] = _buffer.velned.ned_north;
gpsSolDRV.velNED[Y] = _buffer.velned.ned_east;
gpsSolDRV.velNED[Z] = _buffer.velned.ned_down;
gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true;
_new_speed = true;
break;
case MSG_TIMEUTC:
if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
gpsSol.time.year = _buffer.timeutc.year;
gpsSol.time.month = _buffer.timeutc.month;
gpsSol.time.day = _buffer.timeutc.day;
gpsSol.time.hours = _buffer.timeutc.hour;
gpsSol.time.minutes = _buffer.timeutc.min;
gpsSol.time.seconds = _buffer.timeutc.sec;
gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
gpsSolDRV.time.year = _buffer.timeutc.year;
gpsSolDRV.time.month = _buffer.timeutc.month;
gpsSolDRV.time.day = _buffer.timeutc.day;
gpsSolDRV.time.hours = _buffer.timeutc.hour;
gpsSolDRV.time.minutes = _buffer.timeutc.min;
gpsSolDRV.time.seconds = _buffer.timeutc.sec;
gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000);
gpsSol.flags.validTime = true;
gpsSolDRV.flags.validTime = true;
} else {
gpsSol.flags.validTime = false;
gpsSolDRV.flags.validTime = false;
}
break;
case MSG_PVT:
next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
gpsSol.fixType = next_fix_type;
gpsSol.llh.lon = _buffer.pvt.longitude;
gpsSol.llh.lat = _buffer.pvt.latitude;
gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.numSat = _buffer.pvt.satellites;
gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = true;
gpsSol.flags.validEPE = true;
gpsSolDRV.fixType = next_fix_type;
gpsSolDRV.llh.lon = _buffer.pvt.longitude;
gpsSolDRV.llh.lat = _buffer.pvt.latitude;
gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSolDRV.numSat = _buffer.pvt.satellites;
gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true;
gpsSolDRV.flags.validEPE = true;
if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
gpsSol.time.year = _buffer.pvt.year;
gpsSol.time.month = _buffer.pvt.month;
gpsSol.time.day = _buffer.pvt.day;
gpsSol.time.hours = _buffer.pvt.hour;
gpsSol.time.minutes = _buffer.pvt.min;
gpsSol.time.seconds = _buffer.pvt.sec;
gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
gpsSolDRV.time.year = _buffer.pvt.year;
gpsSolDRV.time.month = _buffer.pvt.month;
gpsSolDRV.time.day = _buffer.pvt.day;
gpsSolDRV.time.hours = _buffer.pvt.hour;
gpsSolDRV.time.minutes = _buffer.pvt.min;
gpsSolDRV.time.seconds = _buffer.pvt.sec;
gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000);
gpsSol.flags.validTime = true;
gpsSolDRV.flags.validTime = true;
} else {
gpsSol.flags.validTime = false;
gpsSolDRV.flags.validTime = false;
}
_new_position = true;
@ -800,10 +800,16 @@ STATIC_PROTOTHREAD(gpsConfigure)
case GPS_DYNMODEL_PEDESTRIAN:
configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
break;
case GPS_DYNMODEL_AIR_1G: // Default to this
default:
case GPS_DYNMODEL_AUTOMOTIVE:
configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO);
break;
case GPS_DYNMODEL_AIR_1G:
configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
break;
case GPS_DYNMODEL_AIR_2G: // Default to this
default:
configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO);
break;
case GPS_DYNMODEL_AIR_4G:
configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
break;
@ -980,6 +986,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameUBLOX(newChar)) {
gpsProcessNewDriverData();
ptSemaphoreSignal(semNewDataReady);
break;
}
@ -1042,12 +1049,15 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
gpsState.autoConfigStep = 0;
ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
// M7 and earlier will never get pass this step, so skip it (#9440).
// UBLOX documents that this is M8N and later
if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) {
do {
pollGnssCapabilities();
gpsState.autoConfigStep++;
ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
} while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
}
// Configure GPS module if enabled
if (gpsState.gpsConfig->autoConfig) {
// Configure GPS
@ -1060,7 +1070,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
// GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
while (1) {
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {

2
src/main/io/gps_ublox.h
View file

@ -34,7 +34,9 @@ extern "C" {
#define GPS_CAPA_INTERVAL 5000
#define UBX_DYNMODEL_PEDESTRIAN 3
#define UBX_DYNMODEL_AUTOMOVITE 4
#define UBX_DYNMODEL_AIR_1G 6
#define UBX_DYNMODEL_AIR_2G 7
#define UBX_DYNMODEL_AIR_4G 8
#define UBX_FIXMODE_2D_ONLY 1

97
src/main/io/osd.c
View file

@ -154,6 +154,7 @@
#define OSD_MIN_FONT_VERSION 3
static timeMs_t linearDescentMessageMs = 0;
static timeMs_t notify_settings_saved = 0;
static bool savingSettings = false;
@ -1000,6 +1001,9 @@ static const char * divertingToSafehomeMessage(void)
static const char * navigationStateMessage(void)
{
if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0)
linearDescentMessageMs = 0;
switch (NAV_Status.state) {
case MW_NAV_STATE_NONE:
break;
@ -1011,6 +1015,12 @@ static const char * navigationStateMessage(void)
if (posControl.flags.rthTrackbackActive) {
return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK);
} else {
if (posControl.rthState.rthLinearDescentActive && (linearDescentMessageMs == 0 || linearDescentMessageMs > millis())) {
if (linearDescentMessageMs == 0)
linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds.
return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT);
} else
return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
}
case MW_NAV_STATE_HOLD_INFINIT:
@ -1047,11 +1057,18 @@ static const char * navigationStateMessage(void)
case MW_NAV_STATE_LANDED:
return OSD_MESSAGE_STR(OSD_MSG_LANDED);
case MW_NAV_STATE_LAND_SETTLE:
{
// If there is a FS landing delay occurring. That is handled by the calling function.
if (posControl.landingDelay > 0)
break;
return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND);
}
case MW_NAV_STATE_LAND_START_DESCENT:
// Not used
break;
}
return NULL;
}
@ -1323,7 +1340,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen
}
}
if (STATE(GPS_FIX)) {
if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
@ -1437,7 +1458,11 @@ static void osdDisplayTelemetry(void)
static uint16_t trk_bearing = 0;
if (ARMING_FLAG(ARMED)) {
if (STATE(GPS_FIX)){
if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
){
if (GPS_distanceToHome > 5) {
trk_bearing = GPS_directionToHome;
trk_bearing += 360 + 180;
@ -1815,6 +1840,12 @@ static bool osdDrawSingleElement(uint8_t item)
buff[0] = SYM_SAT_L;
buff[1] = SYM_SAT_R;
tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
strcpy(buff + 2, "ES");
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} else
#endif
if (!STATE(GPS_FIX)) {
hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) {
@ -1872,7 +1903,11 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_HOME_DIR:
{
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
}
@ -2258,7 +2293,7 @@ static bool osdDrawSingleElement(uint8_t item)
p = " WP ";
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
// If navigationRequiresAngleMode() returns false when ALTHOLD is active,
// it means it can be combined with ANGLE, HORIZON, ACRO, etc...
// it means it can be combined with ANGLE, HORIZON, ANGLEHOLD, ACRO, etc...
// and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
p = " AH ";
}
@ -2266,6 +2301,8 @@ static bool osdDrawSingleElement(uint8_t item)
p = "ANGL";
else if (FLIGHT_MODE(HORIZON_MODE))
p = "HOR ";
else if (FLIGHT_MODE(ANGLEHOLD_MODE))
p = "ANGH";
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
return true;
@ -2416,11 +2453,19 @@ static bool osdDrawSingleElement(uint8_t item)
osdCrosshairPosition(&elemPosX, &elemPosY);
osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if (osdConfig()->hud_homing && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
osdHudDrawHoming(elemPosX, elemPosY);
}
if (STATE(GPS_FIX) && isImuHeadingValid()) {
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && isImuHeadingValid()) {
if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
osdHudClear();
@ -3044,7 +3089,11 @@ static bool osdDrawSingleElement(uint8_t item)
digits = 4U;
}
#endif
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta));
@ -3115,7 +3164,11 @@ static bool osdDrawSingleElement(uint8_t item)
int32_t value = 0;
timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta));
@ -3267,7 +3320,11 @@ static bool osdDrawSingleElement(uint8_t item)
STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
int digits = osdConfig()->plus_code_digits;
int digitsRemoved = osdConfig()->plus_code_short * 2;
if (STATE(GPS_FIX)) {
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)) {
olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff));
} else {
// +codes with > 8 digits have a + at the 9th digit
@ -3685,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();
@ -5122,6 +5179,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
messages[messageCount++] = messageBuf;
}
} else if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
messages[messageCount++] = messageBuf;
} else {
const char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
@ -5156,10 +5218,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
}
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
// ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO
// when it doesn't require ANGLE mode (required only in FW
// right now). If if requires ANGLE, its display is handled
// by OSD_FLYMODE.
// right now). If it requires ANGLE, its display is handled by OSD_FLYMODE.
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
}
if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
@ -5196,6 +5257,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (STATE(LANDING_DETECTED)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
}
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
if (isAngleHoldLevel()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_LEVEL);
} else if (navAngleHoldAxis == FD_ROLL) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_ROLL);
} else if (navAngleHoldAxis == FD_PITCH) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
}
}
}
}
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {

4
src/main/io/osd.h
View file

@ -93,6 +93,7 @@
#define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE"
#define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING"
#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME"
#define OSD_MSG_RTH_LINEAR_DESCENT "BEGIN LINEAR DESCENT"
#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION"
#define OSD_MSG_WP_LANDED "WP END>LANDED"
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
@ -118,6 +119,9 @@
#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM"
#define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **"
#define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **"
#define OSD_MSG_ANGLEHOLD_ROLL "(ANGLEHOLD ROLL)"
#define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)"
#define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)"
#ifdef USE_DEV_TOOLS
#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED"

7
src/main/io/osd_dji_hd.c
View file

@ -300,6 +300,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
case FLM_ALTITUDE_HOLD:
case FLM_POSITION_HOLD:
case FLM_MISSION:
case FLM_ANGLEHOLD:
default:
// Unsupported ATM, keep at ANGLE
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
@ -786,7 +787,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta));

74
src/main/io/osd_joystick.c Normal file
View file

@ -0,0 +1,74 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#include "common/crc.h"
#include "common/maths.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "build/build_config.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/settings.h"
#include "fc/runtime_config.h"
#include "drivers/time.h"
#include "drivers/light_ws2811strip.h"
#include "io/serial.h"
#include "io/rcdevice.h"
#include "osd_joystick.h"
#ifdef USE_RCDEVICE
#ifdef USE_LED_STRIP
PG_REGISTER_WITH_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, PG_OSD_JOYSTICK_CONFIG, 0);
PG_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig,
.osd_joystick_enabled = SETTING_OSD_JOYSTICK_ENABLED_DEFAULT,
.osd_joystick_down = SETTING_OSD_JOYSTICK_DOWN_DEFAULT,
.osd_joystick_up = SETTING_OSD_JOYSTICK_UP_DEFAULT,
.osd_joystick_left = SETTING_OSD_JOYSTICK_LEFT_DEFAULT,
.osd_joystick_right = SETTING_OSD_JOYSTICK_RIGHT_DEFAULT,
.osd_joystick_enter = SETTING_OSD_JOYSTICK_ENTER_DEFAULT
);
bool osdJoystickEnabled(void) {
return osdJoystickConfig()->osd_joystick_enabled;
}
void osdJoystickSimulate5KeyButtonPress(uint8_t operation) {
switch (operation) {
case RCDEVICE_CAM_KEY_ENTER:
ledPinStartPWM( osdJoystickConfig()->osd_joystick_enter );
break;
case RCDEVICE_CAM_KEY_LEFT:
ledPinStartPWM( osdJoystickConfig()->osd_joystick_left );
break;
case RCDEVICE_CAM_KEY_UP:
ledPinStartPWM( osdJoystickConfig()->osd_joystick_up );
break;
case RCDEVICE_CAM_KEY_RIGHT:
ledPinStartPWM( osdJoystickConfig()->osd_joystick_right );
break;
case RCDEVICE_CAM_KEY_DOWN:
ledPinStartPWM( osdJoystickConfig()->osd_joystick_down );
break;
}
}
void osdJoystickSimulate5KeyButtonRelease(void) {
ledPinStopPWM();
}
#endif
#endif

26
src/main/io/osd_joystick.h Normal file
View file

@ -0,0 +1,26 @@
#pragma once
#include "config/parameter_group.h"
#ifdef USE_RCDEVICE
#ifdef USE_LED_STRIP
typedef struct osdJoystickConfig_s {
bool osd_joystick_enabled;
uint8_t osd_joystick_down;
uint8_t osd_joystick_up;
uint8_t osd_joystick_left;
uint8_t osd_joystick_right;
uint8_t osd_joystick_enter;
} osdJoystickConfig_t;
PG_DECLARE(osdJoystickConfig_t, osdJoystickConfig);
bool osdJoystickEnabled(void);
// 5 key osd cable simulation
void osdJoystickSimulate5KeyButtonPress(uint8_t operation);
void osdJoystickSimulate5KeyButtonRelease(void);
#endif
#endif

72
src/main/io/rcdevice_cam.c
View file

@ -29,6 +29,7 @@
#include "io/beeper.h"
#include "io/rcdevice_cam.h"
#include "io/osd_joystick.h"
#include "rx/rx.h"
@ -47,6 +48,14 @@ bool waitingDeviceResponse = false;
static bool isFeatureSupported(uint8_t feature)
{
#ifndef UNIT_TEST
#ifdef USE_LED_STRIP
if (!rcdeviceIsEnabled() && osdJoystickEnabled() ) {
return true;
}
#endif
#endif
if (camDevice->info.features & feature) {
return true;
}
@ -72,6 +81,7 @@ static void rcdeviceCameraControlProcess(void)
}
uint8_t behavior = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION;
uint8_t behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION;
switch (i) {
case BOXCAMERA1:
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON)) {
@ -81,11 +91,13 @@ static void rcdeviceCameraControlProcess(void)
if (!ARMING_FLAG(ARMED)) {
behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN;
}
behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN;
}
break;
case BOXCAMERA2:
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) {
behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
}
break;
case BOXCAMERA3:
@ -94,16 +106,43 @@ static void rcdeviceCameraControlProcess(void)
if (!ARMING_FLAG(ARMED)) {
behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE;
}
behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE;
}
break;
default:
break;
}
if (behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) {
if ((behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && rcdeviceIsEnabled()) {
runcamDeviceSimulateCameraButton(camDevice, behavior);
switchStates[switchIndex].isActivated = true;
}
#ifndef UNIT_TEST
#ifdef USE_LED_STRIP
else if ((behavior1 != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && osdJoystickEnabled()) {
switch (behavior1) {
case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN:
osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_ENTER);
break;
case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN:
osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_UP);
break;
case RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE:
osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_DOWN);
break;
}
switchStates[switchIndex].isActivated = true;
}
#endif
#endif
UNUSED(behavior1);
} else {
#ifndef UNIT_TEST
#ifdef USE_LED_STRIP
if (osdJoystickEnabled() && switchStates[switchIndex].isActivated) {
osdJoystickSimulate5KeyButtonRelease();
}
#endif
#endif
switchStates[switchIndex].isActivated = false;
}
}
@ -225,15 +264,25 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
}
#endif
if (camDevice->serialPort == 0 || ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (isButtonPressed) {
if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) {
if ( rcdeviceIsEnabled() ) {
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
waitingDeviceResponse = true;
}
#ifndef UNIT_TEST
#ifdef USE_LED_STRIP
else if (osdJoystickEnabled()) {
osdJoystickSimulate5KeyButtonRelease();
isButtonPressed = false;
}
#endif
#endif
}
} else {
if (waitingDeviceResponse) {
return;
@ -266,16 +315,33 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
}
if (key != RCDEVICE_CAM_KEY_NONE) {
if ( rcdeviceIsEnabled() ) {
rcdeviceSend5KeyOSDCableSimualtionEvent(key);
isButtonPressed = true;
waitingDeviceResponse = true;
}
#ifndef UNIT_TEST
#ifdef USE_LED_STRIP
else if (osdJoystickEnabled()) {
if ( key == RCDEVICE_CAM_KEY_CONNECTION_OPEN ) {
rcdeviceInMenu = true;
} else if ( key == RCDEVICE_CAM_KEY_CONNECTION_CLOSE ) {
rcdeviceInMenu = false;
} else {
osdJoystickSimulate5KeyButtonPress(key);
}
}
#endif
#endif
isButtonPressed = true;
}
}
}
void rcdeviceUpdate(timeUs_t currentTimeUs)
{
if ( rcdeviceIsEnabled() ) {
rcdeviceReceive(currentTimeUs);
}
rcdeviceCameraControlProcess();

13
src/main/io/vtx_tramp.c
View file

@ -575,16 +575,27 @@ const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] =
const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 800 };
const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" };
const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 2000 };
const char * const trampPowerNames_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "2000" };
static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
{
switch (vtxSettingsConfig()->frequencyGroup) {
case FREQUENCYGROUP_1G3:
if (maxPower >= 2000) {
vtxState.metadata.powerTablePtr = trampPowerTable_1G3_2000;
vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_2000;
impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
}
else {
vtxState.metadata.powerTablePtr = trampPowerTable_1G3_800;
vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800;
impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
}
impl_vtxDevice.capability.bandCount = VTX_TRAMP_1G3_BAND_COUNT;
impl_vtxDevice.capability.channelCount = VTX_TRAMP_1G3_CHANNEL_COUNT;
impl_vtxDevice.capability.bandNames = (char **)vtx1G3BandNames;

305
src/main/navigation/navigation.c
View file

@ -55,6 +55,7 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "navigation/rth_trackback.h"
#include "rx/rx.h"
@ -97,7 +98,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -132,6 +133,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
#endif
.waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
.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_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
.land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
@ -153,6 +155,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
},
// MC-specific
@ -257,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);
@ -272,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);
@ -1228,6 +1224,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
{
UNUSED(previousState);
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
// Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
@ -1253,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;
}
@ -1377,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;
@ -1433,6 +1399,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) {
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
posControl.rthState.rthLinearDescentActive = true;
}
}
@ -1443,6 +1410,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (isWaypointReached(tmpHomePos, 0)) {
// Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
posControl.landingDelay = 0;
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
@ -1471,9 +1444,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// If position ok OR within valid timeout - continue
// Action delay before landing if in FS and option enabled
bool pauseLanding = false;
navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
if ((allow == NAV_RTH_ALLOW_LANDING_ALWAYS || allow == NAV_RTH_ALLOW_LANDING_FS_ONLY) && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) {
if (posControl.landingDelay == 0)
posControl.landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay);
batteryState_e batteryState = getBatteryState();
if (millis() < posControl.landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL)
pauseLanding = true;
else
posControl.landingDelay = 0;
}
// If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) {
resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
@ -2382,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);
@ -2506,8 +2494,19 @@ bool validateRTHSanityChecker(void)
return true;
}
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
//schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true;
}
#endif
// Check at 10Hz rate
if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
@ -2731,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) {
@ -2775,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
*-----------------------------------------------------------*/
@ -3552,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)) {
@ -3645,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);
}
/*-----------------------------------------------------------
@ -3702,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;
}
@ -3814,7 +3708,6 @@ void checkManualEmergencyLandingControl(bool forcedActivation)
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
{
static bool canActivateWaypoint = false;
static bool canActivateLaunchMode = false;
//We can switch modes only when ARMED
@ -3862,10 +3755,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
posControl.rthSanityChecker.rthSanityOK = true;
// Keep canActivateWaypoint flag at FALSE if there is no mission loaded
// Also block WP mission if we are executing RTH
if (!isWaypointMissionValid() || isExecutingRTH) {
/* WP mission activation control:
* canActivateWaypoint & waypointWasActivated are used to prevent WP mission
* auto restarting after interruption by Manual or RTH modes.
* WP mode must be deselected before it can be reactivated again. */
static bool waypointWasActivated = false;
const bool isWpMissionLoaded = isWaypointMissionValid();
bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner
if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
canActivateWaypoint = false;
if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
canActivateWaypoint = true;
waypointWasActivated = false;
}
}
/* Airplane specific modes */
@ -3895,30 +3798,26 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
/* Soaring mode, disables altitude control in Position hold and Course hold modes.
* Pitch allowed to freefloat within defined Angle mode deadband */
if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
if (!FLIGHT_MODE(SOARING_MODE)) {
ENABLE_FLIGHT_MODE(SOARING_MODE);
}
} else {
DISABLE_FLIGHT_MODE(SOARING_MODE);
}
}
// Failsafe_RTH (can override MANUAL)
/* If we request forced RTH - attempt to activate it no matter what
* This might switch to emergency landing controller if GPS is unavailable */
if (posControl.flags.forcedRTHActivated) {
// If we request forced RTH - attempt to activate it no matter what
// This might switch to emergency landing controller if GPS is unavailable
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
/* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
* Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
* Also prevent WP falling back to RTH if WP mission planner is active */
const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive;
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) {
/* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded.
* WP prevented from falling back to RTH if WP mission planner is active */
const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive;
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) {
// Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
// If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold
// Without this loss of any of the canActivateNavigation && canActivateAltHold
// will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
// logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc)
// logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc)
if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
@ -3926,24 +3825,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// MANUAL mode has priority over WP/PH/AH
if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
// Block activation if using WP Mission Planner
// Also check multimission mission change status before activating WP mode
// Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded.
// Also check multimission mission change status before activating WP mode.
#ifdef USE_MULTI_MISSION
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
#else
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
#endif
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
waypointWasActivated = true;
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
else {
// Arm the state variable if the WP BOX mode is not enabled
canActivateWaypoint = true;
}
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
@ -3974,8 +3869,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
}
} else {
canActivateWaypoint = false;
// Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight)
canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid()));
}
@ -4056,7 +3949,8 @@ bool navigationPositionEstimateIsHealthy(void)
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) ||
IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
if (usedBypass) {
*usedBypass = false;
@ -4144,7 +4038,11 @@ void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
);
posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
@ -4232,7 +4130,7 @@ void updateWaypointsAndNavigationMode(void)
updateWpMissionPlanner();
// Update RTH trackback
updateRthTrackback(false);
rthTrackBackUpdate(false);
//Update Blackbox data
navCurrentState = (int16_t)posControl.navPersistentId;
@ -4568,3 +4466,26 @@ int32_t navigationGetHeadingError(void)
{
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
}
int8_t navCheckActiveAngleHoldAxis(void)
{
int8_t activeAxis = -1;
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
activeAxis = FD_PITCH;
} else if (altholdActive) {
activeAxis = FD_ROLL;
}
}
return activeAxis;
}
uint8_t getActiveWpNumber(void)
{
return NAV_Status.activeWpNumber;
}

16
src/main/navigation/navigation.h
View file

@ -203,12 +203,15 @@ typedef struct positionEstimationConfig_s {
float w_xy_res_v;
float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
float w_xyz_acc_p;
float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error
uint8_t use_gps_no_baro;
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation;
#endif
} positionEstimationConfig_t;
PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);
@ -245,6 +248,7 @@ typedef struct navConfig_s {
#endif
bool waypoint_load_on_boot; // load waypoints automatically during boot
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_manual_speed; // manual velocity control max horizontal speed
uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt
@ -266,6 +270,7 @@ typedef struct navConfig_s {
uint16_t auto_disarm_delay; // safety time delay for landing detector
uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
uint16_t rth_fs_landing_delay; // Delay upon reaching home before starting landing if in FS (0 = immediate)
} general;
struct {
@ -575,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);
@ -596,7 +604,7 @@ bool isFixedWingAutoThrottleManuallyIncreased(void);
bool navigationIsFlyingAutonomousMode(void);
bool navigationIsExecutingAnEmergencyLanding(void);
bool navigationIsControllingAltitude(void);
/* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
/* Returns true if navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
* or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
*/
bool navigationRTHAllowsLanding(void);
@ -627,6 +635,10 @@ 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);
/* Returns the heading recorded when home position was acquired.
* Note that the navigation system uses deg*100 as unit and angles

11
src/main/navigation/navigation_fixedwing.c
View file

@ -60,9 +60,8 @@
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
// If we are going slower than the minimum ground speed (navConfig()->general.min_ground_speed) - boost throttle to fight against the wind
#define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f
#define NAV_FW_MIN_VEL_SPEED_BOOST 700.0f // 7 m/s
// If this is enabled navigation won't be applied if velocity is below 3 m/s
//#define NAV_FW_LIMIT_MIN_FLY_VELOCITY
@ -518,8 +517,8 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
// POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
// FIXME: verify the above
calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
needToCalculateCircularLoiter = false;
}
else {
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
@ -552,10 +551,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
previousTimePositionUpdate = currentTimeUs;
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
// If we are in the deadband of 50cm/s - don't update speed boost
if (fabsf(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) {
if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) {
throttleSpeedAdjustment += velThrottleBoost;
}
@ -725,7 +724,7 @@ bool isFixedWingLandingDetected(void)
// Check horizontal and vertical velocities are low (cm/s)
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) &&
posControl.actualState.velXY < (100.0f * sensitivity);
( posControl.actualState.velXY < (100.0f * sensitivity));
// Check angular rates are low (degs/s)
bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity);
DEBUG_SET(DEBUG_LANDING, 2, velCondition);

141
src/main/navigation/navigation_pos_estimator.c
View file

@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
.w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
.w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
.w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
@ -92,7 +90,10 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT,
.max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
#ifdef USE_GPS_FIX_ESTIMATION
.allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
#endif
);
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
@ -170,6 +171,15 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
bool isGlitching = false;
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
previousTime = 0;
return true;
}
#endif
if (previousTime == 0) {
isGlitching = false;
}
@ -221,8 +231,16 @@ void onNewGPSData(void)
newLLH.lon = gpsSol.llh.lon;
newLLH.alt = gpsSol.llh.alt;
if (sensors(SENSOR_GPS)) {
if (!STATE(GPS_FIX)) {
if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (!(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)) {
isFirstGPSUpdate = true;
return;
}
@ -389,28 +407,30 @@ static bool gravityCalibrationComplete(void)
return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
}
#define ACC_VIB_FACTOR_S 1.0f
#define ACC_VIB_FACTOR_E 3.0f
static void updateIMUEstimationWeight(const float dt)
{
bool isAccClipped = accIsClipped();
// If accelerometer measurement is clipped - drop the acc weight to zero
static float acc_clip_factor = 1.0f;
// If accelerometer measurement is clipped - drop the acc weight to 0.3
// and gradually restore weight back to 1.0 over time
if (isAccClipped) {
posEstimator.imu.accWeightFactor = 0.0f;
if (accIsClipped()) {
acc_clip_factor = 0.5f;
}
else {
const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
}
// Update accelerometer weight based on vibration levels and clipping
float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s
posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
// DEBUG_VIBE[0-3] are used in IMU
DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
}
float navGetAccelerometerWeight(void)
{
const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
const float accWeightScaled = posEstimator.imu.accWeightFactor;
DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
return accWeightScaled;
@ -502,7 +522,11 @@ static bool navIsAccelerationUsable(void)
static bool navIsHeadingUsable(void)
{
if (sensors(SENSOR_GPS)) {
if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
// If we have GPS - we need true IMU north (valid heading)
return isImuHeadingValid();
}
@ -517,7 +541,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
/* Figure out if we have valid position data from our data sources */
uint32_t newFlags = 0;
if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid &&
if ((sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && posControl.gpsOrigin.valid &&
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
@ -553,13 +581,12 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
static void estimationPredict(estimationContext_t * ctx)
{
const float accWeight = navGetAccelerometerWeight();
/* Prediction step: Z-axis */
if ((ctx->newFlags & EST_Z_VALID)) {
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
}
/* Prediction step: XY-axis */
@ -570,10 +597,10 @@ static void estimationPredict(estimationContext_t * ctx)
// If heading is valid, accelNEU is valid as well. Account for acceleration
if (navIsHeadingUsable() && navIsAccelerationUsable()) {
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
}
}
}
@ -589,7 +616,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
if (ctx->newFlags & EST_BARO_VALID) {
bool correctOK = false;
//ignore baro if difference is too big, baro is probably wrong
const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
//fade out the baro to prevent sudden jump
const float start_epv = positionEstimationConfig()->max_eph_epv;
const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
//use both baro and gps
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
timeUs_t currentTimeUs = micros();
if (!ARMING_FLAG(ARMED)) {
@ -599,44 +635,33 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
}
else {
if (posEstimator.est.vel.z > 15) {
if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
posEstimator.state.isBaroGroundValid = false;
}
posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
}
else {
posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec
}
}
// We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
// We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
// Altitude
const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
// If GPS is available - also use GPS climb rate
if (ctx->newFlags & EST_GPS_Z_VALID) {
// Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
}
ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
// Accelerometer bias
if (!isAirCushionEffectDetected) {
ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
}
return true;
correctOK = true;
}
else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
// If baro is not available - use GPS Z for correction on a plane
if (ctx->newFlags & EST_GPS_Z_VALID) {
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@ -646,20 +671,21 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
else {
// Altitude
const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
// Accelerometer bias
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
}
return true;
correctOK = true;
}
return false;
return correctOK;
}
static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
@ -714,15 +740,14 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
{
if (STATE(GPS_FIX) && navIsHeadingUsable()) {
static timeUs_t lastUpdateTimeUs = 0;
if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate
const float dt = US2S(currentTimeUs - lastUpdateTimeUs);
uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt)));
UNUSED(currentTimeUs);
if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && navIsHeadingUsable()) {
uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x)));
posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
lastUpdateTimeUs = currentTimeUs;
}
}
}
@ -779,7 +804,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
}
// Boost the corrections based on accWeight
const float accWeight = navGetAccelerometerWeight();
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
// Apply corrections
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
@ -817,13 +845,14 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
{
static navigationTimer_t posPublishTimer;
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
/* Publish heading update */
/* IMU operates in decidegrees while INAV operates in deg*100
* Use course over ground when GPS heading valid */
int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
/* Publish position update */
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
// FIXME!!!!!

1
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -39,7 +39,6 @@
#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate
#define INAV_PITOT_UPDATE_RATE 10
#define INAV_COG_UPDATE_RATE_HZ 20 // ground course update rate
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout

10
src/main/navigation/navigation_private.h
View file

@ -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!");
@ -350,6 +348,7 @@ typedef struct {
float rthInitialDistance; // Distance when starting flight home
fpVector3_t homeTmpWaypoint; // Temporary storage for home target
fpVector3_t originalHomePosition; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
bool rthLinearDescentActive; // Activation status of Linear Descent
} rthState_t;
typedef enum {
@ -394,6 +393,7 @@ typedef struct {
rthState_t rthState;
uint32_t homeDistance; // cm
int32_t homeDirection; // deg*100
timeMs_t landingDelay;
/* Safehome parameters */
safehomeState_t safehomeState;
@ -426,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
View 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
View 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);

36
src/main/programming/logic_condition.c
View file

@ -56,6 +56,7 @@
#include "io/vtx.h"
#include "drivers/vtx_common.h"
#include "drivers/light_ws2811strip.h"
PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 4);
@ -475,6 +476,27 @@ static int logicConditionCompute(
}
break;
#ifdef LED_PIN
case LOGIC_CONDITION_LED_PIN_PWM:
if (operandA >=0 && operandA <= 100) {
ledPinStartPWM((uint8_t)operandA);
} else {
ledPinStopPWM();
}
return operandA;
break;
#endif
#ifdef USE_GPS_FIX_ESTIMATION
case LOGIC_CONDITION_DISABLE_GPS_FIX:
if (operandA > 0) {
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
} else {
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
}
return true;
break;
#endif
default:
return false;
break;
@ -656,6 +678,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
#ifdef USE_GPS_FIX_ESTIMATION
if ( STATE(GPS_ESTIMATED_FIX) ){
return gpsSol.numSat; //99
} else
#endif
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
return 0;
} else {
@ -849,13 +876,18 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
return (bool) FLIGHT_MODE(HORIZON_MODE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD:
return (bool) FLIGHT_MODE(ANGLEHOLD_MODE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO:
return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) ||
(bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(ANGLEHOLD_MODE) ||
(bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) ||
(bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:

8
src/main/programming/logic_condition.h
View file

@ -81,7 +81,9 @@ typedef enum {
LOGIC_CONDITION_TIMER = 49,
LOGIC_CONDITION_DELTA = 50,
LOGIC_CONDITION_APPROX_EQUAL = 51,
LOGIC_CONDITION_LAST = 52,
LOGIC_CONDITION_LED_PIN_PWM = 52,
LOGIC_CONDITION_DISABLE_GPS_FIX = 53,
LOGIC_CONDITION_LAST = 54,
} logicOperation_e;
typedef enum logicOperandType_s {
@ -157,6 +159,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO, // 14
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION, // 15
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD, // 16
} logicFlightModeOperands_e;
typedef enum {
@ -188,6 +191,9 @@ typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10),
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11),
#endif
} logicConditionsGlobalFlags_t;
typedef enum {

3
src/main/sensors/acceleration.c
View file

@ -576,6 +576,7 @@ void accUpdate(void)
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
const float accDiff = acc.accADCf[axis] - accFloorFilt;
acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
}
// Filter acceleration
@ -612,7 +613,7 @@ void accGetVibrationLevels(fpVector3_t *accVibeLevels)
float accGetVibrationLevel(void)
{
return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
return acc.accVibe;
}
uint32_t accGetClipCount(void)

3
src/main/sensors/acceleration.h
View file

@ -27,7 +27,7 @@
#define GRAVITY_CMSS 980.665f
#define GRAVITY_MSS 9.80665f
#define ACC_CLIPPING_THRESHOLD_G 7.9f
#define ACC_CLIPPING_THRESHOLD_G 15.9f
#define ACC_VIBE_FLOOR_FILT_HZ 5.0f
#define ACC_VIBE_FILT_HZ 2.0f
@ -58,6 +58,7 @@ typedef struct acc_s {
uint32_t accTargetLooptime;
float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
float accVibeSq[XYZ_AXIS_COUNT];
float accVibe;
uint32_t accClipCount;
bool isClipped;
acc_extremes_t extremes[XYZ_AXIS_COUNT];

2
src/main/sensors/battery.c
View file

@ -134,7 +134,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
.failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.nav = {
.mc = {
.hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
},
@ -147,7 +146,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
.launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
.launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP
}
},
#if defined(USE_POWER_LIMITS)

1
src/main/sensors/compass.c
View file

@ -485,4 +485,5 @@ void compassUpdate(timeUs_t currentTimeUs)
magUpdatedAtLeastOnce = true;
}
#endif

42
src/main/sensors/diagnostics.c
View file

@ -43,12 +43,10 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
if (accIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
else {
} else {
if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
@ -67,8 +65,7 @@ hardwareSensorStatus_e getHwCompassStatus(void)
if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
if (compassIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -76,17 +73,14 @@ hardwareSensorStatus_e getHwCompassStatus(void)
if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
if (compassIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
else {
} else {
if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -139,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
if (rangefinderIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -148,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -165,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
if (pitotIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -174,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -191,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (sensors(SENSOR_GPS)) {
if (isGPSHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -200,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -217,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
if (opflowIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -226,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}

10
src/main/sensors/pitotmeter.c
View file

@ -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
}

Some files were not shown because too many files have changed in this diff Show more