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

Merge branch 'master' into dzikuvx-ez-tune

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2023-08-14 10:55:53 +02:00
commit 618574409a
94 changed files with 3604 additions and 509 deletions
Download patch file Download diff file Expand all files Collapse all files

5
.dir-locals.el Normal file
View file

@ -0,0 +1,5 @@
;;; Directory Local Variables -*- no-byte-compile: t -*-
;;; For more information see (info "(emacs) Directory Variables")
((nil . ((c-basic-offset . 4)
(c-default-style . "k&r"))))

1
.github/workflows/ci.yml vendored
View file

@ -79,7 +79,6 @@ jobs:
- uses: actions/checkout@v3
- name: Install dependencies
run: |
brew update
brew install cmake ninja ruby
- name: Setup environment

3
.gitignore vendored
View file

@ -12,7 +12,7 @@
__pycache__
startup_stm32f10x_md_gcc.s
.vagrant/
.vscode/
#.vscode/
cov-int*
/build/
/obj/
@ -31,3 +31,4 @@ README.pdf
# local changes only
make/local.mk
launch.json

9
.vimrc Normal file
View file

@ -0,0 +1,9 @@
filetype on
filetype indent on
set expandtab
set bs=2
set sw=4
set ts=4

12
.vscode/settings.json vendored Normal file
View file

@ -0,0 +1,12 @@
{
"files.associations": {
"chrono": "c",
"cmath": "c",
"ranges": "c"
},
"editor.tabSize": 4,
"editor.insertSpaces": true,
"editor.detectIndentation": false,
"editor.expandTabs": true,
"C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }"
}

2
cmake/main.cmake
View file

@ -83,7 +83,7 @@ function(setup_executable exe name)
set_target_properties(${exe} PROPERTIES
RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin"
)
if(IS_RELEASE_BUILD AND NOT CMAKE_HOST_APPLE)
if(IS_RELEASE_BUILD AND NOT (CMAKE_HOST_APPLE AND SITL))
set_target_properties(${exe} PROPERTIES
INTERPROCEDURAL_OPTIMIZATION ON
)

3
cmake/sitl.cmake
View file

@ -59,6 +59,9 @@ if(NOT MACOSX)
-Wno-error=maybe-uninitialized
-fsingle-precision-constant
)
if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0)
set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-Wl,--no-warn-rwx-segments")
endif()
else()
set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
)

32
docs/Buzzer.md
View file

@ -62,12 +62,14 @@ beeper list
giving:
```
Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED
Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW
BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT
ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED
```
The `beeper` command syntax follows that of the `feature` command; a minus (`-`) in front of a name disables that function.
So to disable the beeper / buzzer when connected to USB (may enhance domestic harmony)
So to disable the beeper / buzzer when powered by USB (may enhance domestic harmony):
```
beeper -ON_USB
@ -78,17 +80,39 @@ Now the `beeper` command will show:
```
# beeper
Disabled: ON_USB
```
*Note: SYSTEM_INIT sequence is not affected by ON_USB setting and will still be played on USB connection. Disable both ON_USB and SYSTEM_INIT to disable buzzer completely when FC is powered from USB.*
*Note: ON_USB setting requires present and configured battery voltage metter.*
To disable all features use:
```
beeper -ALL
```
To store current set to preferences use (preferences also require ```save```):
```
beeper PREFERED
```
To restore set from preferences use:
```
beeper -PREFERED
As with other CLI commands, the `save` command is needed to save the new settings.
## Types of buzzer supported
The buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
Most FCs require ACTIVE buzzers. Active buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
This means the buzzer must be able to generate its own tone simply by having power applied to it.
Buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all.
Passive buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all.
Passive buzzers are supported on FCs which are designed to work with passive buzzers only (so far there is no available, except rare cases like Matek F765-WSE where passive buzzer is preinstalled).
Examples of a known-working buzzers.

167
docs/OSD.md Normal file
View file

@ -0,0 +1,167 @@
# On Screen Display
The On Screen Display, or OSD, is a feature that overlays flight data over the video image. This can be done on the flight controller, using the analogue MAX7456 chip. Digital systems take the OSD data, via MSP DisplayPort, send it to the video receiver; which combines the data with the image. You can specify what elements are displayed, and their locations on the image. Most systems are character based, and use the MAX7456 analogue setup, or MSP DisplayPort. However, there are some different systems which are also supported. Such as the canvas based FrSKY PixelOSD on analogue. Canvas OSDs draw shapes on the image. Whereas character based OSDs use font characters to display the data.
## Features and Limitations
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 |
| DJI OSD | 30 x 16 | X | | | NO - BF Characters only |
| DJI WTFOS | 60 x 22 | X | | X | YES |
| HDZero | 50 x 18 | X | | X | YES |
| Avatar | 53 x 20 | X | | X | YES |
| DJI O3 | 53 x 20 (HD) | X | | X (partial) | NO - BF Characters only |
## OSD Elements
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 |
| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 |
| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 |
| 5 | OSD_ONTIME | 1.0.0 |
| 6 | OSD_FLYTIME | 1.0.0 |
| 7 | OSD_FLYMODE | 1.0.0 |
| 8 | OSD_CRAFT_NAME | 1.0.0 |
| 9 | OSD_THROTTLE_POS | 1.0.0 |
| 10 | OSD_VTX_CHANNEL | 1.0.0 |
| 11 | OSD_CURRENT_DRAW | 1.0.0 |
| 12 | OSD_MAH_DRAWN | 1.0.0 |
| 13 | OSD_GPS_SPEED | 1.0.0 |
| 14 | OSD_GPS_SATS | 1.0.0 |
| 15 | OSD_ALTITUDE | 1.0.0 |
| 16 | OSD_ROLL_PIDS | 1.6.0 |
| 17 | OSD_PITCH_PIDS | 1.6.0 |
| 18 | OSD_YAW_PIDS | 1.6.0 |
| 19 | OSD_POWER | 1.6.0 |
| 20 | OSD_GPS_LON | 1.6.0 |
| 21 | OSD_GPS_LAT | 1.6.0 |
| 22 | OSD_HOME_DIR | 1.6.0 |
| 23 | OSD_HOME_DIST | 1.6.0 |
| 24 | OSD_HEADING | 1.6.0 |
| 25 | OSD_VARIO | 1.6.0 |
| 26 | OSD_VARIO_NUM | 1.6.0 |
| 27 | OSD_AIR_SPEED | 1.7.3 |
| 28 | OSD_ONTIME_FLYTIME | 1.8.0 |
| 29 | OSD_RTC_TIME | 1.8.0 |
| 30 | OSD_MESSAGES | 1.8.0 |
| 31 | OSD_GPS_HDOP | 1.8.0 |
| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 |
| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 |
| 34 | OSD_HEADING_GRAPH | 1.8.0 |
| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 |
| 36 | OSD_WH_DRAWN | 1.9.0 |
| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 |
| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 |
| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 |
| 40 | OSD_TRIP_DIST | 1.9.1 |
| 41 | OSD_ATTITUDE_PITCH | 2.0.0 |
| 42 | OSD_ATTITUDE_ROLL | 2.0.0 |
| 43 | OSD_MAP_NORTH | 2.0.0 |
| 44 | OSD_MAP_TAKEOFF | 2.0.0 |
| 45 | OSD_RADAR | 2.0.0 |
| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 |
| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 |
| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 |
| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 |
| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 |
| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 |
| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 |
| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 |
| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 |
| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 |
| 56 | OSD_LEVEL_PIDS | 2.0.0 |
| 57 | OSD_POS_XY_PIDS | 2.0.0 |
| 58 | OSD_POS_Z_PIDS | 2.0.0 |
| 59 | OSD_VEL_XY_PIDS | 2.0.0 |
| 60 | OSD_VEL_Z_PIDS | 2.0.0 |
| 61 | OSD_HEADING_P | 2.0.0 |
| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 |
| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 |
| 64 | OSD_RC_EXPO | 2.0.0 |
| 65 | OSD_RC_YAW_EXPO | 2.0.0 |
| 66 | OSD_THROTTLE_EXPO | 2.0.0 |
| 67 | OSD_PITCH_RATE | 2.0.0 |
| 68 | OSD_ROLL_RATE | 2.0.0 |
| 69 | OSD_YAW_RATE | 2.0.0 |
| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 |
| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 |
| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 |
| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 |
| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 |
| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 |
| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 |
| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 |
| 78 | OSD_DEBUG | 2.0.0 |
| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 |
| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 |
| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 |
| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 |
| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 |
| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 |
| 85 | OSD_3D_SPEED | 2.1.0 |
| 86 | OSD_IMU_TEMPERATURE | 2.1.0 |
| 87 | OSD_BARO_TEMPERATURE | 2.1.0 |
| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 |
| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 |
| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 |
| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 |
| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 |
| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 |
| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 |
| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 |
| 96 | OSD_ALTITUDE_MSL | 2.1.0 |
| 97 | OSD_PLUS_CODE | 2.1.0 |
| 98 | OSD_MAP_SCALE | 2.2.0 |
| 99 | OSD_MAP_REFERENCE | 2.2.0 |
| 100 | OSD_GFORCE | 2.2.0 |
| 101 | OSD_GFORCE_X | 2.2.0 |
| 102 | OSD_GFORCE_Y | 2.2.0 |
| 103 | OSD_GFORCE_Z | 2.2.0 |
| 104 | OSD_RC_SOURCE | 2.2.0 |
| 105 | OSD_VTX_POWER | 2.2.0 |
| 106 | OSD_ESC_RPM | 2.3.0 |
| 107 | OSD_ESC_TEMPERATURE | 2.5.0 |
| 108 | OSD_AZIMUTH | 2.6.0 |
| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 |
| 110 | OSD_CRSF_LQ | 2.6.0 |
| 111 | OSD_CRSF_SNR_DB | 2.6.0 |
| 112 | OSD_CRSF_TX_POWER | 2.6.0 |
| 113 | OSD_GVAR_0 | 2.6.0 |
| 114 | OSD_GVAR_1 | 2.6.0 |
| 115 | OSD_GVAR_2 | 2.6.0 |
| 116 | OSD_GVAR_3 | 2.6.0 |
| 117 | OSD_TPA | 2.6.0 |
| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 |
| 119 | OSD_VERSION | 3.0.0 |
| 120 | OSD_RANGEFINDER | 3.0.0 |
| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 |
| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 |
| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 |
| 124 | OSD_GLIDESLOPE | 3.0.1 |
| 125 | OSD_GPS_MAX_SPEED | 4.0.0 |
| 126 | OSD_3D_MAX_SPEED | 4.0.0 |
| 127 | OSD_AIR_MAX_SPEED | 4.0.0 |
| 128 | OSD_ACTIVE_PROFILE | 4.0.0 |
| 129 | OSD_MISSION | 4.0.0 |
| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 |
| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 |
| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 |
| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 |
| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 |
| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 |
| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 |
| 137 | OSD_GLIDE_RANGE | 6.0.0 |
| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 |
| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 |
| 140 | OSD_GROUND_COURSE | 6.0.0 |
| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 |
| 142 | OSD_PILOT_NAME | 6.0.0 |
| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 |

40
docs/Settings.md
View file

@ -1372,6 +1372,16 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in
---
### gps_auto_baud_max_supported
Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0
| Default | Min | Max |
| --- | --- | --- |
| 230400 | | |
---
### gps_auto_config
Enable automatic configuration of UBlox GPS receivers.
@ -1432,6 +1442,16 @@ Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximu
---
### gps_ublox_use_beidou
Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON].
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### gps_ublox_use_galileo
Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON].
@ -1442,6 +1462,16 @@ Enable use of Galileo satellites. This is at the expense of other regional const
---
### gps_ublox_use_glonass
Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON].
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### ground_test_mode
For developer ground test use. Disables motors, sets heading status = Trusted on FW.
@ -3192,6 +3222,16 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal
---
### nav_landing_bump_detection
Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### nav_manual_climb_rate
Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]

BIN
docs/assets/images/StickPositions.png
View file

Binary file not shown.
Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 773 KiB

After

Width:  |  Height:  |  Size: 774 KiB

Before After
Before After

53
docs/development/How to test a pull request.md Normal file
View file

@ -0,0 +1,53 @@
# Introduction
While many of the instructions here are somewhat generic and will likely work for other projects, the goal of these instructions is to assist a non-developer INAV user to acquire firmware that includes a pull request so he can flash it on his supported fc.
Building the pull request manually or using custom/unofficial targets is not the focus of this document.
# Why should you test a pull request?
- You want to volunteer time and resources helping improving INAV for everyone by catching issues before they are introduced in the next release of INAV!
- You reported or are affected by a bug that has been addressed by this pull request and want to help test the fix
- You are interested in testing a new feature implemented by this pull request
# Why should you not test a pull request?
- Pull requests are beta code and may have bugs; bugs may cause you to crash and damage your model
- Upgrading from the stable version of INAV may require changes to your config that are not yet fully documented
# Before you proceed
- Read the comments on the pull request you want to test. It may provide useful context and information on known issues, required configuration changes or what branch of the inav-configurator is required.
- Make sure the pull request has passed all checks, otherwise you may not have pre-compiled firmware images.
- Make a diff all backup of your existing INAV configuration.
- Take notes of what INAV target you are using.
- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](http://seyrsnys.myzen.co.uk/inav-configurator-next/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI.
# Finding the pull request
This is easy, but you will need to be logged in to your GitHub account.
Navigate to the INAV github project and click on [``Pull Requests``](https://github.com/iNavFlight/inav/pulls).
You can just scroll through the list to find a pull request you are interested in, or use the filter bar by typing the name of the pull request, or the number, if you know it.
![Search results](assets/pr_testing/pr_search_result.png)
Once you find the one you are looking for, go ahead an open it!
Click on the ``Checks`` tab
Click on ``Build firmware``, it should take you to the ``Actions`` tab.
![Search results](assets/pr_testing/build_firmware.png)
You should see a summary with a column saying ``Artifacts`` and a number. Click on the number to be taken to the list of artifacts.
![Search results](assets/pr_testing/actions_summary.png)
On the ``Artifacts`` list, there should be an artifact without SITL in its name.
![Search results](assets/pr_testing/artifact_listing.png)
Click on it to download the zip file containing pre-compiled firmware images for all INAV official targets. Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings.
# I have flashed the new firmware, what should I do next?
- You should configure your model, either manually from scratch, or by loading your diff file. Keep in mind that loading a diff file may not always work, as there may have been some other changes in INAV that require attention. But even if you start from scratch, there are usually many sections that are safe to copy over from your diff.
- Try to reproduce the bug reported or play around with the new feature.
- Once you are done testing, don't forget to report your results on the pull request. Both positive results and issues are valid and welcome feedback.

BIN
docs/development/assets/pr_testing/actions_summary.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 12 KiB

BIN
docs/development/assets/pr_testing/artifact_listing.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 71 KiB

BIN
docs/development/assets/pr_testing/build_firmware.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 32 KiB

BIN
docs/development/assets/pr_testing/pr_search_result.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 26 KiB

3
src/main/CMakeLists.txt
View file

@ -501,6 +501,7 @@ main_sources(COMMON_SRC
io/gps.c
io/gps.h
io/gps_ublox.c
io/gps_ublox_utils.c
io/gps_nmea.c
io/gps_msp.c
io/gps_fake.c
@ -533,6 +534,8 @@ main_sources(COMMON_SRC
io/vtx_ffpv24g.h
io/vtx_control.c
io/vtx_control.h
io/vtx_msp.c
io/vtx_msp.h
navigation/navigation.c
navigation/navigation.h

4
src/main/drivers/accgyro/accgyro_icm42605.c
View file

@ -232,8 +232,8 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
}
gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 0);
gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 0);
gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);
return true;
}

38
src/main/drivers/adc.c
View file

@ -47,6 +47,13 @@
#ifndef ADC_CHANNEL_4_INSTANCE
#define ADC_CHANNEL_4_INSTANCE ADC_INSTANCE
#endif
#ifndef ADC_CHANNEL_5_INSTANCE
#define ADC_CHANNEL_5_INSTANCE ADC_INSTANCE
#endif
#ifndef ADC_CHANNEL_6_INSTANCE
#define ADC_CHANNEL_6_INSTANCE ADC_INSTANCE
#endif
#ifdef USE_ADC
@ -99,7 +106,7 @@ uint16_t adcGetChannel(uint8_t function)
}
}
#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN)
#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) || defined(ADC_CHANNEL_5_PIN) || defined(ADC_CHANNEL_6_PIN)
static bool isChannelInUse(int channel)
{
for (int i = 0; i < ADC_FUNCTION_COUNT; i++) {
@ -111,7 +118,7 @@ static bool isChannelInUse(int channel)
}
#endif
#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN)
#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) || !defined(ADC_CHANNEL_5_PIN) || !defined(ADC_CHANNEL_6_PIN)
static void disableChannelMapping(int channel)
{
for (int i = 0; i < ADC_FUNCTION_COUNT; i++) {
@ -192,6 +199,33 @@ void adcInit(drv_adc_config_t *init)
disableChannelMapping(ADC_CHN_4);
#endif
#ifdef ADC_CHANNEL_5_PIN
if (isChannelInUse(ADC_CHN_5)) {
adcConfig[ADC_CHN_5].adcDevice = adcDeviceByInstance(ADC_CHANNEL_5_INSTANCE);
if (adcConfig[ADC_CHN_5].adcDevice != ADCINVALID) {
adcConfig[ADC_CHN_5].tag = IO_TAG(ADC_CHANNEL_5_PIN);
#if defined(USE_ADC_AVERAGING)
activeChannelCount[adcConfig[ADC_CHN_5].adcDevice] += 1;
#endif
}
}
#else
disableChannelMapping(ADC_CHN_5);
#endif
#ifdef ADC_CHANNEL_6_PIN
if (isChannelInUse(ADC_CHN_6)) {
adcConfig[ADC_CHN_6].adcDevice = adcDeviceByInstance(ADC_CHANNEL_6_INSTANCE);
if (adcConfig[ADC_CHN_6].adcDevice != ADCINVALID) {
adcConfig[ADC_CHN_6].tag = IO_TAG(ADC_CHANNEL_6_PIN);
#if defined(USE_ADC_AVERAGING)
activeChannelCount[adcConfig[ADC_CHN_6].adcDevice] += 1;
#endif
}
}
#else
disableChannelMapping(ADC_CHN_6);
#endif
adcHardwareInit(init);
}

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

@ -33,7 +33,9 @@ typedef enum {
ADC_CHN_2,
ADC_CHN_3,
ADC_CHN_4,
ADC_CHN_MAX = ADC_CHN_4,
ADC_CHN_5,
ADC_CHN_6,
ADC_CHN_MAX = ADC_CHN_6,
ADC_CHN_COUNT
} adcChannel_e;

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

@ -85,7 +85,7 @@ static i2cDevice_t i2cHardwareMap[I2CDEV_COUNT] = {
{ .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1L(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 },
{ .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1L(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 },
#if defined(USE_I2C_DEVICE_4)
{ .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1L(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
{ .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB4(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
#endif
#endif
};

10
src/main/drivers/pwm_mapping.c
View file

@ -195,6 +195,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
return true;
}
#endif
#if defined(ADC_CHANNEL_5_PIN)
if (timHw->tag == IO_TAG(ADC_CHANNEL_5_PIN)) {
return true;
}
#endif
#if defined(ADC_CHANNEL_6_PIN)
if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) {
return true;
}
#endif
#endif
return false;

7
src/main/drivers/sdcard/sdcard_spi.c
View file

@ -453,7 +453,6 @@ static bool sdcardSpi_poll(void)
doMore:
switch (sdcard.state) {
case SDCARD_STATE_RESET:
busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);
sdcardSpi_select();
initStatus = sdcardSpi_sendCommand(SDCARD_COMMAND_GO_IDLE_STATE, 0);
@ -476,8 +475,6 @@ static bool sdcardSpi_poll(void)
break;
case SDCARD_STATE_CARD_INIT_IN_PROGRESS:
busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);
if (sdcardSpi_checkInitDone()) {
if (sdcard.version == 2) {
// Check for high capacity card
@ -516,8 +513,6 @@ static bool sdcardSpi_poll(void)
busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
break;
case SDCARD_STATE_INITIALIZATION_RECEIVE_CID:
busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);
if (sdcardSpi_receiveCID()) {
sdcardSpi_deselect();
@ -881,8 +876,6 @@ void sdcardSpi_init(void)
sdcard.operationStartTime = millis();
sdcard.state = SDCARD_STATE_RESET;
sdcard.failureCount = 0;
busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
}
sdcardVTable_t sdcardSpiVTable = {

24
src/main/drivers/sdcard/sdmmc_sdio_h7xx.c
View file

@ -127,14 +127,22 @@ void sdioPinConfigure(void)
sdioHardware = &sdioPinHardware[SDCARD_SDIO_DEVICE];
sdioPin[SDIO_PIN_CK] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCK[0];
sdioPin[SDIO_PIN_CMD] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCMD[0];
sdioPin[SDIO_PIN_D0] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD0[0];
#ifdef SDCARD_SDIO2_CK_ALT
sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[1];
#else
sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[0];
#endif
#ifdef SDCARD_SDIO2_CMD_ALT
sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[1];
#else
sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[0];
#endif
sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0];
#ifdef SDCARD_SDIO_4BIT
sdioPin[SDIO_PIN_D1] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD1[0];
sdioPin[SDIO_PIN_D2] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD2[0];
sdioPin[SDIO_PIN_D3] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD3[0];
sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0];
sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0];
sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0];
#endif
}
@ -254,7 +262,11 @@ bool SD_Init(void)
hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested
#endif
hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE;
#ifdef SDCARD_SDIO_NORMAL_SPEED
hsd1.Init.ClockDiv = SDMMC_NSpeed_CLK_DIV;
#else
hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
#endif
status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit

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

@ -24,6 +24,7 @@
#include "platform.h"
#include "build/debug.h"
#include "common/log.h"
#include "vtx_common.h"
@ -73,8 +74,9 @@ void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t ch
if (!vtxDevice)
return;
if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount))
if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) {
return;
}
if (vtxDevice->vTable->setBandAndChannel) {
vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);

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

@ -36,7 +36,7 @@
#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)
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
#define VTX_SETTINGS_POWER_COUNT 5
#define VTX_SETTINGS_DEFAULT_POWER 1
@ -63,6 +63,7 @@ typedef enum {
VTXDEV_SMARTAUDIO = 3,
VTXDEV_TRAMP = 4,
VTXDEV_FFPV = 5,
VTXDEV_MSP = 6,
VTXDEV_UNKNOWN = 0xFF,
} vtxDevType_e;

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

@ -89,6 +89,7 @@ bool cliMode = false;
#include "io/beeper.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/gps_ublox.h"
#include "io/ledstrip.h"
#include "io/osd.h"
#include "io/serial.h"
@ -3468,6 +3469,23 @@ static void cliStatus(char *cmdline)
cliPrintLinefeed();
#endif
if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) {
cliPrint("GPS: ");
cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate());
cliPrintLinefeed();
//cliPrintLinef(" GNSS Capabilities: %d", gpsUbloxCapLastUpdate());
cliPrintLinef(" GNSS Capabilities:");
cliPrintLine(" GNSS Provider active/default");
cliPrintLine(" GPS 1/1");
if(gpsUbloxHasGalileo())
cliPrintLinef(" Galileo %d/%d", gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault());
if(gpsUbloxHasBeidou())
cliPrintLinef(" BeiDou %d/%d", gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault());
if(gpsUbloxHasGlonass())
cliPrintLinef(" Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault());
cliPrintLinef(" Max concurrent: %d", gpsUbloxMaxGnss());
}
// If we are blocked by PWM init - provide more information
if (getPwmInitError() != PWM_INIT_ERROR_NONE) {
cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());

7
src/main/fc/fc_init.c
View file

@ -120,6 +120,7 @@
#include "io/vtx_control.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_tramp.h"
#include "io/vtx_msp.h"
#include "io/vtx_ffpv24g.h"
#include "io/piniobox.h"
@ -175,7 +176,7 @@ void flashLedsAndBeep(void)
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getPreferredBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
BEEP_ON;
delay(25);
BEEP_OFF;
@ -670,6 +671,10 @@ void init(void)
vtxFuriousFPVInit();
#endif
#ifdef USE_VTX_MSP
vtxMspInit();
#endif
#endif // USE_VTX_CONTROL
// Now that everything has powered up the voltage and cell count be determined.

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

@ -2482,12 +2482,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel
const uint8_t newBand = (newFrequency / 8) + 1;
const uint8_t newChannel = (newFrequency % 8) + 1;
if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
}
vtxSettingsConfigMutable()->band = newBand;
vtxSettingsConfigMutable()->channel = newChannel;
}
if (sbufBytesRemaining(src) > 1) {
vtxSettingsConfigMutable()->power = sbufReadU8(src);
uint8_t newPower = sbufReadU8(src);
uint8_t currentPower = 0;
vtxCommonGetPowerIndex(vtxDevice, &currentPower);
if (newPower != currentPower) {
vtxCommonSetPowerByIndex(vtxDevice, newPower);
vtxSettingsConfigMutable()->power = newPower;
}
// Delegate pitmode to vtx directly
const uint8_t newPitmode = sbufReadU8(src);
uint8_t currentPitmode = 0;
@ -2526,6 +2538,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSol.flags.validEPE = false;
gpsSol.flags.validTime = false;
gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
@ -3535,6 +3548,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
gpsSol.flags.validVelNE = true;
gpsSol.flags.validVelD = true;
gpsSol.flags.validEPE = true;
gpsSol.flags.validTime = false;
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
@ -3589,7 +3603,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (sensors(SENSOR_MAG)) {
mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT
mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;
mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero
mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
} else {
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
@ -3607,6 +3621,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
simulatorData.airSpeed = sbufReadU16(src);
} else {
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
sbufReadU16(src);
}
}
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
}
} else {
DISABLE_STATE(GPS_FIX);

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

@ -64,6 +64,7 @@
#include "io/rcdevice_cam.h"
#include "io/smartport_master.h"
#include "io/vtx.h"
#include "io/vtx_msp.h"
#include "io/osd_dji_hd.h"
#include "io/displayport_msp_osd.h"
#include "io/servo_sbus.h"
@ -109,6 +110,9 @@ void taskHandleSerial(timeUs_t currentTimeUs)
#ifdef USE_MSP_OSD
// Capture MSP Displayport messages to determine if VTX is connected
mspOsdSerialProcess(mspFcProcessCommand);
#ifdef USE_VTX_MSP
mspVtxSerialProcess(mspFcProcessCommand);
#endif
#endif
}

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

@ -181,7 +181,10 @@ typedef enum {
HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV)
HITL_HAS_NEW_GPS_DATA = (1 << 4),
HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2
HITL_AIRSPEED = (1 << 6)
HITL_AIRSPEED = (1 << 6),
HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2
HITL_GPS_TIMEOUT = (1 << 8),
HITL_PITOT_FAILURE = (1 << 9)
} simulatorFlags_t;
typedef struct {

25
src/main/fc/settings.yaml
View file

@ -190,6 +190,9 @@ tables:
- name: nav_fw_wp_turn_smoothing
values: ["OFF", "ON", "ON-CUT"]
enum: wpFwTurnSmoothing_e
- name: gps_auto_baud_max
values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600']
enum: gpsBaudRate_e
constants:
RPYL_PID_MIN: 0
@ -1560,6 +1563,7 @@ groups:
min: 1
max: 3000
- name: PG_GPS_CONFIG
headers: [ "io/gps.h" ]
type: gpsConfig_t
condition: USE_GPS
members:
@ -1591,11 +1595,27 @@ groups:
default_value: ON
field: autoBaud
type: bool
- name: gps_auto_baud_max_supported
description: "Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0"
default_value: "230400"
table: gps_auto_baud_max
field: autoBaudMax
type: uint8_t
- name: gps_ublox_use_galileo
description: "Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]."
default_value: OFF
field: ubloxUseGalileo
type: bool
- name: gps_ublox_use_beidou
description: "Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]."
default_value: OFF
field: ubloxUseBeidou
type: bool
- name: gps_ublox_use_glonass
description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]."
default_value: OFF
field: ubloxUseGlonass
type: bool
- name: gps_min_sats
description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count."
default_value: 6
@ -2354,6 +2374,11 @@ groups:
field: general.land_detect_sensitivity
min: 1
max: 15
- name: nav_landing_bump_detection
description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors."
default_value: OFF
field: general.flags.landing_bump_detection
type: bool
- name: nav_use_midthr_for_althold
description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude"
default_value: OFF

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

@ -154,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;
@ -625,7 +625,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
}
}
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
// Apply simple LPF to angleRateTarget to make response less jerky
// Ideas behind this:
@ -681,13 +681,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
}
#ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) {
static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
float dBoost = 1.0f;
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv;
const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT);
const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv);
if (dBoostGyroAcceleration >= dBoostRateAcceleration) {
//Gyro is accelerating faster than setpoint, we want to smooth out
@ -710,7 +710,7 @@ static float applyDBoost(pidState_t *pidState, float dT) {
}
#endif
static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) {
static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
// Calculate new D-term
float newDTerm = 0;
if (pidState->kD == 0) {
@ -722,7 +722,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT);
newDTerm = delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
}
return(newDTerm);
}
@ -736,19 +736,20 @@ static void applyItermLimiting(pidState_t *pidState) {
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
UNUSED(dT_inv);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
const float newFFTerm = rateTarget * pidState->kFF;
/*
@ -806,14 +807,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
return itermErrorRate;
}
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
{
const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
const float rateError = rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT);
const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
const float rateTargetDelta = rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -1062,6 +1063,9 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
void FAST_CODE pidController(float dT)
{
const float dT_inv = 1.0f / dT;
if (!pidFiltersConfigured) {
return;
}
@ -1135,7 +1139,7 @@ void FAST_CODE pidController(float dT)
}
// Prevent strong Iterm accumulation during stick inputs
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);
for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits
@ -1145,7 +1149,7 @@ void FAST_CODE pidController(float dT)
checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis);
pidControllerApplyFn(&pidState[axis], axis, dT);
pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
}
}
@ -1177,7 +1181,7 @@ void pidInit(void)
itermRelax = pidProfile()->iterm_relax;
yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));
#ifdef USE_D_BOOST
dBoostMin = pidProfile()->dBoostMin;

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

@ -47,7 +47,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
#define FP_PID_RATE_I_MULTIPLIER 4.0f
#define FP_PID_RATE_D_MULTIPLIER 1905.0f
#define FP_PID_RATE_D_FF_MULTIPLIER 7270.0f
#define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_LEVEL_P_MULTIPLIER 1.0f / 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
#define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
#define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f

2
src/main/flight/pid_autotune.c
View file

@ -187,7 +187,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
// Use different max rate in ANLGE mode
if (FLIGHT_MODE(ANGLE_MODE)) {
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER;
maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
}

2
src/main/io/beeper.c
View file

@ -218,7 +218,7 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL;
*/
void beeper(beeperMode_e mode)
{
if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (getBatteryCellCount() < 2))) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)) {
if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (getBatteryState() == BATTERY_NOT_PRESENT))) || IS_RC_MODE_ACTIVE(BOXBEEPERMUTE)) {
beeperSilence();
return;
}

9
src/main/io/displayport_msp_osd.c
View file

@ -515,4 +515,13 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
}
}
mspPort_t *getMspOsdPort()
{
if (mspPort.port) {
return &mspPort;
}
return NULL;
}
#endif // USE_MSP_OSD

1
src/main/io/displayport_msp_osd.h
View file

@ -31,6 +31,7 @@ typedef struct displayPort_s displayPort_t;
displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem);
void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
mspPort_t *getMspOsdPort(void);
// MSP displayport V2 attribute byte bit functions
#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e

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

@ -51,6 +51,7 @@
#include "io/serial.h"
#include "io/gps.h"
#include "io/gps_private.h"
#include "io/gps_ublox.h"
#include "navigation/navigation.h"
@ -73,7 +74,7 @@ gpsStatistics_t gpsStats;
gpsSolutionData_t gpsSol;
// 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 };
baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 };
static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
/* NMEA GPS */
@ -112,7 +113,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
};
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4);
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.provider = SETTING_GPS_PROVIDER_DEFAULT,
@ -122,9 +123,73 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.dynModel = SETTING_GPS_DYN_MODEL_DEFAULT,
.gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT,
.ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT,
.ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT
.ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT,
.ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT,
.ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT,
.autoBaudMax = SETTING_GPS_AUTO_BAUD_MAX_SUPPORTED_DEFAULT
);
int gpsBaudRateToInt(gpsBaudRate_e baudrate)
{
switch(baudrate)
{
case GPS_BAUDRATE_115200:
return 115200;
case GPS_BAUDRATE_57600:
return 57600;
case GPS_BAUDRATE_38400:
return 38400;
case GPS_BAUDRATE_19200:
return 19200;
case GPS_BAUDRATE_9600:
return 9600;
case GPS_BAUDRATE_230400:
return 230400;
case GPS_BAUDRATE_460800:
return 460800;
case GPS_BAUDRATE_921600:
return 921600;
default:
return 0;
}
}
int getGpsBaudrate(void)
{
return gpsBaudRateToInt(gpsState.baudrateIndex);
}
const char *getGpsHwVersion(void)
{
switch(gpsState.hwVersion)
{
case UBX_HW_VERSION_UBLOX5:
return "UBLOX5";
case UBX_HW_VERSION_UBLOX6:
return "UBLOX6";
case UBX_HW_VERSION_UBLOX7:
return "UBLOX7";
case UBX_HW_VERSION_UBLOX8:
return "UBLOX8";
case UBX_HW_VERSION_UBLOX9:
return "UBLOX9";
case UBX_HW_VERSION_UBLOX10:
return "UBLOX10";
default:
return "Unknown";
}
}
uint8_t getGpsProtoMajorVersion(void)
{
return gpsState.swVersionMajor;
}
uint8_t getGpsProtoMinorVersion(void)
{
return gpsState.swVersionMinor;
}
void gpsSetState(gpsState_e state)
{
gpsState.state = state;
@ -287,10 +352,21 @@ bool gpsUpdate(void)
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
gpsUpdateTime();
if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT))
{
gpsSetState(GPS_LOST_COMMUNICATION);
sensorsClear(SENSOR_GPS);
gpsStats.timeouts = 5;
return false;
}
else
{
gpsSetState(GPS_RUNNING);
sensorsSet(SENSOR_GPS);
return gpsSol.flags.hasNewData;
bool res = gpsSol.flags.hasNewData;
gpsSol.flags.hasNewData = false;
return res;
}
}
#endif

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

@ -59,6 +59,8 @@ typedef enum {
GPS_BAUDRATE_19200,
GPS_BAUDRATE_9600,
GPS_BAUDRATE_230400,
GPS_BAUDRATE_460800,
GPS_BAUDRATE_921600,
GPS_BAUDRATE_COUNT
} gpsBaudRate_e;
@ -93,8 +95,11 @@ typedef struct gpsConfig_s {
gpsAutoBaud_e autoBaud;
gpsDynModel_e dynModel;
bool ubloxUseGalileo;
bool ubloxUseBeidou;
bool ubloxUseGlonass;
uint8_t gpsMinSats;
uint8_t ubloxNavHz;
gpsBaudRate_e autoBaudMax;
} gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig);
@ -166,6 +171,13 @@ struct serialPort_s;
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
void mspGPSReceiveNewData(const uint8_t * bufferPtr);
const char *getGpsHwVersion(void);
uint8_t getGpsProtoMajorVersion(void);
uint8_t getGpsProtoMinorVersion(void);
int getGpsBaudrate(void);
int gpsBaudRateToInt(gpsBaudRate_e baudrate);
#if defined(USE_GPS_FAKE)
void gpsFakeSet(
gpsFixType_e fixType,

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

@ -43,6 +43,8 @@ typedef struct {
serialPort_t * gpsPort; // Serial GPS only
uint32_t hwVersion;
uint8_t swVersionMajor;
uint8_t swVersionMinor;
gpsState_e state;
gpsBaudRate_e baudrateIndex;
@ -54,6 +56,8 @@ typedef struct {
timeMs_t lastMessageMs;
timeMs_t timeoutMs;
timeMs_t baseTimeoutMs;
timeMs_t lastCapaPoolMs;
timeMs_t lastCapaUpdMs;
} gpsReceiverData_t;
extern gpsReceiverData_t gpsState;

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

@ -42,6 +42,7 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "io/serial.h"
#include "io/gps.h"
@ -49,31 +50,9 @@
#include "scheduler/protothreads.h"
#define GPS_CFG_CMD_TIMEOUT_MS 200
#define GPS_VERSION_RETRY_TIMES 2
#define MAX_UBLOX_PAYLOAD_SIZE 256
#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
#define UBLOX_SBAS_MESSAGE_LENGTH 16
#include "gps_ublox.h"
#include "gps_ublox_utils.h"
#define UBX_DYNMODEL_PEDESTRIAN 3
#define UBX_DYNMODEL_AIR_1G 6
#define UBX_DYNMODEL_AIR_4G 8
#define UBX_FIXMODE_2D_ONLY 1
#define UBX_FIXMODE_3D_ONLY 2
#define UBX_FIXMODE_AUTO 3
#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
#define UBX_HW_VERSION_UNKNOWN 0
#define UBX_HW_VERSION_UBLOX5 500
#define UBX_HW_VERSION_UBLOX6 600
#define UBX_HW_VERSION_UBLOX7 700
#define UBX_HW_VERSION_UBLOX8 800
#define UBX_HW_VERSION_UBLOX9 900
#define UBX_HW_VERSION_UBLOX10 1000
// SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
// note PRNs last upadted 2020-12-18
@ -97,255 +76,10 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200
"$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600
"$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400
"$PUBX,41,1,0003,0001,460800,0*1C\r\n", // GPS_BAUDRATE_460800
"$PUBX,41,1,0003,0001,921600,0*1C\r\n" // GPS_BAUDRATE_921600
};
// payload types
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_sbas;
typedef struct {
uint8_t class;
uint8_t id;
uint8_t rate;
} ubx_msg;
typedef struct {
uint16_t meas;
uint16_t nav;
uint16_t time;
} ubx_rate;
typedef struct {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
// flags
uint8_t enabled;
uint8_t undefined0;
uint8_t sigCfgMask;
uint8_t undefined1;
} ubx_gnss_element_t;
typedef struct {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
ubx_gnss_element_t config[0];
} ubx_gnss_msg_t;
#define MAX_GNSS 7
#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
typedef union {
uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
ubx_sbas sbas;
ubx_msg msg;
ubx_rate rate;
ubx_gnss_msg_t gnss;
} ubx_payload;
// UBX support
typedef struct {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
typedef struct {
ubx_header header;
ubx_payload payload;
} __attribute__((packed)) ubx_message;
typedef struct {
char swVersion[30]; // Zero-terminated Software Version String
char hwVersion[10]; // Zero-terminated Hardware Version String
} ubx_mon_ver;
typedef struct {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
} ubx_nav_posllh;
typedef struct {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
} ubx_nav_status;
typedef struct {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} ubx_nav_solution;
typedef struct {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct {
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
typedef struct {
uint32_t time; // GPS msToW
uint32_t tAcc;
int32_t nano;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
} ubx_nav_timeutc;
typedef struct {
uint32_t time; // GPS msToW
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fix_type;
uint8_t fix_status;
uint8_t reserved1;
uint8_t satellites;
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
int32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
uint16_t position_DOP;
uint16_t reserved2;
uint16_t reserved3;
} ubx_nav_pvt;
typedef struct {
uint8_t class;
uint8_t msg;
} ubx_ack_ack;
enum {
PREAMBLE1 = 0xB5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
MSG_CLASS_UBX = 0x01,
MSG_CLASS_NMEA = 0xF0,
MSG_VER = 0x04,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_NMEA_GGA = 0x0,
MSG_NMEA_GLL = 0x1,
MSG_NMEA_GSA = 0x2,
MSG_NMEA_GSV = 0x3,
MSG_NMEA_RMC = 0x4,
MSG_NMEA_VGS = 0x5,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_VELNED = 0x12,
MSG_TIMEUTC = 0x21,
MSG_SVINFO = 0x30,
MSG_NAV_SAT = 0x35,
MSG_NAV_SIG = 0x35,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3e
} ubx_protocol_bytes;
enum {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
} ubs_nav_fix_type;
enum {
NAV_STATUS_FIX_VALID = 1
} ubx_nav_status_bits;
enum {
UBX_ACK_WAITING = 0,
UBX_ACK_GOT_ACK = 1,
UBX_ACK_GOT_NAK = 2
} ubx_ack_state;
// Packet checksum accumulators
static uint8_t _ck_a;
static uint8_t _ck_b;
@ -369,7 +103,12 @@ static bool _new_position;
static bool _new_speed;
// Need this to determine if Galileo capable only
static bool capGalileo;
static struct {
uint8_t supported;
int capMaxGnss;
uint8_t defaultGnss;
uint8_t enabledGnss;
} ubx_capabilities = { };
// Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
//15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
@ -401,16 +140,63 @@ static union {
ubx_mon_ver ver;
ubx_nav_timeutc timeutc;
ubx_ack_ack ack;
ubx_mon_gnss gnss;
uint8_t bytes[UBLOX_BUFFER_SIZE];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
bool gpsUbloxHasGalileo(void)
{
while (len--) {
*ck_a += *data;
*ck_b += *ck_a;
data++;
}
return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
}
bool gpsUbloxHasBeidou(void)
{
return ubx_capabilities.supported & UBX_MON_GNSS_BEIDOU_MASK;
}
bool gpsUbloxHasGlonass(void)
{
return ubx_capabilities.supported & UBX_MON_GNSS_GLONASS_MASK;
}
bool gpsUbloxGalileoDefault(void)
{
return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GALILEO_MASK;
}
bool gpsUbloxBeidouDefault(void)
{
return ubx_capabilities.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK;
}
bool gpsUbloxGlonassDefault(void)
{
return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GLONASS_MASK;
}
bool gpsUbloxGalileoEnabled(void)
{
return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GALILEO_MASK;
}
bool gpsUbloxBeidouEnabled(void)
{
return ubx_capabilities.enabledGnss & UBX_MON_GNSS_BEIDOU_MASK;
}
bool gpsUbloxGlonassEnabled(void)
{
return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GLONASS_MASK;
}
uint8_t gpsUbloxMaxGnss(void)
{
return ubx_capabilities.capMaxGnss;
}
timeMs_t gpsUbloxCapLastUpdate(void)
{
return gpsState.lastCapaUpdMs;
}
static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
@ -427,7 +213,7 @@ static void sendConfigMessageUBLOX(void)
uint8_t ck_a=0, ck_b=0;
send_buffer.message.header.preamble1=PREAMBLE1;
send_buffer.message.header.preamble2=PREAMBLE2;
_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
ublox_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
send_buffer.bytes[send_buffer.message.header.length+6] = ck_a;
send_buffer.bytes[send_buffer.message.header.length+7] = ck_b;
serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8);
@ -445,6 +231,15 @@ static void pollVersion(void)
sendConfigMessageUBLOX();
}
static void pollGnssCapabilities(void)
{
send_buffer.message.header.msg_class = CLASS_MON;
send_buffer.message.header.msg_id = MSG_MON_GNSS;
send_buffer.message.header.length = 0;
sendConfigMessageUBLOX();
}
static const uint8_t default_payload[] = {
0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code)
0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
@ -454,7 +249,26 @@ static const uint8_t default_payload[] = {
#define GNSSID_SBAS 1
#define GNSSID_GALILEO 2
#define GNSSID_BEIDOU 3
#define GNSSID_GZSS 5
#define GNSSID_GLONASS 6
// M10 ublox protocol info:
// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf
static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count)
{
ubx_config_data8_t cfg = {};
ubloxCfgFillBytes(&cfg, kvPairs, count);
serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8);
_ack_waiting_msg = cfg.header.msg_id;
_ack_state = UBX_ACK_WAITING;
}
// Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration
// https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
// https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
{
gnss_block->gnssId = GNSSID_SBAS;
@ -473,13 +287,17 @@ static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
{
if (!capGalileo) {
if (!gpsUbloxHasGalileo()) {
return 0;
}
gnss_block->gnssId = GNSSID_GALILEO;
gnss_block->maxTrkCh = 8;
gnss_block->sigCfgMask = 1;
// sigCfgMask
// 0x01 = Galileo E1 (not supported for protocol versions less than 18.00)
// 0x10 = Galileo E5a // off by default
// 0x20 = Galileo E5b // off by default
gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseGalileo) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
@ -491,15 +309,109 @@ static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
return 1;
}
static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block)
{
if (!gpsUbloxHasBeidou()) {
return 0;
}
gnss_block->gnssId = GNSSID_BEIDOU;
gnss_block->maxTrkCh = 8;
// sigCfgMask
// 0x01 = BeiDou B1I
// 0x10 = BeiDou B2I // off by default
// 0x80 = BeiDou B2A // off by default
gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseBeidou) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
} else {
gnss_block->enabled = 0;
gnss_block->resTrkCh = 0;
}
return 1;
}
/*
static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block)
{
if (!ubx_capabilities.capGzss) {
return 0;
}
gnss_block->gnssId = GNSSID_GZSS;
gnss_block->maxTrkCh = 8;
// L1C = 0x01
// L1S = 0x04
// L2C = 0x10
gnss_block->sigCfgMask = 0x01 | 0x04;
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
return 1;
}
*/
static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block)
{
if(!gpsUbloxHasGlonass()) {
return 0;
}
gnss_block->gnssId = GNSSID_GLONASS;
gnss_block->maxTrkCh = 8;
// 0x01 = GLONASS L1
// 0x10 = GLONASS L2 // off by default
gnss_block->sigCfgMask = 0x01;
if (gpsState.gpsConfig->ubloxUseGlonass) {
gnss_block->enabled = 1;
gnss_block->resTrkCh = 4;
} else {
gnss_block->enabled = 0;
gnss_block->resTrkCh = 0;
}
return 1;
}
static void configureGNSS10(void)
{
ubx_config_data8_payload_t gnssConfigValues[] = {
// SBAS
{UBLOX_CFG_SIGNAL_SBAS_ENA, 1},
{UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1},
// Galileo
{UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo},
{UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo},
// Beidou
{UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou},
{UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou},
{UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0},
// Should be enabled with GPS
{UBLOX_CFG_QZSS_ENA, 1},
{UBLOX_CFG_QZSS_L1CA_ENA, 1},
{UBLOX_CFG_QZSS_L1S_ENA, 1},
// Glonass
{UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass},
{UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass}
};
ubloxSendSetCfgBytes(gnssConfigValues, 12);
}
static void configureGNSS(void)
{
int blocksUsed = 0;
send_buffer.message.header.msg_class = CLASS_CFG;
send_buffer.message.header.msg_id = MSG_CFG_GNSS;
send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET
send_buffer.message.payload.gnss.msgVer = 0;
send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
send_buffer.message.payload.gnss.numTrkChUse = 32;
send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max
/* SBAS, always generated */
blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
@ -507,8 +419,14 @@ static void configureGNSS(void)
/* Galileo */
blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
/* BeiDou */
blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]);
/* GLONASS */
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();
}
@ -523,12 +441,12 @@ static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
sendConfigMessageUBLOX();
}
static void configureMSG(uint8_t class, uint8_t id, uint8_t rate)
static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate)
{
send_buffer.message.header.msg_class = CLASS_CFG;
send_buffer.message.header.msg_id = MSG_CFG_SET_RATE;
send_buffer.message.header.length = 3;
send_buffer.message.payload.msg.class = class;
send_buffer.message.payload.msg.msg_class = msg_class;
send_buffer.message.payload.msg.id = id;
send_buffer.message.payload.msg.rate = rate;
sendConfigMessageUBLOX();
@ -565,6 +483,18 @@ static void configureSBAS(void)
sendConfigMessageUBLOX();
}
static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength)
{
if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) {
proto+=8;
float ver = atof(proto);
gpsState.swVersionMajor = (uint8_t)ver;
gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f);
}
}
static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
{
// ublox_5 hwVersion 00040005
@ -600,7 +530,7 @@ static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
return UBX_HW_VERSION_UNKNOWN;
}
static bool gpsParceFrameUBLOX(void)
static bool gpsParseFrameUBLOX(void)
{
switch (_msg_id) {
case MSG_POSLLH:
@ -690,18 +620,51 @@ static bool gpsParceFrameUBLOX(void)
case MSG_VER:
if (_class == CLASS_MON) {
gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) {
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
if (_buffer.ver.swVersion[9] > '2' || true) {
// check extensions;
// after hw + sw vers; each is 30 bytes
for(int j = 40; j < _payload_length; j += 30) {
if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
capGalileo = true;
bool found = false;
for (int j = 40; j < _payload_length && !found; j += 30)
{
// Example content: GPS;GAL;BDS;GLO
if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30))
{
ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK;
found = true;
}
if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30))
{
ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK;
found = true;
}
if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30))
{
ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK;
found = true;
}
}
}
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;
}
}
}
}
break;
case MSG_MON_GNSS:
if(_class == CLASS_MON) {
if (_buffer.gnss.version == 0) {
ubx_capabilities.supported = _buffer.gnss.supported;
ubx_capabilities.defaultGnss = _buffer.gnss.defaultGnss;
ubx_capabilities.enabledGnss = _buffer.gnss.enabled;
ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent;
gpsState.lastCapaUpdMs = millis();
}
}
break;
case MSG_ACK_ACK:
if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_ACK;
@ -808,7 +771,7 @@ static bool gpsNewFrameUBLOX(uint8_t data)
break;
}
if (gpsParceFrameUBLOX()) {
if (gpsParseFrameUBLOX()) {
parsed = true;
}
}
@ -893,12 +856,13 @@ STATIC_PROTOTHREAD(gpsConfigure)
if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
} else {
configureRATE(200); // 5Hz
configureRATE(hz2rate(5)); // 5Hz
gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT;
}
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
configureRATE(200); // 5Hz
configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
@ -931,12 +895,12 @@ STATIC_PROTOTHREAD(gpsConfigure)
configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
}
else {
configureRATE(200); // 5Hz
configureRATE(hz2rate(5)); // 5Hz
}
ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
configureRATE(200); // 5Hz
configureRATE(hz2rate(5)); // 5Hz
ptWait(_ack_state == UBX_ACK_GOT_ACK);
}
}
@ -979,10 +943,20 @@ STATIC_PROTOTHREAD(gpsConfigure)
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
// Configure GNSS for M8N and later
if (gpsState.hwVersion >= 80000) {
if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) {
configureGNSS10();
} else {
configureGNSS();
}
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
if(_ack_state == UBX_ACK_GOT_NAK) {
gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
}
}
ptEnd(0);
@ -1023,6 +997,11 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
// Try sending baud rate switch command at all common baud rates
gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT));
for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) {
if (gpsBaudRateToInt(gpsState.autoBaudrateIndex) > gpsBaudRateToInt(gpsState.gpsConfig->autoBaudMax)) {
// trying higher baud rates fails on m8 gps
// autoBaudRateIndex is not sorted by baud rate
continue;
}
// 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex]
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]);
@ -1044,8 +1023,6 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
}
// Configure GPS module if enabled
if (gpsState.gpsConfig->autoConfig) {
// Reset protocol timeout
gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
@ -1057,8 +1034,18 @@ 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;
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
ptSpawn(gpsConfigure);
}
@ -1070,6 +1057,21 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
while (1) {
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData();
if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {
gpsState.lastCapaPoolMs = millis();
if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN)
{
pollVersion();
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
}
pollGnssCapabilities();
ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
}
}
}
ptEnd(0);

429
src/main/io/gps_ublox.h Normal file
View file

@ -0,0 +1,429 @@
/*
* This file is part of INAV
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "common/time.h"
#ifdef __cplusplus
extern "C" {
#endif
#define GPS_CFG_CMD_TIMEOUT_MS 500
#define GPS_VERSION_RETRY_TIMES 3
#define MAX_UBLOX_PAYLOAD_SIZE 256
#define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
#define UBLOX_SBAS_MESSAGE_LENGTH 16
#define GPS_CAPA_INTERVAL 5000
#define UBX_DYNMODEL_PEDESTRIAN 3
#define UBX_DYNMODEL_AIR_1G 6
#define UBX_DYNMODEL_AIR_4G 8
#define UBX_FIXMODE_2D_ONLY 1
#define UBX_FIXMODE_3D_ONLY 2
#define UBX_FIXMODE_AUTO 3
#define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
#define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
#define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
#define UBX_HW_VERSION_UNKNOWN 0
#define UBX_HW_VERSION_UBLOX5 500
#define UBX_HW_VERSION_UBLOX6 600
#define UBX_HW_VERSION_UBLOX7 700
#define UBX_HW_VERSION_UBLOX8 800
#define UBX_HW_VERSION_UBLOX9 900
#define UBX_HW_VERSION_UBLOX10 1000
#define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1
#define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1
#define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 // U1
#define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 // U1
#define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 // U1
#define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d // U1
#define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // U1 default off
#define UBLOX_CFG_QZSS_ENA 0x10310024 // U1
#define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 // U1
#define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 // U1
#define UBLOX_CFG_GLO_ENA 0x10310025 // U1 default off - may conflict with other constelations
#define UBLOX_CFG_GLO_L1_ENA 0x10310018 // U1 default off
#define UBLOX_CFG_SBAS_PRNSCANMASK 0x50360006 // 0 = auto // X8
#define UBLOX_SBAS_ALL 0x0000000000000000 //Enable search for all SBAS PRNs
#define UBLOX_SBAS_PRN120 0x0000000000000001 //Enable search for SBAS PRN120
#define UBLOX_SBAS_PRN121 0x0000000000000002 //Enable search for SBAS PRN121
#define UBLOX_SBAS_PRN122 0x0000000000000004 //Enable search for SBAS PRN122
#define UBLOX_SBAS_PRN123 0x0000000000000008 //Enable search for SBAS PRN123
#define UBLOX_SBAS_PRN124 0x0000000000000010 //Enable search for SBAS PRN124
#define UBLOX_SBAS_PRN125 0x0000000000000020 //Enable search for SBAS PRN125
#define UBLOX_SBAS_PRN126 0x0000000000000040 //Enable search for SBAS PRN126
#define UBLOX_SBAS_PRN127 0x0000000000000080 //Enable search for SBAS PRN127
#define UBLOX_SBAS_PRN128 0x0000000000000100 //Enable search for SBAS PRN128
#define UBLOX_SBAS_PRN129 0x0000000000000200 //Enable search for SBAS PRN129
#define UBLOX_SBAS_PRN130 0x0000000000000400 //Enable search for SBAS PRN130
#define UBLOX_SBAS_PRN131 0x0000000000000800 //Enable search for SBAS PRN131
#define UBLOX_SBAS_PRN132 0x0000000000001000 //Enable search for SBAS PRN132
#define UBLOX_SBAS_PRN133 0x0000000000002000 //Enable search for SBAS PRN133
#define UBLOX_SBAS_PRN134 0x0000000000004000 //Enable search for SBAS PRN134
#define UBLOX_SBAS_PRN135 0x0000000000008000 //Enable search for SBAS PRN135
#define UBLOX_SBAS_PRN136 0x0000000000010000 //Enable search for SBAS PRN136
#define UBLOX_SBAS_PRN137 0x0000000000020000 //Enable search for SBAS PRN137
#define UBLOX_SBAS_PRN138 0x0000000000040000 //Enable search for SBAS PRN138
#define UBLOX_SBAS_PRN139 0x0000000000080000 //Enable search for SBAS PRN139
#define UBLOX_SBAS_PRN140 0x0000000000100000 //Enable search for SBAS PRN140
#define UBLOX_SBAS_PRN141 0x0000000000200000 //Enable search for SBAS PRN141
#define UBLOX_SBAS_PRN142 0x0000000000400000 //Enable search for SBAS PRN142
#define UBLOX_SBAS_PRN143 0x0000000000800000 //Enable search for SBAS PRN143
#define UBLOX_SBAS_PRN144 0x0000000001000000 //Enable search for SBAS PRN144
#define UBLOX_SBAS_PRN145 0x0000000002000000 //Enable search for SBAS PRN145
#define UBLOX_SBAS_PRN146 0x0000000004000000 //Enable search for SBAS PRN146
#define UBLOX_SBAS_PRN147 0x0000000008000000 //Enable search for SBAS PRN147
#define UBLOX_SBAS_PRN148 0x0000000010000000 //Enable search for SBAS PRN148
#define UBLOX_SBAS_PRN149 0x0000000020000000 //Enable search for SBAS PRN149
#define UBLOX_SBAS_PRN150 0x0000000040000000 //Enable search for SBAS PRN150
#define UBLOX_SBAS_PRN151 0x0000000080000000 //Enable search for SBAS PRN151
#define UBLOX_SBAS_PRN152 0x0000000100000000 //Enable search for SBAS PRN152
#define UBLOX_SBAS_PRN153 0x0000000200000000 //Enable search for SBAS PRN153
#define UBLOX_SBAS_PRN154 0x0000000400000000 //Enable search for SBAS PRN154
#define UBLOX_SBAS_PRN155 0x0000000800000000 //Enable search for SBAS PRN155
#define UBLOX_SBAS_PRN156 0x0000001000000000 //Enable search for SBAS PRN156
#define UBLOX_SBAS_PRN157 0x0000002000000000 //Enable search for SBAS PRN157
#define UBLOX_SBAS_PRN158 0x0000004000000000 //Enable search for SBAS PRN158
// payload types
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_sbas;
typedef struct {
uint8_t msg_class;
uint8_t id;
uint8_t rate;
} ubx_msg;
typedef struct {
uint16_t meas;
uint16_t nav;
uint16_t time;
} ubx_rate;
typedef struct {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
// flags
uint8_t enabled;
uint8_t undefined0;
uint8_t sigCfgMask;
uint8_t undefined1;
} ubx_gnss_element_t;
typedef struct {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
ubx_gnss_element_t config[0];
} ubx_gnss_msg_t;
typedef struct {
uint8_t version;
uint8_t layers;
uint8_t reserved;
} __attribute__((packed)) ubx_config_data_header_v0_t;
typedef struct {
uint8_t version;
uint8_t layers;
uint8_t transaction;
uint8_t reserved;
} __attribute__((packed)) ubx_config_data_header_v1_t;
#define MAX_GNSS 7
#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
typedef union {
uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
ubx_sbas sbas;
ubx_msg msg;
ubx_rate rate;
ubx_gnss_msg_t gnss;
} ubx_payload;
// UBX support
typedef struct {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
} ubx_header;
typedef struct {
uint32_t key;
uint8_t value;
} __attribute__((packed)) ubx_config_data8_payload_t;
#define MAX_CONFIG_SET_VAL_VALUES 32
typedef struct {
ubx_header header;
ubx_config_data_header_v1_t configHeader;
union {
ubx_config_data8_payload_t payload[0];
uint8_t buffer[(MAX_CONFIG_SET_VAL_VALUES * sizeof(ubx_config_data8_payload_t)) + 2]; // 12 key/value pairs + 2 checksum bytes
} data;
} __attribute__((packed)) ubx_config_data8_t;
typedef struct {
ubx_header header;
ubx_payload payload;
} __attribute__((packed)) ubx_message;
typedef struct {
char swVersion[30]; // Zero-terminated Software Version String
char hwVersion[10]; // Zero-terminated Hardware Version String
} ubx_mon_ver;
typedef struct {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
} ubx_nav_posllh;
typedef struct {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
} ubx_nav_status;
typedef struct {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} ubx_nav_solution;
typedef struct {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct {
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
typedef struct {
uint32_t time; // GPS msToW
uint32_t tAcc;
int32_t nano;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
} ubx_nav_timeutc;
typedef struct {
uint32_t time; // GPS msToW
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fix_type;
uint8_t fix_status;
uint8_t reserved1;
uint8_t satellites;
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
int32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
uint16_t position_DOP;
uint16_t reserved2;
uint16_t reserved3;
} ubx_nav_pvt;
#define UBX_MON_GNSS_GPS_MASK (1 << 0)
#define UBX_MON_GNSS_GLONASS_MASK (1 << 1)
#define UBX_MON_GNSS_BEIDOU_MASK (1 << 2)
#define UBX_MON_GNSS_GALILEO_MASK (1 << 3)
typedef struct {
uint8_t version;
uint8_t supported; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
uint8_t defaultGnss; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
uint8_t enabled; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
uint8_t maxConcurrent;
uint8_t reserverd1;
uint8_t reserverd2;
uint8_t reserverd3;
} ubx_mon_gnss;
typedef struct {
uint8_t msg_class;
uint8_t msg;
} ubx_ack_ack;
typedef enum {
UBX_ACK_WAITING = 0,
UBX_ACK_GOT_ACK = 1,
UBX_ACK_GOT_NAK = 2
} ubx_ack_state_t;
typedef enum {
PREAMBLE1 = 0xB5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
MSG_CLASS_UBX = 0x01,
MSG_CLASS_NMEA = 0xF0,
MSG_VER = 0x04,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_NMEA_GGA = 0x0,
MSG_NMEA_GLL = 0x1,
MSG_NMEA_GSA = 0x2,
MSG_NMEA_GSV = 0x3,
MSG_NMEA_RMC = 0x4,
MSG_NMEA_VGS = 0x5,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_VELNED = 0x12,
MSG_TIMEUTC = 0x21,
MSG_SVINFO = 0x30,
MSG_NAV_SAT = 0x35,
MSG_NAV_SIG = 0x35,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3e,
MSG_MON_GNSS = 0x28
} ubx_protocol_bytes_t;
typedef enum {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
} ubs_nav_fix_type_t;
typedef enum {
NAV_STATUS_FIX_VALID = 1
} ubx_nav_status_bits_t;
uint8_t gpsUbloxMaxGnss(void);
timeMs_t gpsUbloxCapLastUpdate(void);
bool gpsUbloxHasGalileo(void);
bool gpsUbloxHasBeidou(void);
bool gpsUbloxHasGlonass(void);
bool gpsUbloxGalileoDefault(void);
bool gpsUbloxBeidouDefault(void);
bool gpsUbloxGlonassDefault(void);
bool gpsUbloxGalileoEnabled(void);
bool gpsUbloxBeidouEnabled(void);
bool gpsUbloxGlonassEnabled(void);
#ifdef __cplusplus
}
#endif

62
src/main/io/gps_ublox_utils.c Normal file
View file

@ -0,0 +1,62 @@
/*
* This file is part of INAV
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "gps_ublox_utils.h"
void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
*ck_a = *ck_b = 0;
while (len--) {
*ck_a += *data;
*ck_b += *ck_a;
data++;
}
}
int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count)
{
if (count > MAX_CONFIG_SET_VAL_VALUES)
count = MAX_CONFIG_SET_VAL_VALUES;
cfg->header.preamble1 = 0xb5;
cfg->header.preamble2 = 0x62;
cfg->header.msg_class = 0x06;
cfg->header.msg_id = 0x8A;
cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count));
cfg->configHeader.layers = 0x1;
cfg->configHeader.transaction = 0;
cfg->configHeader.reserved = 0;
cfg->configHeader.version = 1;
for (int i = 0; i < count; ++i) {
cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key);
cfg->data.payload[i].value = kvPairs[i].value;
}
uint8_t *buf = (uint8_t *)cfg;
uint8_t ck_a, ck_b;
ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b);
buf[cfg->header.length + 6] = ck_a;
buf[cfg->header.length + 7] = ck_b;
return count;
}

34
src/main/io/gps_ublox_utils.h Normal file
View file

@ -0,0 +1,34 @@
/*
* This file is part of INAV
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "gps_ublox.h"
#ifdef __cplusplus
extern "C" {
#endif
int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count);
void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b);
#ifdef __cplusplus
}
#endif

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

@ -3552,10 +3552,10 @@ uint8_t osdIncElementIndex(uint8_t elementIndex)
elementIndex = OSD_AIR_MAX_SPEED;
}
if (elementIndex == OSD_GLIDE_RANGE) {
elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_ITEM_COUNT;
elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME;
}
if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) {
elementIndex = OSD_ITEM_COUNT;
elementIndex = OSD_PILOT_NAME;
}
}
@ -4239,12 +4239,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
bool efficiencyValid = totalDistance >= 10000;
if (feature(FEATURE_GPS)) {
displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :");
uint8_t digits = 3U; // Total number of digits (including decimal point)
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
// Add one digit so no switch to scaled decimal occurs above 99
digits = 4U;
}
#endif
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, 3);
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1);
} else {
@ -4257,7 +4264,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, 3);
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
@ -4266,7 +4273,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
break;
case OSD_UNIT_GA:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, 3);
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1);
} else {
@ -4279,7 +4286,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, 3);
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';
@ -4290,7 +4297,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
FALLTHROUGH;
case OSD_UNIT_METRIC:
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3);
moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits);
if (!moreThanAh) {
tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1);
} else {
@ -4303,7 +4310,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
buff[5] = '\0';
}
} else {
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, 3);
osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits);
tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM);
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = '-';

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

@ -59,6 +59,8 @@ typedef enum {
FUNCTION_MSP_OSD = (1 << 25), // 33554432
} serialPortFunction_e;
#define FUNCTION_VTX_MSP FUNCTION_MSP_OSD
typedef enum {
BAUD_AUTO = 0,
BAUD_1200,

3
src/main/io/vtx_control.c
View file

@ -18,8 +18,10 @@
// Get target build configuration
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/log.h"
#include "config/config_eeprom.h"
#include "config/parameter_group.h"
@ -37,7 +39,6 @@
#include "io/osd.h"
#include "io/vtx_control.h"
#if defined(USE_VTX_CONTROL)
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 4);

701
src/main/io/vtx_msp.c Normal file
View file

@ -0,0 +1,701 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* 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 software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
/* Created by phobos- */
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
#include "build/debug.h"
//#include "cms/cms_menu_vtx_msp.h"
#include "common/crc.h"
#include "common/log.h"
#include "config/feature.h"
#include "drivers/vtx_common.h"
//#include "drivers/vtx_table.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "io/serial.h"
#include "io/vtx_msp.h"
#include "io/vtx_control.h"
#include "io/vtx_string.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx.h"
#include "io/displayport_msp_osd.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "msp/msp.h"
//#include "pg/vtx_table.h"
#include "fc/settings.h"
#include "rx/crsf.h"
//#include "rx/crsf_protocol.h"
#include "rx/rx.h"
#include "telemetry/msp_shared.h"
//static uint16_t mspConfFreq = 0;
static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT;
static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT;
//static uint16_t mspConfPower = 0;
static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT;
static uint8_t mspConfPitMode = 0;
static bool mspVtxConfigChanged = false;
static timeUs_t mspVtxLastTimeUs = 0;
static bool prevLowPowerDisarmedState = false;
static const vtxVTable_t mspVTable; // forward
static vtxDevice_t vtxMsp = {
.vTable = &mspVTable,
.capability.bandCount = VTX_MSP_TABLE_MAX_BANDS,
.capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS,
.capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS,
.capability.bandNames = (char **)vtx58BandNames,
.capability.channelNames = (char **)vtx58ChannelNames,
.capability.powerNames = (char**)saPowerNames
};
STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
static uint8_t mspVtxPortIdentifier = 255;
#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
{
return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF;
}
static bool isLowPowerDisarmed(void)
{
return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
(vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
(vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
}
bool isVtxConfigValid(const vtxConfig_t *cfg)
{
LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n");
for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) {
if (cfg->vtxChannelActivationConditions[i].band ||
(cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) ||
cfg->vtxChannelActivationConditions[i].auxChannelIndex ||
cfg->vtxChannelActivationConditions[i].channel) {
return true;
}
}
LOG_DEBUG(VTX, "msp Invalid Config!\r\n");
return false;
}
void setMspVtxDeviceStatusReady(const int descriptor)
{
LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n");
UNUSED(descriptor);
mspVtxStatus = MSP_VTX_STATUS_READY;
mspVtxConfigChanged = true;
}
void prepareMspFrame(uint8_t *mspFrame)
{
LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n");
/*
HDZERO parsing
fc_band_rx = msp_rx_buf[1];
fc_channel_rx = msp_rx_buf[2];
fc_pwr_rx = msp_rx_buf[3];
fc_pit_rx = msp_rx_buf[4];
fc_lp_rx = msp_rx_buf[8];
*/
uint8_t pitmode = 0;
vtxCommonGetPitMode(&vtxMsp, &pitmode);
mspFrame[0] = VTXDEV_MSP,
mspFrame[1] = vtxSettingsConfig()->band;
mspFrame[2] = vtxSettingsConfig()->channel;
mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
mspFrame[4] = pitmode;
mspFrame[5] = 0; // Freq_L
mspFrame[6] = 0; // Freq_H
mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
mspFrame[8] = isLowPowerDisarmed();
mspFrame[9] = 0; // Pitmode freq Low
mspFrame[10] = 0; // pitmod freq High
mspFrame[11] = 0; // 1 if using vtx table
mspFrame[12] = 0; // vtx table bands or 0
mspFrame[13] = 0; // vtx table channels or 0
mspFrame[14] = 0; // vtx table power levels or 0
}
static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
{
LOG_DEBUG(VTX, "msp CrsfPush\r\n");
#ifndef USE_TELEMETRY_CRSF
UNUSED(mspCommand);
UNUSED(mspFrame);
UNUSED(mspFrameSize);
#else
sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf;
uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header
uint8_t mspChecksum;
mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable
mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize);
uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1; // add 1 for msp checksum
uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize;
uint8_t crsfFrame[crsfFrameSize];
dst->ptr = crsfFrame;
dst->end = ARRAYEND(crsfFrame);
sbufWriteU8(dst, CRSF_SYNC_BYTE);
sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
sbufWriteData(dst, mspHeader, sizeof(mspHeader));
sbufWriteData(dst, mspFrame, mspFrameSize);
sbufWriteU8(dst, mspChecksum);
crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
sbufSwitchToReader(dst, crsfFrame);
crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
crsfRxSendTelemetryData();
#endif
}
static uint16_t packetCounter = 0;
static bool isVtxConfigChanged(void)
{
if(mspVtxStatus == MSP_VTX_STATUS_READY) {
if (mspVtxConfigChanged == true)
return true;
if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n");
mspVtxConfigChanged = true;
prevLowPowerDisarmedState = isLowPowerDisarmed();
}
if (mspConfPowerIndex != vtxSettingsConfig()->power) {
LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n");
mspVtxConfigChanged = true;
}
if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) {
LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n");
mspVtxConfigChanged = true;
}
return mspVtxConfigChanged;
}
return false;
}
static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
{
UNUSED(vtxDevice);
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
uint8_t frame[15];
switch (mspVtxStatus) {
case MSP_VTX_STATUS_OFFLINE:
LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n");
// wait for MSP communication from the VTX
#ifdef USE_CMS
//mspCmsUpdateStatusString();
#endif
break;
case MSP_VTX_STATUS_READY:
LOG_DEBUG(VTX, "msp MspProcess: READY\r\n");
// send an update if stuff has changed with 200ms period
if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n");
prepareMspFrame(frame);
if (isCrsfPortConfig(portConfig)) {
mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
} else {
mspPort_t *port = getMspOsdPort();
if(port != NULL && port->port) {
LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n");
int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE);
if (sent <= 0) {
break;
}
}
}
packetCounter++;
mspVtxLastTimeUs = currentTimeUs;
mspVtxConfigChanged = false;
#ifdef USE_CMS
//mspCmsUpdateStatusString();
#endif
}
break;
default:
mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
break;
}
#if 0
DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
#if defined(USE_MSP_OVER_TELEMETRY)
DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
#else
DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
#endif
#endif
}
static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
{
//LOG_DEBUG(VTX, "msp GetDeviceType\r\n");
UNUSED(vtxDevice);
return VTXDEV_MSP;
}
static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
{
//LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N");
return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
}
static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{
LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n");
UNUSED(vtxDevice);
if (band != mspConfBand || channel != mspConfChannel) {
LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n");
mspVtxConfigChanged = true;
}
mspConfBand = band;
mspConfChannel = channel;
}
static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{
LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n");
UNUSED(vtxDevice);
if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS))
{
if (index != mspConfPowerIndex)
{
LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n");
mspVtxConfigChanged = true;
}
mspConfPowerIndex = index;
}
}
static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
{
LOG_DEBUG(VTX, "msp SetPitMode\r\n");
UNUSED(vtxDevice);
if (onoff != mspConfPitMode) {
LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n");
mspVtxConfigChanged = true;
}
mspConfPitMode = onoff;
}
#if 0
static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{
UNUSED(vtxDevice);
if (freq != mspConfFreq) {
mspVtxConfigChanged = true;
}
mspConfFreq = freq;
}
#endif
static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
if (!vtxMspIsReady(vtxDevice)) {
return false;
}
*pBand = vtxSettingsConfig()->band;
*pChannel = vtxSettingsConfig()->channel;
//LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
return true;
}
static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{
if (!vtxMspIsReady(vtxDevice)) {
return false;
}
uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power;
// Special case, power not set
if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) {
*pIndex = 0;
//LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
return true;
}
*pIndex = power;
//LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
return true;
}
static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{
LOG_DEBUG(VTX, "msp GetFreq\r\n");
if (!vtxMspIsReady(vtxDevice)) {
return false;
}
*pFreq = 5800;
return true;
}
static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
LOG_DEBUG(VTX, "msp GetPower\r\n");
uint8_t powerIndex;
if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) {
return false;
}
*pIndex = powerIndex;
*pPowerMw = *pIndex;
return true;
}
static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
LOG_DEBUG(VTX, "msp GetOsdInfo\r\n");
uint8_t powerIndex;
uint16_t powerMw;
uint16_t freq;
uint8_t band, channel;
if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) {
return false;
}
if (!vtxMspGetFreq(vtxDevice, &freq)) {
return false;
}
if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) {
return false;
}
pOsdInfo->band = band;
pOsdInfo->channel = channel;
pOsdInfo->frequency = freq;
pOsdInfo->powerIndex = powerIndex;
pOsdInfo->powerMilliwatt = powerMw;
pOsdInfo->bandLetter = vtx58BandNames[band][0];
pOsdInfo->bandName = vtx58BandNames[band];
pOsdInfo->channelName = vtx58ChannelNames[channel];
pOsdInfo->powerIndexLetter = '0' + powerIndex;
return true;
}
bool vtxMspInit(void)
{
LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__);
// don't bother setting up this device if we don't have MSP vtx enabled
// Port is shared with MSP_OSD
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
if (!portConfig) {
return false;
}
mspVtxPortIdentifier = portConfig->identifier;
// XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
#if defined(USE_VTX_COMMON)
vtxCommonSetDevice(&vtxMsp);
#endif
mspConfBand = vtxSettingsConfig()->band;
mspConfChannel = vtxSettingsConfig()->channel;
mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode);
vtxInit();
return true;
}
static const vtxVTable_t mspVTable = {
.process = vtxMspProcess,
.getDeviceType = vtxMspGetDeviceType,
.isReady = vtxMspIsReady,
.setBandAndChannel = vtxMspSetBandAndChannel,
.setPowerByIndex = vtxMspSetPowerByIndex,
.setPitMode = vtxMspSetPitMode,
//.setFrequency = vtxMspSetFreq,
.getBandAndChannel = vtxMspGetBandAndChannel,
.getPowerIndex = vtxMspGetPowerIndex,
.getFrequency = vtxMspGetFreq,
//.getStatus = vtxMspGetStatus,
.getPower = vtxMspGetPower,
//.serializeCustomDeviceStatus = NULL,
.getOsdInfo = vtxMspGetOsdInfo,
};
static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
//LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n");
UNUSED(mspPostProcessFn);
sbuf_t *dst = &reply->buf;
sbuf_t *src = &cmd->buf;
const unsigned int dataSize = sbufBytesRemaining(src);
UNUSED(dst);
UNUSED(src);
// Start initializing the reply message
reply->cmd = cmd->cmd;
reply->result = MSP_RESULT_ACK;
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) {
LOG_DEBUG(VTX, "msp wrong vtx\r\n");
return MSP_RESULT_ERROR;
}
switch (cmd->cmd)
{
case MSP_VTXTABLE_BAND:
{
LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n");
uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);
if (deviceType == VTXDEV_MSP)
{
/*
char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1];
memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1);
uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS];
const uint8_t band = sbufReadU8(src);
const uint8_t bandNameLength = sbufReadU8(src);
for (int i = 0; i < bandNameLength; i++) {
const char nameChar = sbufReadU8(src);
if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) {
bandName[i] = toupper(nameChar);
}
}
const char bandLetter = toupper(sbufReadU8(src));
const bool isFactoryBand = (bool)sbufReadU8(src);
const uint8_t channelCount = sbufReadU8(src);
for (int i = 0; i < channelCount; i++)
{
const uint16_t frequency = sbufReadU16(src);
if (i < vtxTableConfig()->channels)
{
frequencies[i] = frequency;
}
}
*/
setMspVtxDeviceStatusReady(1);
}
break;
}
case MSP_VTXTABLE_POWERLEVEL:
{
LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n");
/*
char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
const uint8_t powerLevel = sbufReadU8(src);
const uint16_t powerValue = sbufReadU16(src);
const uint8_t powerLevelLabelLength = sbufReadU8(src);
for (int i = 0; i < powerLevelLabelLength; i++)
{
const char labelChar = sbufReadU8(src);
if (i < VTX_TABLE_POWER_LABEL_LENGTH)
{
powerLevelLabel[i] = toupper(labelChar);
}
}
*/
setMspVtxDeviceStatusReady(1);
}
break;
case MSP_VTX_CONFIG:
{
LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n");
LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n");
uint8_t pitmode = 0;
vtxCommonGetPitMode(vtxDevice, &pitmode);
// VTXDEV_MSP,
sbufWriteU8(dst, VTXDEV_MSP);
// band;
sbufWriteU8(dst, vtxSettingsConfig()->band);
// channel;
sbufWriteU8(dst, vtxSettingsConfig()->channel);
// power; // index based
sbufWriteU8(dst, vtxSettingsConfig()->power);
// pit mode;
// Freq_L
sbufWriteU8(dst, 0);
// Freq_H
sbufWriteU8(dst, 0);
// vtx status
sbufWriteU8(dst, 1);
// lowPowerDisarm
sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
// Pitmode freq Low
sbufWriteU8(dst, 0);
// pitmod freq High
sbufWriteU8(dst, 0);
// 1 if using vtx table
sbufWriteU8(dst, 0);
// vtx table bands or 0
sbufWriteU8(dst, 0);
// vtx table channels or 0
sbufWriteU8(dst, 0);
setMspVtxDeviceStatusReady(1);
break;
}
case MSP_SET_VTX_CONFIG:
LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n");
if (dataSize == 15)
{
if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN)
{
for (int i = 0; i < 15; ++i)
{
uint8_t data = sbufReadU8(src);
switch (i)
{
case 1:
vtxSettingsConfigMutable()->band = data;
break;
case 2:
vtxSettingsConfigMutable()->channel = data;
break;
case 3:
vtxSettingsConfigMutable()->power = data;
break;
case 4:
vtxCommonSetPitMode(vtxDevice, data);
break;
case 7:
// vtx ready
break;
case 8:
vtxSettingsConfigMutable()->lowPowerDisarm = data;
break;
}
}
}
setMspVtxDeviceStatusReady(1);
break;
}
LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n");
return MSP_RESULT_ERROR;
default:
// debug[1]++;
// debug[2] = cmd->cmd;
if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) {
LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd);
}
reply->result = MSP_RESULT_ERROR;
break;
}
// Process DONT_REPLY flag
if (cmd->flags & MSP_FLAG_DONT_REPLY)
{
reply->result = MSP_RESULT_NO_REPLY;
}
return reply->result;
}
void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
{
UNUSED(mspProcessCommandFn);
// Check if VTX is ready
/*
if (mspVtxStatus != MSP_VTX_STATUS_READY) {
LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n");
return;
}
*/
mspPort_t *port = getMspOsdPort();
if(port) {
mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand);
}
}
#endif

53
src/main/io/vtx_msp.h Normal file
View file

@ -0,0 +1,53 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* 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 software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "build/build_config.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
typedef enum {
// Offline - device hasn't responded yet
MSP_VTX_STATUS_OFFLINE = 0,
MSP_VTX_STATUS_READY,
} mspVtxStatus_e;
typedef struct mspPowerTable_s {
int mW; // rfpower
int16_t dbi; // valueV1
} mspPowerTable_t;
#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands
#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels
#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT
#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1
#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8
#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3
bool vtxMspInit(void);
void setMspVtxDeviceStatusReady(const int descriptor);
void prepareMspFrame(uint8_t *mspFrame);
void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);

3
src/main/msp/msp_protocol.h
View file

@ -217,6 +217,9 @@
#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data - needed by msp vtx
#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data - neede by msp vtx
//
// OSD specific
//

9
src/main/msp/msp_serial.c
View file

@ -530,7 +530,7 @@ int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t
return mspSerialEncode(mspPort, &push, version);
}
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version)
{
int ret = 0;
@ -545,11 +545,16 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
continue;
}
ret = mspSerialPushPort(cmd, data, datalen, mspPort, MSP_V1);
ret = mspSerialPushPort(cmd, data, datalen, mspPort, version);
}
return ret; // return the number of bytes written
}
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
{
return mspSerialPushVersion(cmd, data, datalen, MSP_V1);
}
uint32_t mspSerialTxBytesFree(serialPort_t *port)
{
return serialTxBytesFree(port);

1
src/main/msp/msp_serial.h
View file

@ -107,5 +107,6 @@ void mspSerialAllocatePorts(void);
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version);
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen);
int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version);
uint32_t mspSerialTxBytesFree(serialPort_t *port);
mspPort_t * mspSerialPortFind(const struct serialPort_s *serialPort);

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

@ -101,7 +101,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, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -125,6 +125,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode
.rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH
.landing_bump_detection = SETTING_NAV_LANDING_BUMP_DETECTION_DEFAULT, // Detect landing based on touchdown G bump
},
// General navigation parameters
@ -1280,17 +1281,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
}
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING_LEGACY)) {
if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) {
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
} else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
if (STATE(FIXED_WING_LEGACY)) {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
if (navConfig()->general.flags.rth_tail_first) {
@ -1402,7 +1398,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// 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)) {
resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
@ -1421,21 +1417,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
}
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
if (navConfig()->general.rth_home_altitude) {
float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
if (timeToReachHomeAltitude < 1) {
// we almost reached the target home altitude so set the desired altitude to the desired home altitude
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
}
}
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
return NAV_FSM_EVENT_NONE;
}
@ -1477,7 +1459,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
}
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
return NAV_FSM_EVENT_NONE;
}
@ -1500,7 +1482,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
UNUSED(previousState);
if (STATE(ALTITUDE_CONTROL)) {
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME
}
// Prevent I-terms growing when already landed
@ -1703,12 +1685,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
// Adjust altitude to waypoint setting
if (STATE(AIRPLANE)) {
int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
}
posControl.wpAltitudeReached = isWaypointAltitudeReached();
@ -2787,7 +2764,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
// Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
posControl.desiredState.pos.z = pos->z;
}
@ -2878,28 +2855,36 @@ bool isFlightDetected(void)
/*-----------------------------------------------------------
* Z-position controller
*-----------------------------------------------------------*/
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
{
#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
static timeUs_t lastUpdateTimeUs;
timeUs_t currentTimeUs = micros();
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
if (mode == ROC_TO_ALT_RESET) {
lastUpdateTimeUs = currentTimeUs;
posControl.desiredState.pos.z = altitudeToUse;
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
/* ROC_TO_ALT_CONSTANT - constant climb rate
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
const float absClimbRate = fabsf(desiredClimbRate);
const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
}
else { // ROC_TO_ALT_NORMAL
/*
* If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
* In other words, when altitude is reached, allow it only to shrink
*/
if (navConfig()->general.max_altitude > 0 &&
altitudeToUse >= navConfig()->general.max_altitude &&
desiredClimbRate > 0
) {
if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
desiredClimbRate = 0;
}
@ -2925,9 +2910,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
}
} else { // ROC_TO_ALT_RESET or zero desired climbrate
posControl.desiredState.pos.z = altitudeToUse;
}
lastUpdateTimeUs = currentTimeUs;
}
}
static void resetAltitudeController(bool useTerrainFollowing)

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

@ -235,6 +235,7 @@ typedef struct navConfig_s {
uint8_t waypoint_mission_restart; // Waypoint mission restart action
uint8_t rth_trackback_mode; // Useage mode setting for RTH trackback
uint8_t rth_use_linear_descent; // Use linear descent in the RTH head home leg
uint8_t landing_bump_detection; // Allow landing detection based on G bump at touchdown
} flags;
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
@ -621,6 +622,8 @@ bool isAdjustingHeading(void);
float getEstimatedAglPosition(void);
bool isEstimatedAglTrusted(void);
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
/* 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.

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

@ -117,13 +117,13 @@ bool adjustFixedWingAltitudeFromRCInput(void)
if (rcAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
}
@ -157,9 +157,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
const float pitchGainInv = 1.0f / 1.0f;
// Here we use negative values for dive for better clarity
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// Reduce max allowed climb pitch if performing loiter (stall prevention)
if (needToCalculateCircularLoiter) {
maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
}
// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
@ -760,7 +765,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));

62
src/main/navigation/navigation_multicopter.c
View file

@ -133,11 +133,11 @@ bool adjustMulticopterAltitudeFromRCInput(void)
// In terrain follow mode we apply different logic for terrain control
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
// We have solid terrain sensor signal - directly map throttle to altitude
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
posControl.desiredState.pos.z = altTarget;
}
else {
updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
}
// In surface tracking we always indicate that we're adjusting altitude
@ -159,14 +159,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
}
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
@ -720,10 +720,56 @@ bool isMulticopterFlying(void)
/*-----------------------------------------------------------
* Multicopter land detector
*-----------------------------------------------------------*/
#if defined(USE_BARO)
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue)
{
static float baroAltRate;
if (updateValue) {
baroAltRate = newBaroAltRate;
}
return baroAltRate;
}
static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
{
/* Detection based on G bump at touchdown, falling Baro altitude and throttle below hover.
* G bump trigger: > 2g then falling back below 1g in < 0.1s.
* Baro trigger: rate must be -ve at initial trigger g and < -2 m/s when g falls back below 1g
* Throttle trigger: must be below hover throttle with lower threshold for manual throttle control */
static timeMs_t gSpikeDetectTimeMs = 0;
float baroAltRate = updateBaroAltitudeRate(0, false);
if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) {
gSpikeDetectTimeMs = currentTimeMs;
} else if (gSpikeDetectTimeMs) {
if (currentTimeMs < gSpikeDetectTimeMs + 100) {
if (acc.accADCf[Z] < 1.0f && baroAltRate < -200.0f) {
const uint16_t idleThrottle = getThrottleIdleValue();
const uint16_t hoverThrottleRange = currentBatteryProfile->nav.mc.hover_throttle - idleThrottle;
return rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange);
}
} else if (acc.accADCf[Z] <= 1.0f) {
gSpikeDetectTimeMs = 0;
}
}
return false;
}
#endif
bool isMulticopterLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static timeMs_t landingDetectorStartedAt;
DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100);
const timeMs_t currentTimeMs = millis();
#if defined(USE_BARO)
if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && isLandingGbumpDetected(currentTimeMs)) {
return true; // Landing flagged immediately if landing bump detected
}
#endif
bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
@ -735,6 +781,8 @@ bool isMulticopterLandingDetected(void)
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && throttleIsBelowMidHover)
|| (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
static timeMs_t landingDetectorStartedAt;
if (!startCondition || posControl.flags.resetLandingDetector) {
landingDetectorStartedAt = 0;
return posControl.flags.resetLandingDetector = false;
@ -751,7 +799,6 @@ bool isMulticopterLandingDetected(void)
DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
bool possibleLandingDetected = false;
const timeMs_t currentTimeMs = millis();
if (navGetCurrentStateFlags() & NAV_CTL_LAND) {
// We have likely landed if throttle is 40 units below average descend throttle
@ -842,7 +889,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
// target min descent rate 5m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}

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

@ -322,12 +322,17 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) {
posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs));
// baro altitude rate
static float baroAltPrevious = 0;
posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs);
baroAltPrevious = posEstimator.baro.alt;
updateBaroAltitudeRate(posEstimator.baro.baroAltRate, true);
}
}
else {
posEstimator.baro.alt = 0;
posEstimator.baro.lastUpdateTime = 0;
posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv;
}
}
#endif
@ -556,7 +561,14 @@ static void estimationPredict(estimationContext_t * ctx)
static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
{
bool correctionCalculated = false;
DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate
DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude
DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude
DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level
DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate
DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame
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) {
timeUs_t currentTimeUs = micros();
@ -602,12 +614,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
}
correctionCalculated = true;
} else {
pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f);
return true;
}
if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
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
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {
@ -628,20 +637,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
}
correctionCalculated = true;
return true;
}
// DEBUG_ALTITUDE will be always available
DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate
DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude
DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude
DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level
DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate
DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame
DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
return correctionCalculated;
return false;
}
static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
@ -828,7 +827,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
navEPH = posEstimator.est.eph;
navEPV = posEstimator.est.epv;
DEBUG_SET(DEBUG_POS_EST, 0, (int32_t) posEstimator.est.pos.x*1000.0F); // Position estimate X
DEBUG_SET(DEBUG_POS_EST, 1, (int32_t) posEstimator.est.pos.y*1000.0F); // Position estimate Y
if (IS_RC_MODE_ACTIVE(BOXSURFACE) && posEstimator.est.aglQual!=SURFACE_QUAL_LOW){

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

@ -81,6 +81,7 @@ typedef struct {
pt1Filter_t avgFilter;
float alt; // Raw barometric altitude (cm)
float epv;
float baroAltRate; // Baro altitude rate of change (cm/s)
} navPositionEstimatorBARO_t;
typedef struct {

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

@ -61,7 +61,8 @@ typedef enum {
typedef enum {
ROC_TO_ALT_RESET,
ROC_TO_ALT_NORMAL
ROC_TO_ALT_CONSTANT,
ROC_TO_ALT_TARGET
} climbRateToAltitudeControllerMode_e;
typedef enum {
@ -447,7 +448,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode);
bool isNavHoldPositionActive(void);
bool isLastMissionWaypoint(void);

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

@ -199,7 +199,7 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
void batteryInit(void)
{
batteryState = BATTERY_NOT_PRESENT;
batteryCellCount = 1;
batteryCellCount = 0;
batteryFullVoltage = 0;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;

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

@ -62,12 +62,17 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
hardwareSensorStatus_e getHwCompassStatus(void)
{
#if defined(USE_MAG)
#ifdef USE_SIMULATOR
if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
if (compassIsHealthy()) {
return HW_SENSOR_OK;
}
else {
return HW_SENSOR_UNHEALTHY;
}
}
#endif
#if defined(USE_MAG)
if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
if (compassIsHealthy()) {
return HW_SENSOR_OK;

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

@ -194,6 +194,13 @@ STATIC_PROTOTHREAD(pitotThread)
pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
while(1) {
#ifdef USE_SIMULATOR
while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE))
{
ptDelayUs(10000);
}
#endif
// Start measurement
if (pitot.dev.start(&pitot.dev)) {
pitot.lastSeenHealthyMs = millis();

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

@ -26,5 +26,5 @@
void targetConfiguration(void)
{
boardAlignmentMutable()->yawDeciDegrees = 450;
//boardAlignmentMutable()->yawDeciDegrees = 450;
}

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

@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
DEF_TIM(TIM1, CH1, PB1, TIM_USE_LED, 0, 0), // LED
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_LED, 0, 0), // LED
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

8
src/main/target/AOCODARCF4V2/target.h
View file

@ -81,16 +81,16 @@
#define UART2_RX_PIN PA3
#define USE_UART3
#define UART3_TX_PIN PC11
#define UART3_RX_PIN PC10
#define UART3_TX_PIN PC10
#define UART3_RX_PIN PC11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define UART5_TX_PIN PD2
#define UART5_RX_PIN PC12
#define SERIAL_PORT_COUNT 6

1
src/main/target/AOCODARCH7DUAL/target.c
View file

@ -48,6 +48,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812
DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM
};

1
src/main/target/AOCODARCH7DUAL/target.h
View file

@ -28,6 +28,7 @@
#define BEEPER PA15
#define BEEPER_INVERTED
#define BEEPER_PWM_FREQUENCY 2500
// *************** IMU generic ***********************
#define USE_DUAL_GYRO

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

@ -25,8 +25,8 @@
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6
DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7

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

@ -170,6 +170,6 @@
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define MAX_PWM_OUTPUT_PORTS 4
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR

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

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

30
src/main/target/FLYCOLORF7MINI/config.c Normal file
View file

@ -0,0 +1,30 @@
/*
* 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 <stdint.h>
#include "platform.h"
#include "fc/fc_msp_box.h"
#include "io/piniobox.h"
#include "io/serial.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
}

42
src/main/target/FLYCOLORF7MINI/target.c Normal file
View file

@ -0,0 +1,42 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#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"
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(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // WS2812B
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

164
src/main/target/FLYCOLORF7MINI/target.h Normal file
View file

@ -0,0 +1,164 @@
/*
* 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 "FL7M"
#define USBD_PRODUCT_STRING "Flycolor F7 Mini"
#define LED0 PC15
#define BEEPER PC14
#define BEEPER_INVERTED
// *************** SPI1 Gyro & ACC *******************
#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW180_DEG_FLIP
#define MPU6500_CS_PIN PA4
#define MPU6500_SPI_BUS BUS_SPI1
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB6
#define I2C1_SDA PB7
#define USE_I2C_DEVICE_2
#define I2C2_SCL PB10
#define I2C2_SDA PB11
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C2
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_IST8308
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define TEMPERATURE_I2C_BUS BUS_I2C2
#define PITOT_I2C_BUS BUS_I2C2
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C2
// *************** SPI2 OSD ***********************
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
// *************** SPI3 SD BLACKBOX*******************
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN PC13
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// *************** UART *****************************
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 6
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART4
// *************** ADC *****************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#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 CURRENT_METER_ADC_CHANNEL ADC_CHN_3
#define RSSI_ADC_CHANNEL ADC_CHN_2
// *************** PINIO ***************************
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PB8
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED
// *************** LEDSTRIP ************************
#define USE_LED_STRIP
#define WS2811_PIN PA8
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define CURRENT_METER_SCALE 250
#define CURRENT_METER_OFFSET 0
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define MAX_PWM_OUTPUT_PORTS 6
#define USE_DSHOT
#define USE_SERIALSHOT
#define USE_ESC_SENSOR

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

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

40
src/main/target/FLYWOOF405PRO/target.c Normal file
View file

@ -0,0 +1,40 @@
/*
* 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(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2)
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5)
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5)
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8
DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3)
DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

170
src/main/target/FLYWOOF405PRO/target.h Normal file
View file

@ -0,0 +1,170 @@
/*
* 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 "F4PR"
#define USBD_PRODUCT_STRING "FLYWOOF405PRO"
#define LED0 PC14 //Green
#define BEEPER PC13
#define BEEPER_INVERTED
// *************** SPI1 Gyro & ACC *******************
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW90_DEG
#define MPU6000_CS_PIN PB12
#define MPU6000_SPI_BUS BUS_SPI1
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG_FLIP
#define BMI270_CS_PIN PB12
#define BMI270_SPI_BUS BUS_SPI1
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW180_DEG_FLIP
#define ICM42605_CS_PIN PB12
#define ICM42605_SPI_BUS BUS_SPI1
#define USE_EXTI
#define GYRO_INT_EXTI PB13
#define USE_MPU_DATA_READY_SIGNAL
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_BMP085
#define USE_BARO_DPS310
#define USE_BARO_SPL06
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_AK8963
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04_I2C
#define RANGEFINDER_I2C_BUS BUS_I2C1
#define PITOT_I2C_BUS BUS_I2C1
#define TEMPERATURE_I2C_BUS BUS_I2C1
// *************** SPI2 OSD ***************************
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
#define USE_OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PB14//
// *************** Onboard flash ********************
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PB3
#define M25P16_SPI_BUS BUS_SPI3
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// *************** UART *****************************
#define USB_IO
#define USE_VCP
#define VBUS_SENSING_PIN PA8
#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PA10
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN NONE
#define UART5_RX_PIN PD2
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 6
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART3
// *************** ADC ***************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#define ADC_CHANNEL_1_PIN PC3
#define ADC_CHANNEL_2_PIN PC2
#define ADC_CHANNEL_3_PIN PC0
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
// *************** LED2812 ************************
#define USE_LED_STRIP
#define WS2811_PIN PA9
// *************** OTHERS *************************
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL)
#define CURRENT_METER_SCALE 250
#define USE_DSHOT
#define USE_ESC_SENSOR
#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 (BIT(2))
#define MAX_PWM_OUTPUT_PORTS 8

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

@ -0,0 +1 @@
target_stm32h743xi(FOXEERH743)

57
src/main/target/FOXEERH743/config.c Normal file
View file

@ -0,0 +1,57 @@
/*
* 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 "common/axis.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "drivers/sensor.h"
#include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_output.h"
#include "drivers/serial.h"
#include "fc/rc_controls.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "io/serial.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#include "io/piniobox.h"
void targetConfiguration(void)
{
/*
* UART1 is SerialRX
*/
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
}

47
src/main/target/FOXEERH743/target.c Normal file
View file

@ -0,0 +1,47 @@
/*
* 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 <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, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4
DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5
DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8
DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9
DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

165
src/main/target/FOXEERH743/target.h Normal file
View file

@ -0,0 +1,165 @@
/*
* 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 "F743"
#define USBD_PRODUCT_STRING "FOXEERH743"
#define USE_TARGET_CONFIG
#define LED0 PC13
#define BEEPER PD2
#define BEEPER_INVERTED
// *************** SPI1 OSD ****************
#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_MAX7456
#define MAX7456_SPI_BUS BUS_SPI1
#define MAX7456_CS_PIN PA4
// *************** SPI2 Gyro ***********************
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW0_DEG
#define MPU6000_SPI_BUS BUS_SPI2
#define MPU6000_CS_PIN PB12
// *************** SPI3 FLASH ***********************
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
#define SPI3_NSS_PIN PA15
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN SPI3_NSS_PIN
#define W25N01G_SPI_BUS BUS_SPI3
#define W25N01G_CS_PIN SPI3_NSS_PIN
#define USE_BLACKBOX
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define USE_FLASH_W25N01G
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_BARO_SPL06
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_IST8308
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define USE_MAG_VCM5883
#define TEMPERATURE_I2C_BUS BUS_I2C1
#define PITOT_I2C_BUS BUS_I2C1
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C1
// *************** UART *****************************
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define USE_UART7
#define UART7_TX_PIN PE8
#define UART7_RX_PIN PE7
#define USE_UART8
#define UART8_TX_PIN PE1
#define UART8_RX_PIN PE0
#define SERIAL_PORT_COUNT 8
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
// *************** ADC *****************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_CHANNEL_1_PIN PC3
#define ADC_CHANNEL_2_PIN PC5
#define ADC_CHANNEL_3_PIN PC2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define RSSI_ADC_CHANNEL ADC_CHN_2
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3
// *************** LEDSTRIP ************************
#define USE_LED_STRIP
#define WS2811_PIN PA8
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL)
#define CURRENT_METER_SCALE 100
#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 TARGET_IO_PORTE 0xffff
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR

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

@ -0,0 +1 @@
target_stm32h743xi(KAKUTEH7WING HSE_MHZ 16)

48
src/main/target/KAKUTEH7WING/config.c Normal file
View file

@ -0,0 +1,48 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "fc/fc_msp_box.h"
#include "fc/config.h"
#include "rx/rx.h"
#include "io/piniobox.h"
#include "drivers/serial.h"
#include "io/serial.h"
void targetConfiguration(void)
{
serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_GPS;
serialConfigMutable()->portConfigs[1].gps_baudrateIndex = BAUD_115200;
serialConfigMutable()->portConfigs[5].functionMask = FUNCTION_RX_SERIAL;
rxConfigMutable()->receiverType = RX_TYPE_SERIAL;
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[6].msp_baudrateIndex = BAUD_115200;
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3;
pinioBoxConfigMutable()->permanentId[3] = BOX_PERMANENT_ID_USER4;
beeperConfigMutable()->pwmMode = true;
}

59
src/main/target/KAKUTEH7WING/hardware_setup.c Normal file
View file

@ -0,0 +1,59 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "build/build_config.h"
#include "drivers/time.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/io_impl.h"
void initialisePreBootHardware(void)
{
// VDD_3V3_SENSORS_EN
IOInit(DEFIO_IO(PB2), OWNER_SYSTEM, RESOURCE_OUTPUT, 0);
IOConfigGPIO(DEFIO_IO(PB2), IOCFG_OUT_PP);
// IOLo(DEFIO_IO(PB2));
// delay(100);
IOHi(DEFIO_IO(PB2));
// CAM Switch / User3
IOInit(DEFIO_IO(PC13), OWNER_SYSTEM, RESOURCE_OUTPUT, 0);
IOConfigGPIO(DEFIO_IO(PC13), IOCFG_OUT_PP);
IOLo(DEFIO_IO(PC13));
// User1
IOInit(DEFIO_IO(PD4), OWNER_SYSTEM, RESOURCE_OUTPUT, 0);
IOConfigGPIO(DEFIO_IO(PD4), IOCFG_OUT_PP);
IOLo(DEFIO_IO(PD4));
// VTx 9V Switch / User4
IOInit(DEFIO_IO(PE3), OWNER_SYSTEM, RESOURCE_OUTPUT, 0);
IOConfigGPIO(DEFIO_IO(PE3), IOCFG_OUT_PP);
IOHi(DEFIO_IO(PE3));
// User2
IOInit(DEFIO_IO(PE4), OWNER_SYSTEM, RESOURCE_OUTPUT, 0);
IOConfigGPIO(DEFIO_IO(PE4), IOCFG_OUT_PP);
IOLo(DEFIO_IO(PE4));
}

57
src/main/target/KAKUTEH7WING/target.c Normal file
View file

@ -0,0 +1,57 @@
/*
* 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 <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"
BUSDEV_REGISTER_SPI_TAG(busdev_bmi088_gyro, DEVHW_BMI088_GYRO, BMI088_SPI_BUS, BMI088_GYRO_CS_PIN, BMI088_GYRO_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_BMI088_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi088_acc, DEVHW_BMI088_ACC, BMI088_SPI_BUS, BMI088_ACC_CS_PIN, BMI088_ACC_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_BMI088_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
// BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2
DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3
DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4
DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5
DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA_NONE
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S7
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S8
DEF_TIM(TIM15,CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S9
DEF_TIM(TIM15,CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S13
//DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S14 / LED_2812
DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // S14 / LED_2812
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

204
src/main/target/KAKUTEH7WING/target.h Normal file
View file

@ -0,0 +1,204 @@
/*
* 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 "KH7W"
#define USBD_PRODUCT_STRING "KAKUTEH7WING"
#define USE_HARDWARE_PREBOOT_SETUP
#define USE_TARGET_CONFIG
#define LED0 PC15
#define LED1 PC14
#define BEEPER PB9
#define BEEPER_INVERTED
// *************** IMU generic ***********************
// #define USE_DUAL_GYRO
#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
// *************** SPI1 IMU0 BMI088 ******************
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_IMU_BMI088
#define IMU_BMI088_ALIGN CW0_DEG
#define BMI088_SPI_BUS BUS_SPI1
#define BMI088_GYRO_CS_PIN PC9
#define BMI088_GYRO_EXTI_PIN PD10
#define BMI088_ACC_CS_PIN PC8
#define BMI088_ACC_EXTI_PIN PD11
// *************** SPI3 IMU1 ICM42688 ************
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW90_DEG
#define ICM42605_SPI_BUS BUS_SPI3
#define ICM42605_CS_PIN PE12
#define ICM42605_EXTI_PIN PE15
// *************** SPI2 OSD ***********************
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PD3
#define SPI2_MISO_PIN PC2
#define SPI2_MOSI_PIN PC3
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
// *************** SDIO SD BLACKBOX*******************
#define USE_SDCARD
#define USE_SDCARD_SDIO
#define SDCARD_SDIO_DEVICE SDIODEV_2
#define SDCARD_SDIO_4BIT
#define SDCARD_SDIO_NORMAL_SPEED
#define SDCARD_SDIO2_CMD_ALT
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB7
#define USE_I2C_DEVICE_2
#define I2C2_SCL PB10
#define I2C2_SDA PB11
#define USE_I2C_DEVICE_4
#define I2C4_SCL PD12
#define I2C4_SDA PD13
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C4
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_BARO_SPL06
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_IST8308
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define USE_MAG_VCM5883
#define TEMPERATURE_I2C_BUS BUS_I2C1
#define PITOT_I2C_BUS BUS_I2C1
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C1
// *************** UART *****************************
#define USE_VCP
#define VBUS_SENSING_PIN PA9
#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PD5
#define UART2_RX_PIN PD6
#define USE_UART3
#define UART3_TX_PIN PD8
#define UART3_RX_PIN PD9
#define USE_UART5
#define UART5_TX_PIN PB13
#define UART5_RX_PIN PD2
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define USE_UART7
#define UART7_TX_PIN PE8
#define UART7_RX_PIN PE7
#define USE_UART8
#define UART8_TX_PIN PE1
#define UART8_RX_PIN PE0
#define SERIAL_PORT_COUNT 8
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART6
// *************** ADC *****************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_CHANNEL_1_PIN PC5 //VBAT1
#define ADC_CHANNEL_2_PIN PC4 //CURR1
#define ADC_CHANNEL_3_PIN PC0 //RSSI
#define ADC_CHANNEL_5_PIN PA3 //VB2
#define ADC_CHANNEL_6_PIN PA2 //CU2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
// *************** PINIO ***************************
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PC13 // VTX power switcher
#define PINIO2_PIN PE3 // 2xCamera switcher
#define PINIO3_PIN PD4 // User1
#define PINIO4_PIN PE4 // User2
// *************** LEDSTRIP ************************
#define USE_LED_STRIP
#define WS2811_PIN PA15
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_GPS)
#define CURRENT_METER_SCALE 3660 // 36.6
#define VBAT_SCALE_DEFAULT 1818 // 18.18
#define VBAT_SCALE_DEFAULT2 1100 // 11.0
#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 TARGET_IO_PORTE 0xffff
#define MAX_PWM_OUTPUT_PORTS 14
#define USE_DSHOT
#define USE_ESC_SENSOR

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

@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = {
DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2
DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA2 CH7
DEF_TIM(TMR1, CH3, PA10, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6
DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6
DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1), // motor3 DMA2 CH5
DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA2 CH4

2
src/main/target/NEUTRONRCF435MINI/target.h
View file

@ -137,7 +137,7 @@
#define USE_USB_DETECT
#define USE_UART1
#define UART1_RX_PIN PB7
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define USE_UART2

6
src/main/target/SITL/target.h
View file

@ -70,6 +70,9 @@
#define USE_RANGEFINDER_FAKE
#define USE_RX_SIM
#define USE_MSP_OSD
#define USE_OSD
#undef USE_DASHBOARD
#undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!?
@ -87,9 +90,6 @@
#undef USE_TELEMETRY_JETIEXBUS
#undef USE_TELEMETRY_SRXL
#undef USE_TELEMETRY_GHST
#undef USE_VTX_COMMON
#undef USE_VTX_CONTROL
#undef USE_VTX_SMARTAUDIO
#undef USE_VTX_TRAMP
#undef USE_CAMERA_CONTROL
#undef USE_BRUSHED_ESC_AUTODETECT

1
src/main/target/SPEEDYBEEF7V3/target.h
View file

@ -149,6 +149,7 @@
// ********** Optiical Flow adn Lidar **************
#define USE_RANGEFINDER
#define USE_RANGEFINDER_MSP
#define RANGEFINDER_I2C_BUS BUS_I2C1
#define USE_OPFLOW
#define USE_OPFLOW_MSP

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

@ -166,6 +166,7 @@
#define USE_VTX_CONTROL
#define USE_VTX_SMARTAUDIO
#define USE_VTX_TRAMP
#define USE_VTX_MSP
#define USE_PROGRAMMING_FRAMEWORK
#define USE_CLI_BATCH

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

@ -256,6 +256,8 @@ static void SystemClockHSE_Config(void)
pllConfig_t *pll1Config = (HAL_GetREVID() == REV_ID_V) ? &pll1ConfigRevV : &pll1ConfigRevY;
#endif
pll1Config->m = HSE_VALUE / 1000000 / 2; // correction for different HSE_VALUE
// Configure voltage scale.
// It has been pre-configured at PWR_REGULATOR_VOLTAGE_SCALE1,
// and it may stay or overridden by PWR_REGULATOR_VOLTAGE_SCALE0 depending on the clock config.

3
src/test/unit/CMakeLists.txt
View file

@ -37,6 +37,9 @@ set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular
set_property(SOURCE osd_unittest.cc PROPERTY depends "io/osd_utils.c" "io/displayport_msp_osd.c" "common/typeconversion.c")
set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_DISPLAYPORT DISABLE_MSP_BF_COMPAT)
set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c")
set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST)
function(unit_test src)
get_filename_component(basename ${src} NAME)
string(REPLACE ".cc" "" name ${basename} )

90
src/test/unit/gps_ublox_unittest.cc Normal file
View file

@ -0,0 +1,90 @@
/*
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "gtest/gtest.h"
#include "unittest_macros.h"
#include <iostream>
#include <string>
#include <stdio.h>
#include "io/gps_ublox_utils.h"
void dumpCfg(const ubx_config_data8_t *cfg, int valuesAdded)
{
printf("%02x %02x %02x %02x %04x\n", cfg->header.preamble1, cfg->header.preamble2, cfg->header.msg_class, cfg->header.msg_id, cfg->header.length);
printf("%02x %02x %02x %02x\n", cfg->configHeader.version, cfg->configHeader.layers, cfg->configHeader.transaction, cfg->configHeader.reserved);
for(int i =0; i < valuesAdded; ++i) {
printf("%i: %08x %02x\n", i+1, cfg->data.payload[i].key, cfg->data.payload[i].value);
}
uint8_t *chksums = (uint8_t *)&cfg->data.payload[valuesAdded];
printf("%02x %02x\n", chksums[0], chksums[1]);
}
void dumpMemory(uint8_t *mem, int size)
{
for(int i =0; i < size; ++i) {
printf("%02x ", mem[i]);
}
printf("\n");
}
TEST(GPSUbloxTest, TestUbloxCfgFillBytes)
{
ubx_config_data8_t cfg = {};
ubx_config_data8_payload_t kvPairs[] = {
{ 0x10310025, 0x1},
{ 0x42, 0x69},
{ 0x04, 0x20},
{ 0x42, 0x69},
{ 0x04, 0x20},
{ 0x42, 0x69},
{ 0x04, 0x20},
{ 0x42, 0x69},
{ 0x04, 0x20},
{ 0x42, 0x69},
{ 0x04, 0x20},
{ 0x42, 0x69}
};
int valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 12);
EXPECT_TRUE(valuesAdded == 12);
dumpCfg(&cfg, valuesAdded);
valuesAdded = ubloxCfgFillBytes(&cfg, kvPairs, 1);
EXPECT_TRUE(1 == valuesAdded);
// Set glonass enabled, from u-center 2
const uint8_t expected[] = {0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x25, 0x00, 0x31, 0x10, 0x01, 0x02, 0xA7};
EXPECT_FALSE(memcmp((void *)expected, (void *)&cfg, 17));
printf("Expected:\n");
dumpMemory((uint8_t *)expected, 17);
printf("Actual:\n");
dumpMemory((uint8_t *)&cfg, 17);
// osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7);
// std::cout << "'" << buf << "'" << std::endl;
//EXPECT_FALSE(strcmp(buf, " 123.45"));
}