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 agh_cmake

Browse source
This commit is contained in:
Alberto García Hierro 2020-09-18 19:19:36 +01:00
commit bb976d9166
45 changed files with 643 additions and 265 deletions
Download patch file Download diff file Expand all files Collapse all files

1
AUTHORS
View file

@ -37,6 +37,7 @@ Hyon Lim
James Harrison
Jan Staal
Jeremy Waters
Joe Hermaszewski
Joe Poser
Joel Fuster
Johannes Kasberger

25
docs/INAV PID Controller.md Normal file
View file

@ -0,0 +1,25 @@
# INAV PID Controller
What you have to know about INAV PID/PIFF/PIDCD controllers:
1. INAV PID uses floating-point math
1. Rate/Angular Velocity controllers work in dps [degrees per second]
1. P, I, D and Multirotor CD gains are scaled like Betafligfht equivalents, but actual mechanics are different, and PID response might be different
1. Depending on platform type, different controllers are used
1. Fixed-wing uses **PIFF**:
1. Error is computed with a formula `const float rateError = pidState->rateTarget - pidState->gyroRate;`
1. P-term with a formula `rateError * pidState->kP`
1. Simple I-term without Iterm Relax. I-term limit based on stick position is used instead. I-term is no allowed to grow if stick (roll/pitch/yaw) is deflected above threshold defined in `fw_iterm_limit_stick_position`. `pidState->errorGyroIf += rateError * pidState->kI * dT;`
1. No D-term
1. FF-term (Feed Forward) is computed from the controller input with a formula `pidState->rateTarget * pidState->kFF`. Bear in mind, this is not a **FeedForward** from Betaflight!
1. Multirotor uses **PIDCD**:
1. Error is computed with a formula `const float rateError = pidState->rateTarget - pidState->gyroRate;`
1. P-term with a formula `rateError * pidState->kP`
1. I-term
1. Iterm Relax is used to dynamically attenuate I-term during fast stick movements
1. I-term formula `pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);`
1. I-term can be limited when motor output is saturated
1. D-term is computed only from gyro measurement
1. There are 2 LPF filters on D-term
1. D-term can by boosted during fast maneuvers using D-Boost. D-Boost is an equivalent of Betaflight D_min
1. **Control Derivative**, CD, or CD-term is a derivative computed from the setpoint that helps to boost PIDCD controller during fast stick movements. `newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT);` It is an equivalent of Betaflight Feed Forward

56
docs/Inflight Adjustments.md
View file

@ -113,45 +113,45 @@ this reason ensure that you define enough ranges to cover the range channel's us
| 8 | PITCH_ROLL_D |
| 9 | YAW_P |
| 10 | YAW_I |
| 11 | YAW_D |
| 11 | YAW_D_FF |
| 12 | RATE_PROFILE | Switch between 3 rate profiles using a 3 position switch. |
| 13 | PITCH_RATE |
| 14 | ROLL_RATE |
| 15 | PITCH_P |
| 16 | PITCH_I |
| 17 | PITCH_D |
| 17 | PITCH_D_FF |
| 18 | ROLL_P |
| 19 | ROLL_I |
| 20 | RC_YAW_EXPO |
| 21 | MANUAL_RC_EXPO |
| 22 | MANUAL_RC_YAW_EXPO |
| 23 | MANUAL_PITCH_ROLL_RATE |
| 24 | MANUAL_ROLL_RATE |
| 25 | MANUAL_PITCH_RATE |
| 26 | MANUAL_YAW_RATE |
| 27 | NAV_FW_CRUISE_THROTTLE |
| 28 | NAV_FW_PITCH2THR |
| 29 | ROLL_BOARD_ALIGNMENT |
| 30 | PITCH_BOARD_ALIGNMENT |
| 31 | LEVEL_P |
| 32 | LEVEL_I |
| 33 | LEVEL_D |
| 34 | POS_XY_P |
| 35 | POS_XY_I |
| 36 | POS_XY_D |
| 37 | POS_Z_P |
| 38 | POS_Z_I |
| 39 | POS_Z_D |
| 40 | HEADING_P |
| 41 | VEL_XY_P |
| 42 | VEL_XY_I |
| 43 | VEL_XY_D |
| 44 | VEL_Z_P |
| 20 | ROLL_D_FF |
| 21 | RC_YAW_EXPO |
| 22 | MANUAL_RC_EXPO |
| 23 | MANUAL_RC_YAW_EXPO |
| 24 | MANUAL_PITCH_ROLL_RATE |
| 25 | MANUAL_ROLL_RATE |
| 26 | MANUAL_PITCH_RATE |
| 27 | MANUAL_YAW_RATE |
| 28 | NAV_FW_CRUISE_THROTTLE |
| 29 | NAV_FW_PITCH2THR |
| 30 | ROLL_BOARD_ALIGNMENT |
| 31 | PITCH_BOARD_ALIGNMENT |
| 32 | LEVEL_P |
| 33 | LEVEL_I |
| 34 | LEVEL_D |
| 35 | POS_XY_P |
| 36 | POS_XY_I |
| 37 | POS_XY_D |
| 38 | POS_Z_P |
| 39 | POS_Z_I |
| 40 | POS_Z_D |
| 41 | HEADING_P |
| 42 | VEL_XY_P |
| 43 | VEL_XY_I |
| 44 | VEL_XY_D |
| 45 | VEL_Z_P |
| 46 | VEL_Z_I |
| 47 | VEL_Z_D |
| 48 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE |
| 49 | PROFILE | Switch between 3 rate profiles using a 3 position switch. |
| 49 | ADJUSTMENT_VTX_POWER_LEVEL |
## Examples

38
docs/Programming Framework.md
View file

@ -63,6 +63,11 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` |
| 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` |
| 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` |
| 33 | SIN | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 34 | COS | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 35 | TAN | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
### Operands
@ -173,3 +178,36 @@ Sets Thhrottle output to about `50%` when Logic Condition `0` evaluates as `true
If Logic Condition `0` evaluates as `true`, motor throttle control is bound to RC channel 7 instead of throttle channel
### Set VTX channel with a POT
Set VTX channel with a POT on the radio assigned to RC channel 6
```
logic 0 1 -1 15 1 6 0 1000 0
logic 1 1 -1 37 4 0 0 7 0
logic 2 1 -1 14 4 1 0 1 0
logic 3 1 -1 31 4 2 0 0 0
```
Steps:
1. Normalize range `[1000:2000]` to `[0:1000]` by substracting `1000`
2. Scale range `[0:1000]` to `[0:7]`
3. Increase range by `1` to have the range of `[1:8]`
4. Assign LC#2 to VTX channel function
### Set VTX power with a POT
Set VTX power with a POT on the radio assigned to RC channel 6. In this example we scale POT to 4 power level `[1:4]`
```
logic 0 1 -1 15 1 6 0 1000 0
logic 1 1 -1 37 4 0 0 3 0
logic 2 1 -1 14 4 1 0 1 0
logic 3 1 -1 25 4 2 0 0 0
```
Steps:
1. Normalize range [1000:2000] to [0:1000] by substracting `1000`
2. Scale range [0:1000] to [0:3]
3. Increase range by `1` to have the range of [1:4]
4. Assign LC#2 to VTX power function

50
docs/development/Building in Eclipse.md
View file

@ -1,50 +0,0 @@
# Short Version
Install the latest Eclipse Standard/SDK and install the **C/C++ developments Tools** plugins
![plugin eclipse](http://i.imgur.com/IdJ8ki1.png)
Import the project using the wizard **Existing Code as Makefile Project**
![](http://i.imgur.com/XsVCwe2.png)
Adjust your build option if necessary
![](https://camo.githubusercontent.com/64a1d32400d6be64dd4b5d237df1e7f1b817f61b/687474703a2f2f692e696d6775722e636f6d2f6641306d30784d2e706e67)
Make sure you have a valid ARM toolchain and Ruby in the path
![](http://i.imgur.com/dAbscJo.png)
# Long version
* First you need an ARM toolchain. Good choices are **GCC ARM Embedded** (https://launchpad.net/gcc-arm-embedded) or **Yagarto** (http://www.yagarto.de).
* Install Ruby (see the document for your operating system).
* Now download Eclipse and unpack it somewhere. At the time of writing Eclipse 4.2 was the latest stable version.
* To work with ARM projects in Eclipse you need a few plugins:
+ **Eclipse C Development Tools** (CDT) (available via *Help > Install new Software*).
+ **Zylin Embedded CDT Plugin** (http://opensource.zylin.com/embeddedcdt.html).
+ **GNU ARM Eclipse** (http://sourceforge.net/projects/gnuarmeclipse/).
+ If you want to hook up an SWD debugger you also need the **GDB Hardware Debugging** plugin (Also available via *Install new Software*).
* Now clone the project to your harddrive.
* Create a new C project in Eclipse and choose ARM Cross Target Application and your ARM toolchain.
* Import the Git project into the C project in Eclipse via *File > Import > General > File System*.
* Activate Git via *Project > Team > Share Project*.
* Switch to the development branch in Eclipse (*Project > Team > Switch To > development*).
* The next thing you need to do is adjust the project configuration. There is a Makefile included that works but you might want to use GNU ARM Eclipse's automatic Makefile generation. Open the Project configuration and go to *C/C++ Build > Settings*
* Under *Target Processor* choose "cortex-m3"
* Under *ARM Yagarto [Windows/Mac OS] Linker > General* (or whatever toolchain you chose)
+ Browse to the Script file *stm32_flash.ld*
+ Uncheck "Do not use standard start files"
+ Check "Remove unused sections"
* Under *ARM Yagarto [Windows/Mac OS] Linker > Libraries*
+ Add "m" for the math library
* Under *ARM Yagarto [Windows/Mac OS] Compiler > Preprocessor* add the following 2 items to "Defined Symbols":
+ STM32F10X_MD
+ USE_STDPERIPH_DRIVER
* Under *ARM Yagarto [Windows/Mac OS] Compiler > Directories* add the following 3 items
+ ${workspace_loc:/${ProjName}/lib/CMSIS/CM3/CoreSupport}
+ ${workspace_loc:/${ProjName}/lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x}
+ ${workspace_loc:/${ProjName}/lib/STM32F10x_StdPeriph_Driver/inc}
* Under *ARM Yagarto [Windows/Mac OS] Compiler > Miscellaneous* add the following item to "Other flags":
+ -fomit-frame-pointer
* The code in the support directory is for uploading firmware to the board and is meant for your host machine. Hence, it must not be included in the build process. Just right-click on it to open its properties and choose "Exclude from build" under *C/C++ Build > Settings*
* The last thing you need to do is adding your toolchain to the PATH environment variable.
+ Go to *Project > Properties > C/C++ Build > Environment*, add a variable named "PATH" and fill in the full path of your toolchain's binaries.
+ Make sure "Append variables to native environment" is selected.
* Try to build the project via *Project > Build Project*.
* **Note**: If you're getting "...could not be resolved" errors for data types like int32_t etc. try to disable and re-enable the Indexer under *Project > Properties > C/C++ General > Indexer*.

2
docs/development/Building in Windows 10 with Linux Subsystem.md
View file

@ -1,4 +1,4 @@
# Building in Windows 10 with Linux subsystem
# Building in Windows 10 with Linux subsystem [Recommended]
Linux subsystem for Windows 10 is probably the simplest way of building INAV under Windows 10.

2
docs/development/Building in Windows light.md
View file

@ -1,4 +1,4 @@
# Building in windows light
# Building in windows light [Deprecated]
no cygwin and no path changes
## Install Git for windows

102
docs/development/IDE - Visual Studio Code with Windows 10.md Normal file
View file

@ -0,0 +1,102 @@
# IDE - Visual Studio Code with Windows 10
![Visual Studio Code](assets/vscode01.png)
[Visual Studio Code](https://code.visualstudio.com/) is probably the best free option for all Windows 10 users. It provides almost seamless integration with WSL running Ubuntu, syntax highlighting, building, and hardware debugging.
## Setup
1. Setup build environment using [generic WSL guide](Building%20in%20Windows%2010%20with%20Linux%20Subsystem.md)
1. Download and install [Visual Studio Code](https://code.visualstudio.com/)
1. From the VS Code Extensions download [Remote - WSL](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-wsl) plugin
1. Open INAV folder
1. Use `Ctrl + Shift + P` to run option `Remote-WSL: Reopen Folder in WSL`
1. Allow firewall and other permissions if requested
1. Install plugins in WSL workspace:
1. [C/C++ from Microsoft](https://marketplace.visualstudio.com/items?itemName=ms-vscode.cpptools) for C/C++ support
1. [Bookmarks](https://marketplace.visualstudio.com/items?itemName=alefragnani.Bookmarks) for simpler navigation
1. Configure the environment using the following snippets as a base
### C propertiues
Edit file `./.vscode/c_cpp_properties.json` to setup enabled `defines`
```
{
"configurations": [
{
"name": "Win32",
"includePath": [
"${workspaceRoot}",
"${workspaceRoot}/src/main/**"
],
"browse": {
"limitSymbolsToIncludedHeaders": false,
"path": [
"${workspaceRoot}/**"
]
},
"intelliSenseMode": "msvc-x64",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
"USE_NAV",
"NAV_FIXED_WING_LANDING",
"USE_OSD",
"USE_GYRO_NOTCH_1",
"USE_GYRO_NOTCH_2",
"USE_DTERM_NOTCH",
"USE_ACC_NOTCH",
"USE_GYRO_BIQUAD_RC_FIR2",
"USE_D_BOOST",
"USE_SERIALSHOT",
"USE_ANTIGRAVITY",
"USE_ASYNC_GYRO_PROCESSING",
"USE_RPM_FILTER",
"USE_GLOBAL_FUNCTIONS",
"USE_DYNAMIC_FILTERS",
"USE_IMU_BNO055",
"USE_SECONDARY_IMU",
"USE_DSHOT",
"FLASH_SIZE 480",
"USE_I2C_IO_EXPANDER",
"USE_PCF8574",
"USE_ESC_SENSOR"
]
}
],
"version": 4
}
```
### Tasks
Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard shortcut and from Command Console.
```
{
// See https://go.microsoft.com/fwlink/?LinkId=733558
// for the documentation about the tasks.json format
"version": "2.0.0",
"tasks": [
{
"label": "Build Matek F722-SE",
"type": "shell",
"command": "make TARGET=MATEKF722SE",
"group": "build",
"problemMatcher": []
},
{
"label": "Build Matek F722",
"type": "shell",
"command": "make TARGET=MATEKF722",
"group": {
"kind": "build",
"isDefault": true
},
"problemMatcher": []
}
]
}
```

63
docs/development/PID Internals.md
View file

@ -1,63 +0,0 @@
### IO variables
`gyroADC/8192*2000 = deg/s`
`gyroADC/4 ~ deg/s`
`rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250
`inclination` - in 0.1 degree, roll and pitch deviation from horizontal position
`max_angle_inclination` - in 0.1 degree, default 50 degrees (500)
`axisPID` - output to mixer, will be added to throttle(`<1000-2000>`), output range is `<minthrottle, maxthrottle>` (default `<1150 - 1850>`)
### PID controller 0, "MultiWii" (default)
#### Leveling term
```
error = constrain(2*rcCommand[axis], limit +- max_angle_inclination) - inclination[axis]
Pacc = constrain(P8[PIDLEVEL]/100 * error, limit +- 5 * D8[PIDLEVEL])
Iacc = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096
```
#### Gyro term
```
Pgyro = rcCommand[axis];
error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroADC[axis] / 4; (conversion so that error is in deg/s ?)
Igyro = integrate(error, limit +-16000) / 10 / 8 * I8[axis] / 100 (conversion back to mixer units ?)
```
reset I term if
- axis rotation rate > +-64deg/s
- axis is YAW and rcCommand>+-100
##### Mode dependent mix(yaw is always from gyro)
- HORIZON - proportionally according to max deflection
```
deflection = MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])) / 500 ; limit to 0.0 .. 1.0
P = Pacc * (1-deflection) + Pgyro * deflection
I = Iacc * (1-deflection) + Igyro * deflection
```
- gyro
```
P = Pgyro
I = Igyro
```
- ANGLE
```
P = Pacc
I = Iacc
```
#### Gyro stabilization
```
P -= gyroADC[axis] / 4 * dynP8 / 10 / 8
D = -mean(diff(gyroADC[axis] / 4), over 3 samples) * 3 * dynD8 / 32
[equivalent to :]
D = - (gyroADC[axis]/4 - (<3 loops old>gyroADC[axis]/4)) * dynD8 / 32
```
This can be seen as sum of
- PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroADC
- PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroADC

BIN
docs/development/assets/vscode01.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 564 KiB

28
src/main/cms/cms_menu_osd.c
View file

@ -139,11 +139,39 @@ static long osdElemActionsOnEnter(const OSD_Entry *from)
#define OSD_ELEMENT_ENTRY(name, osd_item_id) OSD_ITEM_ENTRY(name, osd_item_id)
static const OSD_Entry menuCrsfRxEntries[]=
{
OSD_LABEL_ENTRY("-- CRSF RX --"),
OSD_SETTING_ENTRY("CRSF SNR LEVEL", SETTING_OSD_SNR_ALARM),
OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_RSSI_ALARM),
OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),
OSD_ELEMENT_ENTRY("TX POWER", OSD_CRSF_TX_POWER),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuCrsf = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUCRF",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = menuCrsfRxEntries,
};
static const OSD_Entry menuOsdElemsEntries[] =
{
OSD_LABEL_ENTRY("--- OSD ITEMS ---"),
OSD_ELEMENT_ENTRY("RSSI", OSD_RSSI_VALUE),
#ifdef USE_SERIALRX_CRSF
OSD_SUBMENU_ENTRY("CRSF RX", &cmsx_menuCrsf),
#endif // CRSF Menu
OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE),
OSD_ELEMENT_ENTRY("MAIN BATT SC", OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE),
OSD_ELEMENT_ENTRY("CELL VOLTAGE", OSD_MAIN_BATT_CELL_VOLTAGE),

20
src/main/common/olc.c
View file

@ -29,7 +29,7 @@ static olc_coord_t initial_resolution;
static void init_constants(void)
{
static int inited = 0;
static bool inited = 0;
if (inited) {
return;
}
@ -45,20 +45,6 @@ static void init_constants(void)
initial_resolution = powf(ENCODING_BASE, initial_exponent) * OLC_DEG_MULTIPLIER;
}
// Raises a number to an exponent, handling negative exponents.
static float pow_negf(float base, float exponent)
{
if (exponent == 0) {
return 1;
}
if (exponent > 0) {
return powf(base, exponent);
}
return 1 / powf(base, -exponent);
}
// Compute the latitude precision value for a given code length. Lengths <= 10
// have the same precision for latitude and longitude, but lengths > 10 have
// different precisions due to the grid method having fewer columns than rows.
@ -66,10 +52,10 @@ static float compute_precision_for_length(int length)
{
// Magic numbers!
if (length <= (int)PAIR_CODE_LEN) {
return pow_negf(ENCODING_BASE, floorf((length / -2) + 2));
return powf(ENCODING_BASE, floorf((length / -2) + 2));
}
return pow_negf(ENCODING_BASE, -3) / powf(5, length - (int)PAIR_CODE_LEN);
return powf(ENCODING_BASE, -3) / powf(5, length - (int)PAIR_CODE_LEN);
}
static olc_coord_t adjust_latitude(olc_coord_t lat, size_t code_len)

3
src/main/drivers/accgyro/accgyro_icm20689.c
View file

@ -120,6 +120,9 @@ static void icm20689AccAndGyroInit(gyroDev_t *gyro)
#ifdef USE_MPU_DATA_READY_SIGNAL
busWrite(busDev, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
// Switch SPI to fast speed
busSetSpeed(busDev, BUS_SPEED_FAST);
}
bool icm20689GyroDetect(gyroDev_t *gyro)

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

@ -46,6 +46,7 @@
#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN)
#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
#define IOCFG_AF_OD IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
#define IOCFG_AF_OD_UP IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
#define IOCFG_IPD IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN)
#define IOCFG_IPU IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
@ -63,6 +64,7 @@
#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN)
#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_UP)
#define IOCFG_AF_OD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
#define IOCFG_AF_OD_UP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP)
#define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN)
#define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP)
#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL)
@ -74,6 +76,7 @@
# define IOCFG_OUT_OD 0
# define IOCFG_AF_PP 0
# define IOCFG_AF_OD 0
# define IOCFG_AF_OD_UP 0
# define IOCFG_IPD 0
# define IOCFG_IPU 0
# define IOCFG_IN_FLOATING 0

6
src/main/drivers/osd_symbols.h
View file

@ -240,6 +240,12 @@
#define SYM_HUD_ARROWS_D2 0x1AC // 428 2 arrows down
#define SYM_HUD_ARROWS_D3 0x1AD // 429 3 arrows down
#define SYM_2RSS 0xEA // RSSI 2
#define SYM_DB 0xEB // dB
#define SYM_DBM 0xEC // dBm
#define SYM_SRN 0xEE // SNR
#define SYM_MW 0xED // mW
#else
#define TEMP_SENSOR_SYM_COUNT 0

1
src/main/drivers/serial.h
View file

@ -49,6 +49,7 @@ typedef enum portOptions_t {
SERIAL_BIDIR_OD = 0 << 4,
SERIAL_BIDIR_PP = 1 << 4,
SERIAL_BIDIR_NOPULL = 1 << 5, // disable pulls in BIDIR RX mode
SERIAL_BIDIR_UP = 0 << 5, // enable pullup in BIDIR mode
} portOptions_t;
typedef void (*serialReceiveCallbackPtr)(uint16_t data, void *rxCallbackData); // used by serial drivers to return frames to app

9
src/main/drivers/serial_uart_stm32f4xx.c
View file

@ -292,10 +292,13 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
if (options & SERIAL_BIDIR) {
IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device));
if (options & SERIAL_BIDIR_PP)
if (options & SERIAL_BIDIR_PP) {
IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
else
IOConfigGPIOAF(tx, IOCFG_AF_OD, uart->af);
} else {
IOConfigGPIOAF(tx,
(options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_OD : IOCFG_AF_OD_UP,
uart->af);
}
}
else {
if (mode & MODE_TX) {

44
src/main/fc/fc_core.c
View file

@ -649,16 +649,21 @@ void processRx(timeUs_t currentTimeUs)
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
if (!ARMING_FLAG(ARMED)) {
DISABLE_STATE(ANTI_WINDUP_DEACTIVATED);
}
const rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
// In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL)
if (FLIGHT_MODE(MANUAL_MODE) || !ARMING_FLAG(ARMED)) {
/* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */
DISABLE_STATE(ANTI_WINDUP);
pidResetErrorAccumulators();
}
else if (STATE(FIXED_WING_LEGACY) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
if (throttleStatus == THROTTLE_LOW) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
// ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
ENABLE_STATE(ANTI_WINDUP);
}
@ -674,14 +679,37 @@ void processRx(timeUs_t currentTimeUs)
else {
DISABLE_STATE(ANTI_WINDUP);
}
} else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
if (throttleStatus == THROTTLE_LOW) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
ENABLE_STATE(ANTI_WINDUP);
}
else {
DISABLE_STATE(ANTI_WINDUP);
}
}
else {
DISABLE_STATE(ANTI_WINDUP);
pidResetErrorAccumulators();
}
}
else {
DISABLE_STATE(ANTI_WINDUP);
if (rollPitchStatus != CENTERED) {
ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
}
}
}
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
DISABLE_STATE(ANTI_WINDUP);
//This case applies only to MR when Airmode management is throttle threshold activated
if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) {
pidResetErrorAccumulators();
}
}
//---------------------------------------------------------
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}

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

@ -599,6 +599,15 @@ void init(void)
#endif
#ifdef USE_BLACKBOX
//Do not allow blackbox to be run faster that 1kHz. It can cause UAV to drop dead when digital ESC protocol is used
const uint32_t blackboxLooptime = getLooptime() * blackboxConfig()->rate_denom / blackboxConfig()->rate_num;
if (blackboxLooptime < 1000) {
blackboxConfigMutable()->rate_num = 1;
blackboxConfigMutable()->rate_denom = ceil(1000 / getLooptime());
}
// SDCARD and FLASHFS are used only for blackbox
// Make sure we only init what's necessary for blackbox
switch (blackboxConfig()->device) {

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

@ -1404,6 +1404,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP2_COMMON_TZ:
sbufWriteU16(dst, (uint16_t)timeConfig()->tz_offset);
sbufWriteU8(dst, (uint8_t)timeConfig()->tz_automatic_dst);
break;
case MSP2_INAV_AIR_SPEED:
@ -2726,7 +2727,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_COMMON_SET_TZ:
if (dataSize == 2)
timeConfigMutable()->tz_offset = (int16_t)sbufReadU16(src);
else
else if (dataSize == 3) {
timeConfigMutable()->tz_offset = (int16_t)sbufReadU16(src);
timeConfigMutable()->tz_automatic_dst = (uint8_t)sbufReadU8(src);
} else
return MSP_RESULT_ERROR;
break;

24
src/main/fc/rc_adjustments.c
View file

@ -101,7 +101,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
@ -113,7 +113,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_YAW_D,
.adjustmentFunction = ADJUSTMENT_YAW_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
@ -137,7 +137,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.adjustmentFunction = ADJUSTMENT_PITCH_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
@ -149,7 +149,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.adjustmentFunction = ADJUSTMENT_ROLL_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
@ -441,18 +441,18 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
case ADJUSTMENT_PITCH_ROLL_D_FF:
case ADJUSTMENT_PITCH_D_FF:
applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
FALLTHROUGH;
case ADJUSTMENT_ROLL_D:
applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
case ADJUSTMENT_ROLL_D_FF:
applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_P:
@ -463,8 +463,8 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_D:
applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
case ADJUSTMENT_YAW_D_FF:
applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_NAV_FW_CRUISE_THR:

8
src/main/fc/rc_adjustments.h
View file

@ -33,19 +33,19 @@ typedef enum {
ADJUSTMENT_YAW_RATE = 5,
ADJUSTMENT_PITCH_ROLL_P = 6,
ADJUSTMENT_PITCH_ROLL_I = 7,
ADJUSTMENT_PITCH_ROLL_D = 8,
ADJUSTMENT_PITCH_ROLL_D_FF = 8,
ADJUSTMENT_YAW_P = 9,
ADJUSTMENT_YAW_I = 10,
ADJUSTMENT_YAW_D = 11,
ADJUSTMENT_YAW_D_FF = 11,
ADJUSTMENT_RATE_PROFILE = 12, // Unused, placeholder for compatibility
ADJUSTMENT_PITCH_RATE = 13,
ADJUSTMENT_ROLL_RATE = 14,
ADJUSTMENT_PITCH_P = 15,
ADJUSTMENT_PITCH_I = 16,
ADJUSTMENT_PITCH_D = 17,
ADJUSTMENT_PITCH_D_FF = 17,
ADJUSTMENT_ROLL_P = 18,
ADJUSTMENT_ROLL_I = 19,
ADJUSTMENT_ROLL_D = 20,
ADJUSTMENT_ROLL_D_FF = 20,
ADJUSTMENT_RC_YAW_EXPO = 21,
ADJUSTMENT_MANUAL_RC_EXPO = 22,
ADJUSTMENT_MANUAL_RC_YAW_EXPO = 23,

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

@ -57,8 +57,9 @@ typedef enum {
typedef enum {
STICK_CENTER = 0,
THROTTLE_THRESHOLD
} airmodeAndAntiWindupHandlingType_e;
THROTTLE_THRESHOLD,
STICK_CENTER_ONCE
} airmodeHandlingType_e;
typedef enum {
ROL_LO = (1 << (2 * ROLL)),

2
src/main/fc/rc_modes.c
View file

@ -63,7 +63,7 @@ static void processAirmodeAirplane(void) {
}
static void processAirmodeMultirotor(void) {
if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
if ((rcControlsConfig()->airmodeHandlingType == STICK_CENTER) || (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE)) {
if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
ENABLE_STATE(AIRMODE_ACTIVE);
} else {

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

@ -131,6 +131,7 @@ typedef enum {
MOVE_FORWARD_ONLY = (1 << 22),
SET_REVERSIBLE_MOTORS_FORWARD = (1 << 23),
FW_HEADING_USE_YAW = (1 << 24),
ANTI_WINDUP_DEACTIVATED = (1 << 25),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

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

@ -125,7 +125,7 @@ tables:
values: ["OFF", "RP", "RPY"]
enum: itermRelax_e
- name: airmodeHandlingType
values: ["STICK_CENTER", "THROTTLE_THRESHOLD"]
values: ["STICK_CENTER", "THROTTLE_THRESHOLD", "STICK_CENTER_ONCE"]
- name: nav_extra_arming_safety
values: ["OFF", "ON", "ALLOW_BYPASS"]
enum: navExtraArmingSafety_e
@ -404,20 +404,20 @@ groups:
min: INT16_MIN
max: INT16_MAX
- name: maggain_x
description: "Magnetometer calibration X gain. If 0, no calibration or calibration failed"
default_value: "0"
description: "Magnetometer calibration X gain. If 1024, no calibration or calibration failed"
default_value: "1024"
field: magGain[X]
min: INT16_MIN
max: INT16_MAX
- name: maggain_y
description: "Magnetometer calibration Y gain. If 0, no calibration or calibration failed"
default_value: "0"
description: "Magnetometer calibration Y gain. If 1024, no calibration or calibration failed"
default_value: "1024"
field: magGain[Y]
min: INT16_MIN
max: INT16_MAX
- name: maggain_z
description: "Magnetometer calibration Z gain. If 0, no calibration or calibration failed"
default_value: "0"
description: "Magnetometer calibration Z gain. If 1024, no calibration or calibration failed"
default_value: "1024"
field: magGain[Z]
min: INT16_MIN
max: INT16_MAX
@ -2710,6 +2710,12 @@ groups:
condition: USE_BARO
min: -550
max: 1250
- name: osd_snr_alarm
description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
default_value: "5"
field: snr_alarm
min: -12
max: 8
- name: osd_temp_label_align
description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`"
default_value: "LEFT"

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

@ -1114,3 +1114,12 @@ const pidBank_t * pidBank(void) {
pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
}
uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
{
if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
return &pidBank->pid[pidIndex].FF;
} else {
return &pidBank->pid[pidIndex].D;
}
}

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

@ -198,3 +198,4 @@ void autotuneUpdateState(void);
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
pidType_e pidIndexGetType(pidIndex_e pidIndex);
uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

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

@ -230,15 +230,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainFF);
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainFF);
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainFF);
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF);
break;
}
}

20
src/main/io/cms_menu_misc.h Normal file
View file

@ -0,0 +1,20 @@
/*
* 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/>.
*/
#pragma once
extern const CMS_Menu cmsx_menuMisc;

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

@ -1185,7 +1185,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
}
@ -1211,7 +1211,7 @@ static bool osdDrawSingleElement(uint8_t item)
uint8_t elemPosX = OSD_X(pos);
uint8_t elemPosY = OSD_Y(pos);
textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
char buff[32];
char buff[32] = {0};
switch (item) {
case OSD_RSSI_VALUE:
@ -1635,6 +1635,57 @@ static bool osdDrawSingleElement(uint8_t item)
return true;
}
case OSD_CRSF_RSSI_DBM:
if (rxLinkStatistics.activeAnt == 0) {
buff[0] = SYM_RSSI;
tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM);
if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
} else {
buff[0] = SYM_2RSS;
tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM);
if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
}
break;
case OSD_CRSF_LQ:
buff[0] = SYM_BLANK;
tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} else if (rxLinkStatistics.uplinkLQ < osdConfig()->rssi_alarm) {
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
break;
case OSD_CRSF_SNR_DB: {
const char* showsnr = "-12";
const char* hidesnr = " ";
int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR;
if (osdSNR_Alarm <= osdConfig()->snr_alarm) {
buff[0] = SYM_SRN;
tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB);
}
else if (osdSNR_Alarm > osdConfig()->snr_alarm) {
if (cmsInMenu) {
buff[0] = SYM_SRN;
tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB);
} else {
buff[0] = SYM_BLANK;
tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK);
}
}
break;
}
case OSD_CRSF_TX_POWER: {
tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW);
break;
}
case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair
osdCrosshairPosition(&elemPosX, &elemPosY);
@ -1781,15 +1832,15 @@ static bool osdDrawSingleElement(uint8_t item)
#endif
case OSD_ROLL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D);
osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D_FF);
return true;
case OSD_PITCH_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D);
osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D_FF);
return true;
case OSD_YAW_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D);
osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D_FF);
return true;
case OSD_LEVEL_PIDS:
@ -2604,6 +2655,13 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
#ifdef USE_SERIALRX_CRSF
osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(22, 11);
osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(23, 9);
osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
#endif
osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;

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

@ -153,6 +153,10 @@ typedef enum {
OSD_ESC_RPM,
OSD_ESC_TEMPERATURE,
OSD_AZIMUTH,
OSD_CRSF_RSSI_DBM,
OSD_CRSF_LQ,
OSD_CRSF_SNR_DB,
OSD_CRSF_TX_POWER,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@ -220,6 +224,9 @@ typedef struct osdConfig_s {
float gforce_alarm;
float gforce_axis_alarm_min;
float gforce_axis_alarm_max;
#ifdef USE_SERIALRX_CRSF
int16_t snr_alarm; //CRSF SNR alarm in dB
#endif
#ifdef USE_BARO
int16_t baro_temp_alarm_min;
int16_t baro_temp_alarm_max;

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

@ -277,6 +277,31 @@ static int logicConditionCompute(
return operandB;
break;
#endif
case LOGIC_CONDITION_SIN:
temporaryValue = (operandB == 0) ? 500 : operandB;
return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break;
case LOGIC_CONDITION_COS:
temporaryValue = (operandB == 0) ? 500 : operandB;
return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break;
break;
case LOGIC_CONDITION_TAN:
temporaryValue = (operandB == 0) ? 500 : operandB;
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break;
case LOGIC_CONDITION_MAP_INPUT:
return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
break;
case LOGIC_CONDITION_MAP_OUTPUT:
return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
break;
default:
return false;
break;

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

@ -62,7 +62,12 @@ typedef enum {
LOGIC_CONDITION_SET_VTX_BAND = 30,
LOGIC_CONDITION_SET_VTX_CHANNEL = 31,
LOGIC_CONDITION_SET_OSD_LAYOUT = 32,
LOGIC_CONDITION_LAST = 33,
LOGIC_CONDITION_SIN = 33,
LOGIC_CONDITION_COS = 34,
LOGIC_CONDITION_TAN = 35,
LOGIC_CONDITION_MAP_INPUT = 36,
LOGIC_CONDITION_MAP_OUTPUT = 37,
LOGIC_CONDITION_LAST = 38,
} logicOperation_e;
typedef enum logicOperandType_s {

18
src/main/rx/crsf.c
View file

@ -58,6 +58,8 @@ static timeUs_t crsfFrameStartAt = 0;
static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0;
// The power levels represented by uplinkTXPower above in mW (250mW added to full TX in v4.00 firmware)
const uint16_t crsfPowerStates[] = {0, 10, 25, 100, 500, 1000, 2000, 250};
/*
* CRSF protocol
@ -108,7 +110,7 @@ struct crsfPayloadRcChannelsPacked_s {
typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
struct crsfPayloadLinkStatistics_s {
typedef struct crsfPayloadLinkStatistics_s {
uint8_t uplinkRSSIAnt1;
uint8_t uplinkRSSIAnt2;
uint8_t uplinkLQ;
@ -119,7 +121,8 @@ struct crsfPayloadLinkStatistics_s {
uint8_t downlinkRSSI;
uint8_t downlinkLQ;
int8_t downlinkSNR;
} __attribute__ ((__packed__));
uint8_t activeAnt;
} __attribute__ ((__packed__)) crsfPayloadLinkStatistics_t;
typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
@ -229,13 +232,18 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
}
crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
// Inject link quality into channel 17
// Inject link quality into channel 17.
const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
crsfChannelData[16] = scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 191, 1791); // will map to [1000;2000] range
lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1);
rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ;
rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR;
rxLinkStatistics.rfMode = linkStats->rfMode;
rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower];
rxLinkStatistics.activeAnt = linkStats->activeAntenna;
// This is not RC channels frame, update channel value but don't indicate frame completion
return RX_FRAME_PENDING;
}

78
src/main/rx/eleres.c
View file

@ -97,6 +97,24 @@ static uint8_t rfRxBuffer[DATA_PACKAGE_SIZE];
static uint8_t txFull = 0;
static uint8_t statusRegisters[2];
static uint8_t *eleresSignaturePtr;
static uint32_t lastIrqTime = 0;
typedef struct {
uint8_t idx;
int32_t lat; // lat 10E7
int32_t lon; // lon 10E7
int32_t alt; // altitude (cm)
uint16_t heading; // <20>
uint16_t speed; //cm/s
uint8_t lq; // Link quality, from 0 to 4
}__attribute__((packed)) eleres_radar_packet_t;
typedef struct {
uint8_t idx;
int32_t lat; // lat 10E7
int32_t lon; // lon 10E7
int32_t alt; // altitude (cm)
}__attribute__((packed)) eleres_wp_packet_t;
static uint8_t rfmSpiRead(uint8_t address)
{
@ -381,6 +399,7 @@ rx_spi_received_e eleresDataReceived(uint8_t *payload, uint16_t *linkQuality)
if (rxSpiCheckIrq())
{
lastIrqTime = millis();
statusRegisters[0] = rfmSpiRead(0x03);
statusRegisters[1] = rfmSpiRead(0x04);
//only if RC frame received
@ -424,6 +443,49 @@ static void parseStatusRegister(const uint8_t *payload)
channelHoppingTime = (rfRxBuffer[20] & 0x0F)+18;
dataReady |= DATA_FLAG;
} else if ((rfRxBuffer[0] & 127) == 'R') { //radar poi info
eleres_radar_packet_t p;
uint8_t idx=0;
firstRun = 0;
goodFrames++;
if ((rfRxBuffer[21] & 0xF0) == 0x20)
hoppingChannel = rfRxBuffer[21] & 0x0F;
channelHoppingTime = (rfRxBuffer[20] & 0x0F)+18;
//parse plane info
memcpy(&p, &rfRxBuffer[1], sizeof(p));
idx = MIN(p.idx, RADAR_MAX_POIS - 1); // Radar poi number, 0 to 3
radar_pois[idx].gps.lat = p.lat;
radar_pois[idx].gps.lon = p.lon;
radar_pois[idx].gps.alt = p.alt;
radar_pois[idx].heading = p.heading;
radar_pois[idx].speed = p.speed;
radar_pois[idx].lq = p.lq;
if (p.lat == 0 || p.lon == 0)
radar_pois[idx].state = 0;
else
radar_pois[idx].state = 1;
} else if ((rfRxBuffer[0] & 127) == 'W') { //WP info
eleres_wp_packet_t p;
navWaypoint_t tp; //targetPoint
firstRun = 0;
goodFrames++;
if ((rfRxBuffer[21] & 0xF0) == 0x20)
hoppingChannel = rfRxBuffer[21] & 0x0F;
channelHoppingTime = (rfRxBuffer[20] & 0x0F)+18;
//parse plane info
memcpy(&p, &rfRxBuffer[1], sizeof(p));
tp.action = NAV_WP_ACTION_WAYPOINT;
tp.alt = p.alt;
tp.lat = p.lat;
tp.lon = p.lon;
setWaypoint(p.idx, &tp);
} else if (eleresConfig()->eleresLocEn && eleresConfig()->eleresTelemetryEn && bkgLocEnable==2) {
if ((rfRxBuffer[0] == 'H' && rfRxBuffer[2] == 'L') ||
rfRxBuffer[0]=='T' || rfRxBuffer[0]=='P' || rfRxBuffer[0]=='G') {
@ -498,6 +560,20 @@ void eleresSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
else
led_time = cr_time;
//watch IRQ - if not received, restart RFM
if (eleresConfig()->eleresTelemetryEn && millis() - lastIrqTime > 2000)
{
if (lastIrqTime < 4000)
{
rfmSpiWrite(0x07, 0x80); //restart rfm22 in case of problems only at startup
delay(1);
}
rfm22bInitParameter();
toRxMode();
lastIrqTime = millis();
}
if ((dataReady & LOCALIZER_FLAG) == 0) {
if (cr_time > nextPackTime+2) {
if ((cr_time-lastPackTime > 1500) || firstRun) {
@ -699,7 +775,7 @@ void eleresInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
rfmSpiWrite(0x07, 0x80);
delay(100);
eleresSignaturePtr = (uint8_t*)&eleresConfigMutable()->eleresSignature;
eleresSignaturePtr = (uint8_t*)&eleresConfig()->eleresSignature;
rfm22bInitParameter();
bindChannels(eleresSignaturePtr,holList);

30
src/main/rx/fport2.c
View file

@ -53,9 +53,10 @@
#include "rx/fport2.h"
#define FPORT2_MAX_TELEMETRY_RESPONSE_DELAY_US 3000
#define FPORT2_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define FPORT2_MIN_OTA_RESPONSE_DELAY_US 50
#define FPORT2_MAX_TELEMETRY_RESPONSE_DELAY_US 3000
#define FPORT2_OTA_MAX_RESPONSE_TIME_US_DEFAULT 200
#define FPORT2_OTA_MIN_RESPONSE_DELAY_US_DEFAULT 50
#define FPORT2_MAX_TELEMETRY_AGE_MS 500
#define FPORT2_FC_COMMON_ID 0x1B
#define FPORT2_FC_MSP_ID 0x0D
@ -180,6 +181,8 @@ static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId =
static smartPortPayload_t *otaResponsePayload = NULL;
static bool otaMode = false;
static bool otaDataNeedsProcessing = false;
static uint16_t otaMinResponseDelay = FPORT2_OTA_MIN_RESPONSE_DELAY_US_DEFAULT;
static uint16_t otaMaxResponseTime = FPORT2_OTA_MAX_RESPONSE_TIME_US_DEFAULT;
static uint32_t otaDataAddress;
static uint8_t otaDataBuffer[FPORT2_OTA_DATA_FRAME_BYTES];
static timeUs_t otaFrameEndTimestamp = 0;
@ -380,9 +383,24 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
break;
#if defined(USE_TELEMETRY_SMARTPORT)
case CFT_OTA_START:
case CFT_OTA_START: {
uint8_t otaMinResponseDelayByte = frame->control.ota[0];
if ((otaMinResponseDelayByte > 0) && (otaMinResponseDelayByte <= 4)) {
otaMinResponseDelay = otaMinResponseDelayByte * 100;
} else {
otaMinResponseDelay = FPORT2_OTA_MIN_RESPONSE_DELAY_US_DEFAULT;
}
uint8_t otaMaxResponseTimeByte = frame->control.ota[1];
if (otaMaxResponseTimeByte > 0) {
otaMaxResponseTime = otaMaxResponseTimeByte * 100;
} else {
otaMaxResponseTime = FPORT2_OTA_MAX_RESPONSE_TIME_US_DEFAULT;
}
otaMode = true;
break;
}
case CFT_OTA_DATA:
if (otaMode) {
@ -506,7 +524,7 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
#if defined(USE_TELEMETRY_SMARTPORT)
if (((mspPayload || hasTelemetryRequest) && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT2_MIN_TELEMETRY_RESPONSE_DELAY_US)
|| (otaResponsePayload && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT2_MIN_OTA_RESPONSE_DELAY_US)) {
|| (otaResponsePayload && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= otaMinResponseDelay)) {
hasTelemetryRequest = false;
clearToSend = true;
result |= RX_FRAME_PROCESSING_REQUIRED;
@ -576,7 +594,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
}
timeUs_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp);
if (!firmwareUpdateError && (otaResponseTime < 400)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash)
if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash)
writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload);
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME, otaResponseTime);
}
@ -656,7 +674,7 @@ bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
NULL,
FPORT2_BAUDRATE,
MODE_RXTX,
FPORT2_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
FPORT2_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (tristateWithDefaultOnIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
);
if (fportPort) {

1
src/main/rx/rx.c
View file

@ -102,6 +102,7 @@ static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
rxLinkStatistics_t rxLinkStatistics;
rxRuntimeConfig_t rxRuntimeConfig;
static uint8_t rcSampleIndex = 0;

11
src/main/rx/rx.h
View file

@ -175,8 +175,17 @@ typedef enum {
RSSI_SOURCE_MSP,
} rssiSource_e;
extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
typedef struct rxLinkStatistics_s {
int16_t uplinkRSSI; // RSSI value in dBm
uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100]
int8_t uplinkSNR; // The SNR of the uplink in dB
uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
uint16_t uplinkTXPower; // power in mW
uint8_t activeAnt;
} rxLinkStatistics_t;
extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount
extern rxLinkStatistics_t rxLinkStatistics;
void lqTrackerReset(rxLinkQualityTracker_e * lqTracker);
void lqTrackerAccumulate(rxLinkQualityTracker_e * lqTracker, uint16_t rawValue);
void lqTrackerSet(rxLinkQualityTracker_e * lqTracker, uint16_t rawValue);

11
src/main/rx/sbus_channels.c
View file

@ -82,11 +82,18 @@ uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannel
return RX_FRAME_COMPLETE;
}
uint16_t sbusDecodeChannelValue(uint16_t sbusValue)
uint16_t sbusDecodeChannelValue(uint16_t sbusValue, bool safeValuesOnly)
{
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
if (safeValuesOnly) {
// Clip channel values to more or less safe values (988 .. 2012)
return (5 * constrain(sbusValue, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX) / 8) + 880;
}
else {
// Use full range of values (11 bit, channel values in range 880 .. 2159)
return (5 * constrain(sbusValue, 0, 2047) / 8) + 880;
}
}
uint16_t sbusEncodeChannelValue(uint16_t rcValue)
@ -96,7 +103,7 @@ uint16_t sbusEncodeChannelValue(uint16_t rcValue)
static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{
return sbusDecodeChannelValue(rxRuntimeConfig->channelData[chan]);
return sbusDecodeChannelValue(rxRuntimeConfig->channelData[chan], false);
}
void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig)

2
src/main/rx/sbus_channels.h
View file

@ -72,7 +72,7 @@ typedef struct sbusFrame_s {
} __attribute__ ((__packed__)) sbusFrame_t;
uint16_t sbusDecodeChannelValue(uint16_t sbusValue);
uint16_t sbusDecodeChannelValue(uint16_t sbusValue, bool safeValuesOnly);
uint16_t sbusEncodeChannelValue(uint16_t rcValue);
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);

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

@ -73,6 +73,7 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
.rollDeciDegrees = 0,
.pitchDeciDegrees = 0,
.yawDeciDegrees = 0,
.magGain = {1024, 1024, 1024},
);
#ifdef USE_MAG
@ -338,8 +339,8 @@ void compassUpdate(timeUs_t currentTimeUs)
// Check magZero
if (
(compassConfig()->magZero.raw[X] == 0 && compassConfig()->magZero.raw[Y] == 0 && compassConfig()->magZero.raw[Z] == 0) ||
compassConfig()->magGain[X] == 0 || compassConfig()->magGain[Y] == 0 || compassConfig()->magGain[Z] == 0
compassConfig()->magZero.raw[X] == 0 && compassConfig()->magZero.raw[Y] == 0 && compassConfig()->magZero.raw[Z] == 0 &&
compassConfig()->magGain[X] == 1024 && compassConfig()->magGain[Y] == 1024 && compassConfig()->magGain[Z] == 1024
) {
DISABLE_STATE(COMPASS_CALIBRATED);
}
@ -363,7 +364,7 @@ void compassUpdate(timeUs_t currentTimeUs)
for (int axis = 0; axis < 3; axis++) {
compassConfigMutable()->magZero.raw[axis] = 0;
compassConfigMutable()->magGain[axis] = 0;
compassConfigMutable()->magGain[axis] = 1024;
magPrev[axis] = 0;
}

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

@ -26,7 +26,7 @@
void targetConfiguration(void)
{
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_TELEMETRY_SMARTPORT_MASTER;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_FRSKY_OSD;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_FRSKY_OSD;
rxConfigMutable()->serialrx_inverted = 1;
}

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

@ -170,7 +170,7 @@
#define SERIAL_PORT_COUNT 8
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_PROVIDER SERIALRX_FPORT2
#define SERIALRX_UART SERIAL_PORT_USART8
// *************** ADC *****************************

3
src/main/telemetry/crsf.c
View file

@ -273,7 +273,8 @@ typedef enum {
CRSF_RF_POWER_100_mW = 3,
CRSF_RF_POWER_500_mW = 4,
CRSF_RF_POWER_1000_mW = 5,
CRSF_RF_POWER_2000_mW = 6
CRSF_RF_POWER_2000_mW = 6,
CRSF_RF_POWER_250_mW = 7
} crsrRfPower_e;
/*