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

Merge branch 'master' into master

Browse source
This commit is contained in:
Stefano Della Valle 2023-12-16 13:45:01 +01:00 • committed by GitHub
commit 6a082ac1e0
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
225 changed files with 5273 additions and 1417 deletions
Download patch file Download diff file Expand all files Collapse all files

5
.vscode/settings.json vendored
View file

@ -2,7 +2,10 @@
"files.associations": {
"chrono": "c",
"cmath": "c",
"ranges": "c"
"ranges": "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)

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

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.

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)

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

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

88
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 |
---
@ -2444,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 |
| --- | --- | --- |
@ -2454,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 |
| --- | --- | --- |
@ -2464,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 |
| --- | --- | --- |
@ -2474,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 |
| --- | --- | --- |
@ -3692,6 +3712,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 +3852,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]
@ -5058,7 +5098,7 @@ Selection of pitot hardware.
| Default | Min | Max |
| --- | --- | --- |
| VIRTUAL | | |
| NONE | | |
---
@ -5994,11 +6034,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

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.

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

95
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
{
if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE){
USB_Tx_ptr = APP_Rx_ptr_out;
} else {
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;
}
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

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

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

@ -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__ ({ \

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;

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)

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

43
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);
}
@ -760,7 +747,7 @@ void processRx(timeUs_t currentTimeUs)
} else {
DISABLE_FLIGHT_MODE(MANUAL_MODE);
}
}else{
} else {
DISABLE_FLIGHT_MODE(MANUAL_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
@ -2581,6 +2581,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
}*/
// //////////////////////////////////////////////////////////
}
}
}
@ -3676,36 +3699,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]]) {

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;

29
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);

65
src/main/fc/settings.yaml
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"]
@ -588,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
@ -838,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
@ -1463,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
@ -1608,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
@ -1730,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
@ -1754,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
@ -1778,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
@ -2195,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
@ -2285,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"
@ -2322,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
@ -2363,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
@ -2486,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
@ -2592,6 +2611,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"
@ -3810,10 +3835,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]."

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

79
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 axis angle override, get the correct angle from Logic Conditions
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));
//Apply the Level PID controller
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);

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

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

@ -430,7 +430,7 @@ static void configureGNSS(void)
blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]);
send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed);
send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
sendConfigMessageUBLOX();
}
@ -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;
@ -649,7 +649,7 @@ static bool gpsParseFrameUBLOX(void)
}
}
}
for (int j = 40; j < _payload_length; j += 30) {
for(int j = 40; j < _payload_length; j += 30) {
if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) {
gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30);
break;
@ -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;
}
@ -1038,16 +1045,19 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
pollVersion();
gpsState.autoConfigStep++;
ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
} while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
} while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
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

103
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
@ -3995,7 +4052,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) {
logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing
}
}
// Draw Logo(s)
if (usePilotLogo && !singular) {
@ -4036,7 +4093,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
}
return logoRow;
}
}
uint8_t drawStats(uint8_t row) {
#ifdef USE_STATS
@ -4574,7 +4631,7 @@ static void osdShowHDArmScreen(void)
#if defined(USE_GPS)
if (feature(FEATURE_GPS)) {
if (STATE(GPS_FIX_HOME)) {
if (osdConfig()->osd_home_position_arm_screen) {
if (osdConfig()->osd_home_position_arm_screen){
osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
uint8_t gap = 1;
@ -4630,7 +4687,7 @@ static void osdShowHDArmScreen(void)
if (armScreenRow < (osdDisplayPort->rows - 4))
armScreenRow = drawStats(armScreenRow);
#endif // USE_STATS
}
}
static void osdShowSDArmScreen(void)
{
@ -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));

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

@ -97,7 +97,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 +132,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_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
@ -155,6 +156,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
@ -1227,6 +1229,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.
@ -1432,6 +1437,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;
}
}
@ -1442,6 +1448,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);
@ -1470,9 +1482,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
@ -2505,8 +2532,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;
@ -3813,7 +3851,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
@ -3861,10 +3898,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 */
@ -3894,30 +3941,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;
}
@ -3925,24 +3968,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)) {
@ -3973,8 +4012,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()));
}
@ -4055,7 +4092,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;
@ -4143,7 +4181,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) {
@ -4567,3 +4609,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;
}

12
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_auto_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_manual_speed; // manual velocity control max horizontal speed
@ -268,6 +272,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 {
@ -595,7 +600,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 +632,9 @@ bool isEstimatedAglTrusted(void);
void checkManualEmergencyLandingControl(bool forcedActivation);
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
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
* are in the [0, 360 * 100) interval.

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

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

@ -350,6 +350,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 +395,7 @@ typedef struct {
rthState_t rthState;
uint32_t homeDistance; // cm
int32_t homeDirection; // deg*100
timeMs_t landingDelay;
/* Safehome parameters */
safehomeState_t safehomeState;

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

@ -493,6 +493,16 @@ static int logicConditionCompute(
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;
@ -675,6 +685,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 {
@ -868,13 +883,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:

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

@ -83,7 +83,8 @@ typedef enum {
LOGIC_CONDITION_APPROX_EQUAL = 51,
LOGIC_CONDITION_LED_PIN_PWM = 52,
LOGIC_CONDITION_RESET_MAG_CALIBRATION = 53,
LOGIC_CONDITION_LAST = 54,
LOGIC_CONDITION_DISABLE_GPS_FIX = 54,
LOGIC_CONDITION_LAST = 55,
} logicOperation_e;
typedef enum logicOperandType_s {
@ -159,6 +160,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 {
@ -190,6 +192,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)

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

13
src/main/target/AIRBOTF4/target.c
View file

@ -32,12 +32,13 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT
DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED | TIM_USE_ANY, 0, 0), // LED D1_ST0, n/a on older AIRBOTF4
DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port)
DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN
DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN
//Airbot F4 don't have those outputs, they exist only in the original Revo
// DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port)
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN
// DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN
// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN
// DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

10
src/main/target/ALIENFLIGHTF4/target.c
View file

@ -23,11 +23,10 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1
DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5
DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7
DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2
DEF_TIM(TIM1, CH3, PB15, TIM_USE_PWM | TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6
// DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1
// DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5
// DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7
// DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM6 - PB8 OUT1 - DMA1_ST7
DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM7 - PB9 OUT2 - DMA_NONE
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM8 - PA0 OUT3 - DMA1_ST2
@ -36,6 +35,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM11 - PC7 OUT6 - DMA2_ST3
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // PWM13 - PC8 OUT7 - DMA2_ST4
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM13 - PC9 OUT8 - DMA2_ST7
DEF_TIM(TIM1, CH3, PB15, TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

2
src/main/target/ALIENFLIGHTNGF7/target.c
View file

@ -23,7 +23,6 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, 0, 1), // PPM - DMA2_ST1
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM1 - DMA2_ST2
DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM2 - DMA1_ST5
DEF_TIM(TIM8, CH2N, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM3 - DMA2_ST3
@ -36,6 +35,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM10 - DMA2_ST6
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM11 - DMA1_ST7
DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM12 - DMA_NONE
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1), // PPM - DMA2_ST1
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

12
src/main/target/ANYFC/target.c
View file

@ -23,12 +23,12 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN
DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN
DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN
// DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN
// DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN
// DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN
// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S10_OUT 16
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT 12

15
src/main/target/ANYFCF7/target.c
View file

@ -23,12 +23,13 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN
DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7
// DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7
DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST0
@ -40,6 +41,8 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

12
src/main/target/ANYFCM7/target.c
View file

@ -26,12 +26,12 @@
#define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL
timerHardware_t timerHardware[] = {
DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN
DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7
// DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN
// DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
// DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7
DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST1

2
src/main/target/AOCODARCF405AIO/target.c
View file

@ -28,7 +28,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU65
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH2, PA10, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM2, CH2, PA10, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2

2
src/main/target/AOCODARCF4V2/target.c
View file

@ -25,7 +25,7 @@
#include "drivers/bus.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0 ), // PPM IN
// DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S2

2
src/main/target/AOCODARCF722AIO/target.c
View file

@ -27,7 +27,7 @@
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S1
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // S2

2
src/main/target/AOCODARCF7DUAL/target.c
View file

@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS,
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM
// DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM
DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2, 2, 7)
DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7)

2
src/main/target/AOCODARCF7MINI/target.c
View file

@ -37,7 +37,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS,
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1
// DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5)
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1, 5, 4)

1
src/main/target/ATOMRCF405NAVI_DELUX/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f405xg(ATOMRCF405NAVI_DELUX)

37
src/main/target/ATOMRCF405NAVI_DELUX/config.c Normal file
View file

@ -0,0 +1,37 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdint.h>
#include "platform.h"
#include "fc/fc_msp_box.h"
#include "io/piniobox.h"
#include "io/serial.h"
void targetConfiguration(void)
{
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200;
pinioBoxConfigMutable()->permanentId[0] = BOXARM;
}

56
src/main/target/ATOMRCF405NAVI_DELUX/target.c Normal file
View file

@ -0,0 +1,56 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/pinio.h"
#include "drivers/sensor.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S9
DEF_TIM(TIM8, CH2N, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S10
DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S11
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_STRIP
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

189
src/main/target/ATOMRCF405NAVI_DELUX/target.h Normal file
View file

@ -0,0 +1,189 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "AT4D"
#define USBD_PRODUCT_STRING "AtomRCF405NAVI_DELUX"
#define LED0 PA13
#define LED1 PA14
#define BEEPER PB2
#define BEEPER_INVERTED
/*
* SPI defines
*/
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PC2
#define SPI2_MOSI_PIN PC3
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
/*
* I2C defines
*/
#define USE_I2C
#define DEFAULT_I2C_BUS BUS_I2C1
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
//ICM42688-P
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW270_DEG
#define ICM42605_CS_PIN PA4
#define ICM42605_SPI_BUS BUS_SPI1
/*
* Magnetometer
*/
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#define USE_MAG_ALL
/*
* Barometer
*/
#define USE_BARO
#define BARO_I2C_BUS DEFAULT_I2C_BUS
#define USE_BARO_ALL
/*
* Serial ports
*/
#define USE_VCP
#define USE_UART1
#define USE_UART2
#define USE_UART3
#define USE_UART4
#define USE_UART5
#define USE_UART6
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define UART3_TX_PIN PC10
#define UART3_RX_PIN PC11
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 7
/*
* ADC
*/
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC1
#define ADC_CHANNEL_3_PIN PC4
#define ADC_CHANNEL_4_PIN PC5
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define AIRSPEED_ADC_CHANNEL ADC_CHN_4
#define CURRENT_METER_SCALE 128
/*
* OSD
*/
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
/*
* SD Card
*/
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_SPI_BUS BUS_SPI3
#define SDCARD_CS_PIN PC14
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
/*
* LED Strip
*/
#define USE_LED_STRIP
#define WS2811_PIN PA8
/*
* Other configs
*/
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT )
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2
#define USE_RANGEFINDER
#define USE_RANGEFINDER_MSP
#define USE_OPFLOW
#define USE_OPFLOW_MSP
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define MAX_PWM_OUTPUT_PORTS 11
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PC15
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED

2
src/main/target/AXISFLYINGF7PRO/target.c
View file

@ -33,7 +33,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S7
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // CAM CONTROL
};

2
src/main/target/BEEROTORF4/target.c
View file

@ -24,7 +24,7 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN
// DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN
DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 - DMAR: DMA2_ST5
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 -

2
src/main/target/BETAFLIGHTF4/target.c
View file

@ -24,7 +24,7 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM
// Motors
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7

10
src/main/target/BETAFPVF435/target.h
View file

@ -31,7 +31,7 @@
//#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
//#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_AUTO
//#define ENABLE_DSHOT
#define ENABLE_DSHOT
// *************** Gyro & ACC **********************
#define USE_SPI
@ -177,7 +177,7 @@
#define USE_LED_STRIP
#define WS2811_PIN PB6
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP )
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP )
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
@ -189,11 +189,13 @@
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define TARGET_IO_PORTE BIT(2)
#define TARGET_IO_PORTH (BIT(1)|BIT(2)|BIT(3))
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
//#define USE_DSHOT
//#define USE_ESC_SENSOR
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_ESCSERIAL
#define USE_RPM_FILTER

2
src/main/target/BETAFPVF722/target.c
View file

@ -25,7 +25,7 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 D(1, 4, 5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2 D(2, 3, 7)

2
src/main/target/BLACKPILL_F411/target.c
View file

@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), //ST1 pad -softserial_tx1
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM
// DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM
};

2
src/main/target/BLUEJAYF4/target.c
View file

@ -23,7 +23,7 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2
DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4

4
src/main/target/CLRACINGF4AIR/target.c
View file

@ -21,8 +21,7 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0),
DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0),
// DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0),
#if defined(CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),
@ -39,6 +38,7 @@
DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0),
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0),
#endif
DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0),
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

16
src/main/target/COLIBRI/target.c
View file

@ -24,14 +24,14 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH3, PA10, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S2_IN
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S3_IN
DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S4_IN
DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, 0, 0), // S5_IN
DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM, 0, 0), // S6_IN
DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // S7_IN
DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // S8_IN
// DEF_TIM(TIM1, CH3, PA10, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN
// DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S2_IN
// DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S3_IN
// DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S4_IN
// DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, 0, 0), // S5_IN
// DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM, 0, 0), // S6_IN
// DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // S7_IN
// DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // S8_IN
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT

1
src/main/target/DAKEFPVF405/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f405xg(DAKEFPVF405)

31
src/main/target/DAKEFPVF405/config.c Normal file
View file

@ -0,0 +1,31 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "io/serial.h"
#include "rx/rx.h"
#include "fc/fc_msp_box.h"
#include "io/piniobox.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
}

36
src/main/target/DAKEFPVF405/target.c Normal file
View file

@ -0,0 +1,36 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0),
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

168
src/main/target/DAKEFPVF405/target.h Normal file
View file

@ -0,0 +1,168 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "DAK4"
#define USBD_PRODUCT_STRING "DAKEFPV F405"
#define LED0 PC14
#define LED1 PC15
#define BEEPER PC3
#define BEEPER_INVERTED
// Buses
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1
// Gyro
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW90_DEG
#define MPU6500_CS_PIN PA4
#define MPU6500_SPI_BUS BUS_SPI1
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW90_DEG
#define MPU6000_CS_PIN PA4
#define MPU6000_SPI_BUS BUS_SPI1
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW90_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PA4
//Baro
#define USE_BARO
#define USE_BARO_BMP280
#define BMP280_SPI_BUS BUS_SPI2
#define BMP280_CS_PIN PC13
#define USE_BARO_DPS310
#define DPS310_SPI_BUS BUS_SPI2
#define DPS310_CS_PIN PC13
// M25P256 flash
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN PA15
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
// Serial ports
#define USE_VCP
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define USE_UART2
#define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART4
#define UART4_RX_PIN PA1
#define UART4_TX_PIN PA0
#define USE_UART5
#define UART5_RX_PIN PD2
#define UART5_TX_PIN PC12
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 7
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1
// Mag
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#define USE_MAG_ALL
#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
#define USE_RANGEFINDER
#define USE_RANGEFINDER_MSP
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
// ADC
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC1
#define ADC_CHANNEL_2_PIN PC2
#define ADC_CHANNEL_3_PIN PC0
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define RSSI_ADC_CHANNEL ADC_CHN_2
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY )
#define USE_LED_STRIP
#define WS2811_PIN PB3
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define USE_DSHOT
#define USE_ESC_SENSOR
#define MAX_PWM_OUTPUT_PORTS 6
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PC5
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED

1
src/main/target/DAKEFPVF722/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f722xe(DAKEFPVF722)

28
src/main/target/DAKEFPVF722/config.c Normal file
View file

@ -0,0 +1,28 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "io/serial.h"
#include "rx/rx.h"
void targetConfiguration(void)
{
}

36
src/main/target/DAKEFPVF722/target.c Normal file
View file

@ -0,0 +1,36 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0),
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

150
src/main/target/DAKEFPVF722/target.h Normal file
View file

@ -0,0 +1,150 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "DAK7"
#define USBD_PRODUCT_STRING "DAKEFPV F722"
#define LED0 PC14
#define LED1 PC15
#define BEEPER PC3
#define BEEPER_INVERTED
// Buses
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1
// Gyro
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW90_DEG
#define MPU6500_CS_PIN PA4
#define MPU6500_SPI_BUS BUS_SPI1
//Baro
#define USE_BARO
#define USE_BARO_BMP280
#define BMP280_SPI_BUS BUS_SPI2
#define BMP280_CS_PIN PA13
// M25P256 flash
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN PA15
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
// Serial ports
#define USE_VCP
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define USE_UART2
#define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART4
#define UART4_RX_PIN PA1
#define UART4_TX_PIN PA0
#define USE_UART5
#define UART5_RX_PIN PD2
#define UART5_TX_PIN PC12
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 7
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1
// Mag
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#define USE_MAG_ALL
#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
#define USE_RANGEFINDER
#define USE_RANGEFINDER_MSP
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
// ADC
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC1
#define ADC_CHANNEL_2_PIN PC2
#define ADC_CHANNEL_3_PIN PC0
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define RSSI_ADC_CHANNEL ADC_CHN_2
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY )
#define USE_LED_STRIP
#define WS2811_PIN PB3
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define USE_DSHOT
#define USE_ESC_SENSOR
#define MAX_PWM_OUTPUT_PORTS 6

2
src/main/target/DALRCF405/target.c
View file

@ -22,7 +22,7 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7)
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (2,2)

2
src/main/target/DALRCF722DUAL/target.c
View file

@ -28,7 +28,7 @@
#define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL
timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0),
// DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0),
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO,0,0), //S1 DMA2_ST2 T8CH1
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO,0,0), //S2 DMA2_ST3 T8CH2

16
src/main/target/F4BY/target.c
View file

@ -6,14 +6,14 @@
#include "drivers/timer.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN
DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN
DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN
DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN
DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN
DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN
DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN
DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN
// DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN
// DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN
// DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN
// DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN
// DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN
// DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN
// DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN
// DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN
DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT
DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT

10
src/main/target/FF_F35_LIGHTNING/target.c
View file

@ -33,11 +33,11 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0),
DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 0),
#ifdef WINGFC
DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0, 0), // UART3 RX
#else
DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // UART3 TX
#endif
// #ifdef WINGFC
// DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0, 0), // UART3 RX
// #else
// DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // UART3 TX
// #endif
};

2
src/main/target/FF_FORTINIF4/target.c
View file

@ -27,7 +27,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S2_OUT - DMA1_ST2
DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 1, 1 ), // S3_OUT - DMA1_ST6
DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S4_OUT - DMA1_ST1
DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN
// DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED - DMA1_ST3
};

2
src/main/target/FIREWORKSV2/target.c
View file

@ -44,7 +44,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6000, DEVHW_MPU6000, IMU_2_SPI_BUS, IMU_
BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6500, DEVHW_MPU6500, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
// DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
// Motor output 1: use different set of timers for MC and FW
//DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM _USE_SERVO, 1, 0), // S1_OUT D(1,7)

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