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 'development' into dzikuvx-global-variables

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-04-10 19:17:23 +02:00
commit 3f54ba2313
162 changed files with 2239 additions and 3324 deletions
Download patch file Download diff file Expand all files Collapse all files

68
.github/workflows/ci.yml vendored Normal file
View file

@ -0,0 +1,68 @@
name: Build firmware
# Don't enable CI on push, just on PR. If you
# are working on the main repo and want to trigger
# a CI build submit a draft PR.
on: pull_request
jobs:
build:
runs-on: ubuntu-18.04
strategy:
matrix:
start: [0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100]
count: [10]
steps:
- uses: actions/checkout@v2
- name: Setup environment
run: |
# This is the hash of the commit for the PR
# when the action is triggered by PR, empty otherwise
COMMIT_ID=${{ github.event.pull_request.head.sha }}
# This is the hash of the commit when triggered by push
# but the hash of refs/pull/<n>/merge, which is different
# from the hash of the latest commit in the PR, that's
# why we try github.event.pull_request.head.sha first
COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
echo "::set-env name=TARGETS::$(./src/utils/build-targets.sh -n -s ${{ matrix.start }} -c ${{ matrix.count }})"
echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}"
echo "::set-env name=BUILD_NAME::inav-$(make print_version)-${BUILD_SUFFIX}"
echo "::set-env name=IS_LAST_JOB::$([ $(expr ${{ strategy.job-index }} + 1) = ${{ strategy.job-total }} ] && echo yes)"
- name: Ensure all targets will be tested
if: ${{ env.IS_LAST_JOB }}
run: |
UNTESTED=$(./src/utils/build-targets.sh -n -s $(expr ${{ matrix.start }} + ${{ matrix.count }}) -c 10000)
if ! [ -z "${UNTESTED}" ]; then
echo "Untested targets: ${UNTESTED}" >&2
exit 1
fi
- uses: actions/cache@v1
with:
path: downloads
key: ${{ runner.os }}-downloads-${{ hashFiles('Makefile') }}-${{ hashFiles('**/make/*.mk')}}
- name: Install ARM toolchain
run: make arm_sdk_install
- name: Build targets (${{ matrix.start }})
if: ${{ env.TARGETS }}
run: ./src/utils/build-targets.sh -s ${{ matrix.start }} -c ${{ matrix.count }} -S ${{ env.BUILD_SUFFIX }}
- name: Upload artifacts
if: ${{ env.TARGETS }}
uses: actions/upload-artifact@v2-preview
with:
name: ${{ env.BUILD_NAME }}.zip
path: ./obj/*.hex
test:
needs: [build]
runs-on: ubuntu-18.04
steps:
- uses: actions/checkout@v2
- uses: actions/cache@v1
with:
path: downloads
key: ${{ runner.os }}-downloads-${{ hashFiles('Makefile') }}-${{ hashFiles('**/make/*.mk')}}
- name: Install ARM toolchain
run: make arm_sdk_install
- name: Run Tests
run: make test

8
Dockerfile
View file

@ -4,9 +4,9 @@ FROM ubuntu:bionic
VOLUME /home/src/
WORKDIR /home/src/
ARG TOOLCHAIN_VERSION_SHORT
ENV TOOLCHAIN_VERSION_SHORT ${TOOLCHAIN_VERSION_SHORT:-"8-2018q4"}
ENV TOOLCHAIN_VERSION_SHORT ${TOOLCHAIN_VERSION_SHORT:-"9-2019q4"}
ARG TOOLCHAIN_VERSION_LONG
ENV TOOLCHAIN_VERSION_LONG ${TOOLCHAIN_VERSION_LONG:-"8-2018-q4-major"}
ENV TOOLCHAIN_VERSION_LONG ${TOOLCHAIN_VERSION_LONG:-"9-2019-q4-major"}
# Essentials
RUN mkdir -p /home/src && \
@ -14,10 +14,10 @@ RUN mkdir -p /home/src && \
apt-get install -y software-properties-common ruby make git gcc wget curl bzip2
# Toolchain
RUN wget -P /tmp "https://developer.arm.com/-/media/Files/downloads/gnu-rm/$TOOLCHAIN_VERSION_SHORT/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-linux.tar.bz2"
RUN wget -P /tmp "https://developer.arm.com/-/media/Files/downloads/gnu-rm/$TOOLCHAIN_VERSION_SHORT/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-x86_64-linux.tar.bz2"
RUN mkdir -p /opt && \
cd /opt && \
tar xvjf "/tmp/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-linux.tar.bz2" -C /opt && \
tar xvjf "/tmp/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-x86_64-linux.tar.bz2" -C /opt && \
chmod -R -w "/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG"
ENV PATH="/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG/bin:$PATH"

56
Makefile
View file

@ -64,8 +64,6 @@ endif
# Things that need to be maintained as the source changes
#
FORKNAME = inav
# Working directories
SRC_DIR := $(ROOT)/src/main
OBJECT_DIR := $(ROOT)/obj/main
@ -85,17 +83,9 @@ MHZ_VALUE ?=
# used for turning on features like VCP and SDCARD
FEATURES =
include $(ROOT)/make/version.mk
include $(ROOT)/make/targets.mk
REVISION = $(shell git rev-parse --short HEAD)
FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )
FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )
FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
FC_VER_SUFFIX ?=
BUILD_DATE = $(shell date +%Y%m%d)
# Search path for sources
@ -174,12 +164,15 @@ SIZE = $(ARM_SDK_PREFIX)size
# Tool options.
#
# Save original CFLAGS before modifying them, so we don't
# add them twice when calling this Makefile recursively
# for each target
SAVED_CFLAGS := $(CFLAGS)
ifeq ($(DEBUG),GDB)
OPTIMIZE = -O0
LTO_FLAGS = $(OPTIMIZE)
LTO_FLAGS =
else
OPTIMIZE = -Os
LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE)
LTO_FLAGS = -flto -fuse-linker-plugin
endif
ifneq ($(SEMIHOSTING),)
@ -252,9 +245,6 @@ CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
# Things we will build
#
TARGET_BIN := $(BIN_DIR)/$(FORKNAME)_$(FC_VER)
ifneq ($(FC_VER_SUFFIX),)
TARGET_BIN := $(TARGET_BIN)-$(FC_VER_SUFFIX)
endif
TARGET_BIN := $(TARGET_BIN)_$(TARGET)
ifneq ($(BUILD_SUFFIX),)
TARGET_BIN := $(TARGET_BIN)_$(BUILD_SUFFIX)
@ -301,16 +291,38 @@ $(TARGET_ELF): $(TARGET_OBJS)
$(V1) $(CROSS_CC) -o $@ $(filter %.o, $^) $(LDFLAGS)
$(V0) $(SIZE) $(TARGET_ELF)
OPTIMIZE_FLAG_SPEED := "-Os"
OPTIMIZE_FLAG_SIZE := "-Os"
OPTIMIZE_FLAG_NORMAL := "-Os"
ifneq ($(TARGET_MCU_GROUP), STM32F3)
OPTIMIZE_FLAG_SPEED := "-Ofast"
OPTIMIZE_FLAG_SIZE := "-Os"
OPTIMIZE_FLAG_NORMAL := "-O2"
endif
define compile_file
echo "%% $(1) $(2) $<" "$(STDOUT)" && \
$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
endef
# Compile
$(TARGET_OBJ_DIR)/%.o: %.c
$(V1) mkdir -p $(dir $@)
$(V1) echo %% $(notdir $<) "$(STDOUT)"
$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $<
$(V1) $(if $(findstring $<,$(SIZE_OPTIMISED_SRC)), \
$(call compile_file,(size),$(OPTIMIZE_FLAG_SIZE)) \
, \
$(if $(findstring $<,$(SPEED_OPTIMISED_SRC)), \
$(call compile_file,(speed),$(OPTIMIZE_FLAG_SPEED)) \
, \
$(call compile_file,(normal),$(OPTIMIZE_FLAG_NORMAL)) \
) \
)
ifeq ($(GENERATE_ASM), 1)
$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $<
endif
# Assemble
$(TARGET_OBJ_DIR)/%.o: %.s
$(V1) mkdir -p $(dir $@)
@ -340,7 +352,7 @@ release: $(RELEASE_TARGETS)
$(VALID_TARGETS):
$(V0) echo "" && \
echo "Building $@" && \
$(MAKE) -j 8 TARGET=$@ && \
CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$@ && \
echo "Building $@ succeeded."
## clean : clean up all temporary / machine-generated files

14
docs/Cli.md
View file

@ -363,8 +363,6 @@ A shorter form is also supported to enable and disable functions using `serial <
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
| nav_mc_vel_z_d | 10 | D gain of velocity PID controller |
| nav_mc_pos_xy_p | 65 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity |
| nav_mc_pos_xy_i | 120 | Controls deceleration time. Measured in 1/100 sec. Expected hold position is placed at a distance calculated as decelerationTime * currentVelocity |
| nav_mc_pos_xy_d | 10 | |
| nav_mc_vel_xy_p | 40 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations |
| nav_mc_vel_xy_i | 15 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot |
| nav_mc_vel_xy_d | 100 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. |
@ -423,20 +421,16 @@ A shorter form is also supported to enable and disable functions using `serial <
| dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter |
| dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
| dyn_notch_width_percent | 8 | Distance in % of the attenuated frequency for double dynamic filter notched. When set to `0` single dynamic notch filter is used |
| dyn_notch_range | MEDIUM | Dynamic gyro filter range. Possible values `LOW` `MEDIUM` `HIGH`. `MEDIUM` should work best for 5-6" multirotors. `LOW` should work best with 7" and bigger. `HIGH` should work with everything below 4" |
| dyn_notch_q | 120 | Q factor for dynamic notches |
| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
| dynamic_gyro_notch_enabled | `OFF` | Enable/disable dynamic gyro notch also known as Matrix Filter |
| dynamic_gyro_notch_range | `MEDIUM` | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers |
| dynamic_gyro_notch_q | 120 | Q factor for dynamic notches |
| dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) |
| gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| rpm_gyro_filter_enabled | `OFF` | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV |
| rpm_dterm_filter_enabled | `OFF` | RPM filter for D-term. Experimental, probably will be removed in the next release |
| rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine |
| rpm_gyro_min_hz | 150 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` |
| rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting |
| dterm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by D-term RPM filter. Default value of `1` usually works just fine |
| rpm_dterm_min_hz | 150 | - |
| rpm_dterm_q | 500 | - |
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| `pid_type` | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` |

9
docs/Logic Conditions.md
View file

@ -70,6 +70,15 @@ Logic conditions can be edited using INAV Configurator user interface, of via CL
| 14 | TROTTLE_POS | in `%` |
| 15 | ATTITUDE_ROLL | in `degrees` |
| 16 | ATTITUDE_PITCH | in `degrees` |
| 17 | IS_ARMED | boolean `0`/`1` |
| 18 | IS_AUTOLAUNCH | boolean `0`/`1` |
| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` |
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_WP | boolean `0`/`1` |
| 24 | IS_LANDING | boolean `0`/`1` |
| 25 | IS_FAILSAFE | boolean `0`/`1` |
#### FLIGHT_MODE

61
docs/Navigation.md
View file

@ -1,6 +1,6 @@
# Navigation
Navigation system in INAV is responsible for assisting the pilot allowing altitude and position hold, return-to-home and waypoint flight.
The navigation system in INAV is responsible for assisting the pilot allowing altitude and position hold, return-to-home and waypoint flight.
## NAV ALTHOLD mode - altitude hold
@ -23,7 +23,7 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co
## NAV POSHOLD mode - position hold
Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). Can be activated together with ALTHOLD to get a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From inav 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
### CLI parameters affecting ALTHOLD mode:
* *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.
@ -36,7 +36,7 @@ PID meaning:
## NAV RTH - return to home mode
Home for RTH is position, where copter was armed. RTH requires accelerometer, compass and GPS sensors.
Home for RTH is the position where vehicle was armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. RTH requires accelerometer, compass and GPS sensors.
If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
@ -54,22 +54,57 @@ When deciding what altitude to maintain, RTH has 4 different modes of operation
`wp load` - Load list of waypoints from EEPROM to FC.
`wp <n> <action> <lat> <lon> <alt> <p1> <flag>` - Set parameters of waypoint with index `<n>`.
`wp <n> <action> <lat> <lon> <alt> <p1> <p2> <p3> <flag>` - Set parameters of waypoint with index `<n>`. Note that prior to inav 2.5, the `p2` and `p3` parameters were not required. From 2.5, inav will accept either version but always saves and lists the later full version.
Parameters:
* `<action>` - When 0 then waypoint is not used, when 1 then it is normal waypoint, when 4 then it is RTH.
* `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.5 and later:
* 0 - Unused / Unassigned
* 1 - WAYPOINT
* 3 - POSHOLD_TIME
* 4 - RTH
* 6 - JUMP
* 8 - LAND
* `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
* `<lon>` - Longitude.
* `<alt>` - Altitude in cm.
* `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
* `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number).
* `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
* `<p3>` - Reserved for future use. If `p2` is provided, then `p3` is also required.
* `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.
`wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`).
`wp reset` - Resets the list, sets the waypoints number to 0 and mark it as invalid (but doesn't delete the waypoints).
`wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions).
### `wp` example
```
# wp load
# wp
#wp 11 valid
wp 0 1 543533193 -45179273 3500 0 0 0 0
wp 1 1 543535723 -45193913 3500 0 0 0 0
wp 2 1 543544541 -45196617 5000 0 0 0 0
wp 3 1 543546578 -45186895 5000 0 0 0 0
wp 4 6 0 0 0 1 2 0 0
wp 5 1 543546688 -45176009 3500 0 0 0 0
wp 6 1 543541225 -45172673 3500 0 0 0 0
wp 7 6 0 0 0 0 1 0 0
wp 8 3 543531383 -45190405 3500 45 0 0 0
wp 9 1 543548470 -45182104 3500 0 0 0 0
wp 10 8 543540521 -45178091 6000 0 0 0 165
wp 11 0 0 0 0 0 0 0 0
...
wp 59 0 0 0 0 0 0 0 0
```
Note that the `wp` CLI command shows waypoint list indices, while the MW-XML definition used by mwp, ezgui and the configurator use WP numbers.

4
docs/Rx.md
View file

@ -154,11 +154,9 @@ The receiver type can be set from the configurator or CLI.
```
# get receiver_type
receiver_type = NONE
Allowed values: NONE, PWM, PPM, SERIAL, MSP, SPI, UIB
Allowed values: NONE, PPM, SERIAL, MSP, SPI, UIB
```
Note that `PWM` is a synonym for `NONE`.
### RX signal-loss detection
The software has signal loss detection which is always enabled. Signal loss detection is used for safety and failsafe reasons.

111
docs/development/Building in Ubuntu.md
View file

@ -1,111 +0,0 @@
# Building in Ubuntu
iNav requires a reasonably modern `gcc-arm-none-eabi` compiler. As a consequence of the long term support options in Ubuntu, it is possible that the distribution compiler will be too old to build iNav firmware. For example, Ubuntu 16.04 LTS ships with version 4.9.3 which cannot compile contemporary iNav.
As of August 2018, the recommendation for Ubuntu releases is:
| Release | Compiler Source |
| ------- | --------------- |
| 16.04 or earlier | Use the 'official' PPA |
| 17.10 | Use the 'official' PPA as the distro compiler (5.4) *may* be too old |
| 18.04 | Use the 'official' PPA, as the distro compiler (6.3) was broken when last tested |
For Ubuntu derivatives (ElementaryOS, Mint etc.), please check the distro provided version, and if it's lower than 6, use the PPA.
e.g. ElementaryOS
```
$ apt show gcc-arm-none-eabi
...
Version: 15:4.9.3+svn231177-1
```
This 4.9.3 and will not build iNav, so we need the PPA.
## Installer commands
Older versions of Debian / Ubuntu and derivatives use the `apt-get` command; newer versions use `apt`. Use the appropriate command for your release.
# Prerequisites
Regardless of the cross-compiler version, it is necessary to install some other tools:
```
sudo apt install git
sudo apt install gcc
sudo apt install ruby
```
A ruby release of at least 2 or later is recommended, if your release only provides 1.9, it is necessary to install a later version:
```
sudo apt-get remove ruby
sudo apt-add-repository ppa:brightbox/ruby-ng
sudo apt-get update
sudo apt-get install ruby2.4 ruby2.4-dev
```
# Using the Distro compiler
In case Ubuntu ever provides a modern compiler (as of August 2018, not recommended):
```
sudo apt install gcc-arm-none-eabi
```
# Using the PPA compiler
The PPA compiler is recommended for all cases:
```
sudo apt-get remove binutils-arm-none-eabi gcc-arm-none-eabi
sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa
sudo apt-get update
sudo apt-get install gcc-arm-embedded
```
After these steps, on Ubuntu 16.04, (at least of March 2018) you should now have:
```
$ arm-none-eabi-gcc -dumpversion
7.2.1
```
Which is more than adequate for our purposes.
# Building from the Github repository
After the ARM cross-compiler toolchain from is installed, you should be able to build from source.
```
mkdir src
cd src
git clone https://github.com/iNavFlight/inav.git
cd inav
make
```
If you have a github account with registered ssh key you can replace the `git clone` command with `git clone git@github.com:iNavFlight/inav.git` instead of the https link.
By default, this builds the REVO target, to build another target, specify the target name to the make command, for example:
```
make TARGET=MATEKF405
```
The resultant hex file are in the `obj/` directory.
You can use the INAV Configurator to flash the local ```obj/inav_TARGET.hex``` file, or use `stm32flash` or `dfu-util` directly from the command line.
[msp-tool](https://github.com/fiam/msp-tool) and [flash.sh](https://github.com/stronnag/mwptools/blob/master/docs/MiscTools.asciidoc#flashsh) provide / describe 3rd party helper tools for command line flashing.
# Updating and rebuilding
In order to update your local firmware build:
* Navigate to the local iNav repository
* Use the following steps to pull the latest changes and rebuild your local version of iNav:
```
cd src/inav
git pull
make TARGET=<TARGET>
```

10
docs/development/serial_printf_debugging.md
View file

@ -2,10 +2,12 @@
## Overview
iNav offers a function to use serial `printf` style debugging.
inav offers a function to use serial `printf` style debugging.
This provides a simple and intuitive debugging facility. This facility is only available after the serial sub-system has been initialised, which should be adequate for all but the most hard-core debugging requirements.
In order to use this feature, the source file must include `common/log.h`.
## CLI settings
It is necessary to set a serial port for serial logging using the function mask `FUNCTION_LOG`, 32768. For convenience this may be shared with MSP (mask 1), but no other function.
@ -42,7 +44,7 @@ Log levels are defined in `src/main/common/log.h`, at the time of writing these
These are used at both compile time and run time.
At compile time, a maximum level may be defined. As of iNav 2.3, for F3 targets the maximum level is ERROR, for F4/F7 the maximum level is DEBUG.
At compile time, a maximum level may be defined. As of inav 2.3, for F3 targets the maximum level is ERROR, for F4/F7 the maximum level is DEBUG.
At run time, the level defines the level that will be displayed, so for a F4 or F7 target that has compile time suport for all log levels, if the CLI sets
```
@ -52,7 +54,7 @@ then only `ERROR`, `WARNING` and `INFO` levels will be output.
## Log Topic
Log levels are defined in `src/main/common/log.h`, at the time of writing:
Log topics are defined in `src/main/common/log.h`, at the time of writing:
* SYSTEM
* GYRO
@ -96,7 +98,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa
* msp-tool https://github.com/fiam/msp-tool
* mwp https://github.com/stronnag/mwptools
* iNav Configurator
* inav Configurator
For example, with the final lines of `src/main/fc/fc_init.c` set to:

17
make/source.mk
View file

@ -99,6 +99,7 @@ COMMON_SRC = \
flight/wind_estimator.c \
flight/gyroanalyse.c \
flight/rpm_filter.c \
flight/dynamic_gyro_notch.c \
io/beeper.c \
io/esc_serialshot.c \
io/frsky_osd.c \
@ -162,9 +163,7 @@ COMMON_SRC = \
cms/cms_menu_navigation.c \
cms/cms_menu_osd.c \
cms/cms_menu_saveexit.c \
cms/cms_menu_vtx_smartaudio.c \
cms/cms_menu_vtx_tramp.c \
cms/cms_menu_vtx_ffpv.c \
cms/cms_menu_vtx.c \
drivers/display_ug2864hsweg01.c \
drivers/rangefinder/rangefinder_hcsr04.c \
drivers/rangefinder/rangefinder_hcsr04_i2c.c \
@ -270,7 +269,6 @@ TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
@ -279,3 +277,14 @@ endif
# Search path and source files for the ST stdperiph library
VPATH := $(VPATH):$(STDPERIPH_DIR)/src
SIZE_OPTIMISED_SRC := ""
SPEED_OPTIMISED_SRC := ""
ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
# SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
# ./src/main/common/filter.c \
# SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
# ./src/main/common/maths.c \
endif #!F3

7
make/system-id.mk
View file

@ -18,6 +18,13 @@ ifeq ($(UNAME), Linux)
LINUX := 1
endif
# FreeBSD
ifeq ($(UNAME), FreeBSD)
OSFAMILY := linux
LINUX := 1
endif
# Mac OSX
ifeq ($(UNAME), Darwin)
OSFAMILY := macosx

34
make/tools.mk
View file

@ -7,8 +7,9 @@
#
###############################################################
GCC_REQUIRED_VERSION ?= 8.2.1
ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-8-2018-q4-major
GCC_REQUIRED_VERSION ?= 9.2.1
ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-9-2019-q4-major
ARM_SDK_URL_BASE := https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/9-2019q4/gcc-arm-none-eabi-9-2019-q4-major
.PHONY: arm_sdk_version
@ -17,11 +18,9 @@ arm_sdk_version:
.PHONY: arm_sdk_install
ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/8-2018q4/gcc-arm-none-eabi-8-2018-q4-major
# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
ifdef LINUX
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-x86_64-linux.tar.bz2
endif
ifdef MACOSX
@ -33,9 +32,20 @@ ifdef WINDOWS
endif
ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
ARM_SDK_DOWNLOAD_PATH := $(DL_DIR)/$(ARM_SDK_FILE)
SDK_INSTALL_MARKER := $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc-$(GCC_REQUIRED_VERSION)
.PHONY: arm_sdk_print_filename
arm_sdk_print_filename:
@echo $(ARM_SDK_FILE)
.PHONY: arm_sdk_print_download_path
arm_sdk_print_download_path:
@echo $(ARM_SDK_DOWNLOAD_PATH)
arm_sdk_install: | $(TOOLS_DIR)
arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER)
@ -43,17 +53,17 @@ arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER)
$(SDK_INSTALL_MARKER):
ifneq ($(OSFAMILY), windows)
# binary only release so just extract it
$(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)"
$(V1) tar -C $(TOOLS_DIR) -xjf "$(ARM_SDK_DOWNLOAD_PATH)"
else
$(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)"
$(V1) unzip -q -d $(ARM_SDK_DIR) "$(ARM_SDK_DOWNLOAD_PATH)"
endif
.PHONY: arm_sdk_download
arm_sdk_download: | $(DL_DIR)
arm_sdk_download: $(DL_DIR)/$(ARM_SDK_FILE)
$(DL_DIR)/$(ARM_SDK_FILE):
arm_sdk_download: $(ARM_SDK_DOWNLOAD_PATH)
$(ARM_SDK_DOWNLOAD_PATH):
# download the source only if it's newer than what we already have
$(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" -z "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)"
$(V1) curl -L -k -o "$(ARM_SDK_DOWNLOAD_PATH)" -z "$(ARM_SDK_DOWNLOAD_PATH)" "$(ARM_SDK_URL)"
## arm_sdk_clean : Uninstall Arm SDK
@ -70,7 +80,7 @@ arm_sdk_clean:
ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists)
ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi-
else ifeq (,$(findstring _install,$(MAKECMDGOALS)))
else ifeq (,$(findstring print_,$(MAKECMDGOALS))$(filter targets,$(MAKECMDGOALS))$(findstring arm_sdk,$(MAKECMDGOALS)))
GCC_VERSION = $(shell arm-none-eabi-gcc -dumpversion)
ifeq ($(GCC_VERSION),)
$(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo)
@ -80,4 +90,4 @@ else ifeq (,$(findstring _install,$(MAKECMDGOALS)))
# ARM tookchain is in the path, and the version is what's required.
ARM_SDK_PREFIX ?= arm-none-eabi-
endif
endif

18
make/version.mk Normal file
View file

@ -0,0 +1,18 @@
FORKNAME := inav
FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )
FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )
FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
FC_VER_SUFFIX ?=
ifneq ($(FC_VER_SUFFIX),)
FC_VER += -$(FC_VER_SUFFIX)
endif
REVISION := $(shell git rev-parse --short HEAD)
.PHONY: print_version
print_version:
@echo $(FC_VER)

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

@ -624,14 +624,14 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
#ifdef USE_NAV
return STATE(FIXED_WING);
return STATE(FIXED_WING_LEGACY);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
#ifdef USE_NAV
return !STATE(FIXED_WING);
return !STATE(FIXED_WING_LEGACY);
#else
return false;
#endif
@ -1369,7 +1369,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->magADC[i] = mag.magADC[i];
#endif
#ifdef USE_NAV
if (!STATE(FIXED_WING)) {
if (!STATE(FIXED_WING_LEGACY)) {
// log requested velocity in cm/s
blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained);
@ -1384,7 +1384,7 @@ static void loadMainState(timeUs_t currentTimeUs)
}
#ifdef USE_NAV
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// log requested pitch in decidegrees
blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional);
@ -1693,10 +1693,9 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
@ -1726,10 +1725,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_harmonics", "%d", rpmFilterConfig()->gyro_harmonics);
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_min_hz", "%d", rpmFilterConfig()->gyro_min_hz);
BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_q", "%d", rpmFilterConfig()->gyro_q);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_filter_enabled", "%d", rpmFilterConfig()->dterm_filter_enabled);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_harmonics", "%d", rpmFilterConfig()->dterm_harmonics);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_min_hz", "%d", rpmFilterConfig()->dterm_min_hz);
BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_q", "%d", rpmFilterConfig()->dterm_q);
#endif
default:
return true;

79
src/main/build/atomic.h
View file

@ -17,25 +17,9 @@
#pragma once
// only set_BASEPRI is implemented in device library. It does always create memory barrier
// missing versions are implemented here
#ifdef UNIT_TEST
static inline void __set_BASEPRI(uint32_t basePri) {(void)basePri;}
static inline void __set_BASEPRI_MAX(uint32_t basePri) {(void)basePri;}
static inline void __set_BASEPRI_nb(uint32_t basePri) {(void)basePri;}
static inline void __set_BASEPRI_MAX_nb(uint32_t basePri) {(void)basePri;}
#else
// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier
__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri)
{
__ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
}
__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri)
{
__ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
}
#endif // UNIT_TEST
// cleanup BASEPRI restore function, with global memory barrier
@ -51,68 +35,19 @@ static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
return 1;
}
// cleanup BASEPRI restore function, no memory barrier
static inline void __basepriRestore(uint8_t *val)
{
__set_BASEPRI_nb(*val);
}
// set BASEPRI_MAX function, no memory barrier, returns true
static inline uint8_t __basepriSetRetVal(uint8_t prio)
{
__set_BASEPRI_MAX_nb(prio);
return 1;
}
// The CMSIS provides the function __set_BASEPRI(priority) for changing the value of the BASEPRI register.
// The function uses the hardware convention for the ‘priority’ argument, which means that the priority must
// be shifted left by the number of unimplemented bits (8 – __NVIC_PRIO_BITS).
//
// NOTE: The priority numbering convention used in __set_BASEPRI(priority) is thus different than in the
// NVIC_SetPriority(priority) function, which expects the “priority” argument not shifted.
// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
// Full memory barrier is placed at start and exit of block
#ifdef UNIT_TEST
#define ATOMIC_BLOCK(prio) {}
#define ATOMIC_BLOCK_NB(prio) {}
#else
#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
__ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
// Be careful when using this, you must use some method to prevent optimizer form breaking things
// - lto is used for Cleanflight compilation, so function call is not memory barrier
// - use ATOMIC_BARRIER or proper volatile to protect used variables
// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
// but that can change in future versions
#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
__ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
__ToDo = __basepriSetMemRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 )
#endif // UNIT_TEST
// ATOMIC_BARRIER
// Create memory barrier
// - at the beginning (all data must be reread from memory)
// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
// ideally this would only protect memory passed as parameter (any type should work), but gcc is currently creating almost full barrier
// this macro can be used only ONCE PER LINE, but multiple uses per block are fine
#if (__GNUC__ > 9)
#warning "Please verify that ATOMIC_BARRIER works as intended"
// increment version number is BARRIER works
// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
// you should check that local variable scope with cleanup spans entire block
#endif
#ifndef __UNIQL
# define __UNIQL_CONCAT2(x,y) x ## y
# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
#endif
// this macro uses local function for cleanup. CLang block can be substituted
#define ATOMIC_BARRIER(data) \
__extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \
__asm__ volatile ("\t# barier(" #data ") end\n" : : "m" (**__d)); \
} \
typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
__asm__ volatile ("\t# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
// define these wrappers for atomic operations, use gcc buildins
#define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val)
#define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val)

3
src/main/build/debug.h
View file

@ -67,5 +67,8 @@ typedef enum {
DEBUG_ERPM,
DEBUG_RPM_FILTER,
DEBUG_RPM_FREQ,
DEBUG_NAV_YAW,
DEBUG_DYNAMIC_FILTER,
DEBUG_DYNAMIC_FILTER_FREQUENCY,
DEBUG_COUNT
} debugType_e;

2
src/main/build/version.h
View file

@ -16,7 +16,7 @@
*/
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x

2
src/main/cms/cms.c
View file

@ -766,7 +766,7 @@ void cmsMenuOpen(void)
{
if (!cmsInMenu) {
// New open
setServoOutputEnabled(false);
setServoOutputEnabled(false);
pCurrentDisplay = cmsDisplayPortSelectCurrent();
if (!pCurrentDisplay)
return;

20
src/main/cms/cms_menu_builtin.c
View file

@ -50,13 +50,6 @@
#include "cms/cms_menu_battery.h"
#include "cms/cms_menu_misc.h"
// VTX supplied menus
#include "cms/cms_menu_vtx_smartaudio.h"
#include "cms/cms_menu_vtx_tramp.h"
#include "cms/cms_menu_vtx_ffpv.h"
// Info
static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1];
@ -111,19 +104,8 @@ static const OSD_Entry menuFeaturesEntries[] =
#if defined(USE_NAV)
OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation),
#endif
#if defined(VTX) || defined(USE_RTC6705)
OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtx),
#endif // VTX || USE_RTC6705
#if defined(USE_VTX_CONTROL)
#if defined(USE_VTX_SMARTAUDIO)
OSD_SUBMENU_ENTRY("VTX SA", &cmsx_menuVtxSmartAudio),
#endif
#if defined(USE_VTX_TRAMP)
OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp),
#endif
#if defined(USE_VTX_FFPV)
OSD_SUBMENU_ENTRY("VTX FFPV", &cmsx_menuVtxFFPV),
#endif
OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl),
#endif // VTX_CONTROL
#ifdef USE_LED_STRIP
OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip),

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

@ -378,6 +378,7 @@ static const OSD_Entry menuOsdHud2Entries[] = {
OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN),
OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX),
OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST),
OSD_SETTING_ENTRY("NEXT WAYPOINTS", SETTING_OSD_HUD_WP_DISP),
OSD_BACK_ENTRY,
OSD_END_ENTRY,
};

296
src/main/cms/cms_menu_vtx.c
View file

@ -15,132 +15,246 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <ctype.h>
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#include "build/version.h"
#if defined(USE_CMS) && defined(USE_VTX_CONTROL)
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_vtx.h"
#include "config/feature.h"
#include "drivers/vtx_common.h"
#ifdef USE_CMS
#include "fc/config.h"
#if defined(VTX) || defined(USE_RTC6705)
#include "io/vtx_string.h"
#include "io/vtx.h"
static bool featureRead = false;
static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;
static long cmsx_Vtx_FeatureRead(void)
{
if (!featureRead) {
cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0;
featureRead = true;
}
// Config-time settings
static uint8_t vtxBand = 0;
static uint8_t vtxChan = 0;
static uint8_t vtxPower = 0;
static uint8_t vtxPitMode = 0;
return 0;
}
static long cmsx_Vtx_FeatureWriteback(void)
{
if (cmsx_featureVtx)
featureSet(FEATURE_VTX);
else
featureClear(FEATURE_VTX);
return 0;
}
static const char * const vtxBandNames[] = {
"A",
"B",
"E",
"F",
"R",
static const char * const vtxCmsPitModeNames[] = {
"---", "OFF", "ON "
};
static const OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]};
static const OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1};
// Menus (these are not const because we update them at run-time )
static OSD_TAB_t cms_Vtx_EntBand = { &vtxBand, VTX_SETTINGS_BAND_COUNT, vtx58BandNames };
static OSD_TAB_t cms_Vtx_EntChan = { &vtxChan, VTX_SETTINGS_CHANNEL_COUNT, vtx58ChannelNames };
static OSD_TAB_t cms_Vtx_EntPower = { &vtxPower, VTX_SETTINGS_POWER_COUNT, vtx58DefaultPowerNames };
static const OSD_TAB_t cms_Vtx_EntPitMode = { &vtxPitMode, 2, vtxCmsPitModeNames };
static void cmsx_Vtx_ConfigRead(void)
{
#ifdef VTX
cmsx_vtxBand = masterConfig.vtx_band;
cmsx_vtxChannel = masterConfig.vtx_channel + 1;
#endif // VTX
#ifdef USE_RTC6705
cmsx_vtxBand = masterConfig.vtx_channel / 8;
cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1;
#endif // USE_RTC6705
}
static void cmsx_Vtx_ConfigWriteback(void)
{
#ifdef VTX
masterConfig.vtx_band = cmsx_vtxBand;
masterConfig.vtx_channel = cmsx_vtxChannel - 1;
#endif // VTX
#ifdef USE_RTC6705
masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1;
#endif // USE_RTC6705
}
static long cmsx_Vtx_onEnter(void)
{
cmsx_Vtx_FeatureRead();
cmsx_Vtx_ConfigRead();
return 0;
}
static long cmsx_Vtx_onExit(const OSD_Entry *self)
static long cms_Vtx_configPitMode(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
cmsx_Vtx_ConfigWriteback();
if (vtxPitMode == 0) {
vtxPitMode = 1;
}
// Pit mode changes are immediate, without saving
vtxCommonSetPitMode(vtxCommonDevice(), vtxPitMode >= 2 ? 1 : 0);
return 0;
}
#ifdef VTX
static const OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1};
static const OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1};
#endif // VTX
static const OSD_Entry cmsx_menuVtxEntries[] =
static long cms_Vtx_configBand(displayPort_t *pDisp, const void *self)
{
OSD_LABEL_ENTRY("--- VTX ---"),
OSD_BOOL_ENTRY("ENABLED", &cmsx_featureVtx),
#ifdef VTX
OSD_UINT8_ENTRY("VTX MODE", &entryVtxMode),
OSD_UINT16_ENTRY("VTX MHZ", &entryVtxMhz),
#endif // VTX
OSD_TAB_ENTRY("BAND", &entryVtxBand),
OSD_UINT8_ENTRY("CHANNEL", &entryVtxChannel),
#ifdef USE_RTC6705
OSD_BOOL_ENTRY("LOW POWER", &masterConfig.vtx_power),
#endif // USE_RTC6705
UNUSED(pDisp);
UNUSED(self);
if (vtxBand == 0) {
vtxBand = 1;
}
return 0;
}
static long cms_Vtx_configChan(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (vtxChan == 0) {
vtxChan = 1;
}
return 0;
}
static long cms_Vtx_configPower(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (vtxPower == 0) {
vtxPower = 1;
}
return 0;
}
static void cms_Vtx_initSettings(void)
{
vtxDevice_t * vtxDevice = vtxCommonDevice();
vtxDeviceCapability_t vtxDeviceCapability;
if (vtxCommonGetDeviceCapability(vtxDevice, &vtxDeviceCapability)) {
cms_Vtx_EntBand.max = vtxDeviceCapability.bandCount;
cms_Vtx_EntBand.names = (const char * const *)vtxDeviceCapability.bandNames;
cms_Vtx_EntChan.max = vtxDeviceCapability.channelCount;
cms_Vtx_EntChan.names = (const char * const *)vtxDeviceCapability.channelNames;
cms_Vtx_EntPower.max = vtxDeviceCapability.powerCount;
cms_Vtx_EntPower.names = (const char * const *)vtxDeviceCapability.powerNames;
}
else {
cms_Vtx_EntBand.max = VTX_SETTINGS_BAND_COUNT;
cms_Vtx_EntBand.names = vtx58BandNames;
cms_Vtx_EntChan.max = VTX_SETTINGS_CHANNEL_COUNT;
cms_Vtx_EntChan.names = vtx58ChannelNames;
cms_Vtx_EntPower.max = VTX_SETTINGS_POWER_COUNT;
cms_Vtx_EntPower.names = vtx58DefaultPowerNames;
}
vtxBand = vtxSettingsConfig()->band;
vtxChan = vtxSettingsConfig()->channel;
vtxPower = vtxSettingsConfig()->power;
// If device is ready - read actual PIT mode
if (vtxCommonDeviceIsReady(vtxDevice)) {
uint8_t onoff;
vtxCommonGetPitMode(vtxDevice, &onoff);
vtxPitMode = onoff ? 2 : 1;
}
else {
vtxPitMode = 0;
}
}
static long cms_Vtx_onEnter(const OSD_Entry *self)
{
UNUSED(self);
cms_Vtx_initSettings();
return 0;
}
static long cms_Vtx_Commence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxBand, vtxChan);
vtxCommonSetPowerByIndex(vtxCommonDevice(), vtxPower);
vtxCommonSetPitMode(vtxCommonDevice(), vtxPitMode == 2 ? 1 : 0);
vtxSettingsConfigMutable()->band = vtxBand;
vtxSettingsConfigMutable()->channel = vtxChan;
vtxSettingsConfigMutable()->power = vtxPower;
saveConfigAndNotify();
return MENU_CHAIN_BACK;
}
static bool cms_Vtx_drawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "-- ---- ----";
// bc ffff pppp
// 012345678901
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
vtxDevice_t * vtxDevice = vtxCommonDevice();
vtxDeviceOsdInfo_t osdInfo;
if (!vtxDevice || !vtxCommonGetOsdInfo(vtxDevice, &osdInfo) || !vtxCommonDeviceIsReady(vtxDevice)) {
return true;
}
buf[0] = osdInfo.bandLetter;
buf[1] = osdInfo.channelName[0];
buf[2] = ' ';
if (osdInfo.frequency)
tfp_sprintf(&buf[3], "%4d", osdInfo.frequency);
else
tfp_sprintf(&buf[3], "----");
if (osdInfo.powerIndex) {
// If OSD driver provides power in milliwatt - display MW, otherwise - power level
if (osdInfo.powerMilliwatt) {
tfp_sprintf(&buf[7], " %4d", osdInfo.powerMilliwatt);
}
else {
tfp_sprintf(&buf[7], " PL=%c", osdInfo.powerIndex);
}
} else {
tfp_sprintf(&buf[7], " ----");
}
return true;
}
static const OSD_Entry cms_menuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", cms_Vtx_Commence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtx = {
static const CMS_Menu cms_menuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTRC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cms_menuCommenceEntries,
};
static const OSD_Entry cms_menuVtxEntries[] =
{
OSD_LABEL_ENTRY("--- VTX ---"),
OSD_LABEL_FUNC_DYN_ENTRY("", cms_Vtx_drawStatusString),
OSD_TAB_CALLBACK_ENTRY("PIT", cms_Vtx_configPitMode, &cms_Vtx_EntPitMode),
OSD_TAB_CALLBACK_ENTRY("BAND", cms_Vtx_configBand, &cms_Vtx_EntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", cms_Vtx_configChan, &cms_Vtx_EntChan),
OSD_TAB_CALLBACK_ENTRY("POWER", cms_Vtx_configPower, &cms_Vtx_EntPower),
OSD_SUBMENU_ENTRY("SET", &cms_menuCommence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtxControl = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUVTX",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_Vtx_onEnter,
.onExit= cmsx_Vtx_onExit,
.onGlobalExit = cmsx_Vtx_FeatureWriteback,
.entries = cmsx_menuVtxEntries
.onEnter = cms_Vtx_onEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cms_menuVtxEntries
};
#endif // VTX || USE_RTC6705
#endif // CMS

2
src/main/cms/cms_menu_vtx.h
View file

@ -17,4 +17,4 @@
#pragma once
extern const CMS_Menu cmsx_menuVtx;
extern const CMS_Menu cmsx_menuVtxControl;

214
src/main/cms/cms_menu_vtx_ffpv.c
View file

@ -1,214 +0,0 @@
/*
* 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/>.
*/
#include <ctype.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(USE_CMS) && defined(USE_VTX_FFPV)
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "io/vtx_string.h"
#include "io/vtx_ffpv24g.h"
#include "io/vtx.h"
static bool ffpvCmsDrawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "- -- ---- ---";
// m bc ffff ppp
// 01234567890123
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_FFPV || !vtxCommonDeviceIsReady(vtxDevice)) {
return true;
}
buf[0] = '*';
buf[1] = ' ';
buf[2] = ffpvBandLetters[ffpvGetRuntimeState()->band];
buf[3] = ffpvChannelNames[ffpvGetRuntimeState()->channel][0];
buf[4] = ' ';
tfp_sprintf(&buf[5], "%4d", ffpvGetRuntimeState()->frequency);
tfp_sprintf(&buf[9], " %3d", ffpvGetRuntimeState()->powerMilliwatt);
return true;
}
uint8_t ffpvCmsBand = 1;
uint8_t ffpvCmsChan = 1;
uint16_t ffpvCmsFreqRef;
static uint8_t ffpvCmsPower = 1;
static const OSD_TAB_t ffpvCmsEntBand = { &ffpvCmsBand, VTX_FFPV_BAND_COUNT, ffpvBandNames };
static const OSD_TAB_t ffpvCmsEntChan = { &ffpvCmsChan, VTX_FFPV_CHANNEL_COUNT, ffpvChannelNames };
static const OSD_TAB_t ffpvCmsEntPower = { &ffpvCmsPower, VTX_FFPV_POWER_COUNT, ffpvPowerNames };
static void ffpvCmsUpdateFreqRef(void)
{
if (ffpvCmsBand > 0 && ffpvCmsChan > 0) {
ffpvCmsFreqRef = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1];
}
}
static long ffpvCmsConfigBand(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (ffpvCmsBand == 0) {
// Bounce back
ffpvCmsBand = 1;
}
else {
ffpvCmsUpdateFreqRef();
}
return 0;
}
static long ffpvCmsConfigChan(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (ffpvCmsChan == 0) {
// Bounce back
ffpvCmsChan = 1;
}
else {
ffpvCmsUpdateFreqRef();
}
return 0;
}
static long ffpvCmsConfigPower(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (ffpvCmsPower == 0) {
// Bounce back
ffpvCmsPower = 1;
}
return 0;
}
static long ffpvCmsCommence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
// call driver directly
ffpvSetBandAndChannel(ffpvCmsBand, ffpvCmsChan);
ffpvSetRFPowerByIndex(ffpvCmsPower);
// update'vtx_' settings
vtxSettingsConfigMutable()->band = ffpvCmsBand;
vtxSettingsConfigMutable()->channel = ffpvCmsChan;
vtxSettingsConfigMutable()->power = ffpvCmsPower;
vtxSettingsConfigMutable()->freq = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1];
saveConfigAndNotify();
return MENU_CHAIN_BACK;
}
static void ffpvCmsInitSettings(void)
{
ffpvCmsBand = ffpvGetRuntimeState()->band;
ffpvCmsChan = ffpvGetRuntimeState()->channel;
ffpvCmsPower = ffpvGetRuntimeState()->powerIndex;
ffpvCmsUpdateFreqRef();
}
static long ffpvCmsOnEnter(const OSD_Entry *from)
{
UNUSED(from);
ffpvCmsInitSettings();
return 0;
}
static const OSD_Entry ffpvCmsMenuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", ffpvCmsCommence),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu ffpvCmsMenuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTRC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = ffpvCmsMenuCommenceEntries,
};
static const OSD_Entry ffpvMenuEntries[] =
{
OSD_LABEL_ENTRY("- TRAMP -"),
OSD_LABEL_FUNC_DYN_ENTRY("", ffpvCmsDrawStatusString),
OSD_TAB_CALLBACK_ENTRY("BAND", ffpvCmsConfigBand, &ffpvCmsEntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", ffpvCmsConfigChan, &ffpvCmsEntChan),
OSD_UINT16_RO_ENTRY("(FREQ)", &ffpvCmsFreqRef),
OSD_TAB_CALLBACK_ENTRY("POWER", ffpvCmsConfigPower, &ffpvCmsEntPower),
OSD_SUBMENU_ENTRY("SET", &ffpvCmsMenuCommence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtxFFPV = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTR",
.GUARD_type = OME_MENU,
#endif
.onEnter = ffpvCmsOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = ffpvMenuEntries,
};
#endif

23
src/main/cms/cms_menu_vtx_ffpv.h
View file

@ -1,23 +0,0 @@
/*
* 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
#include "cms/cms.h"
#include "cms/cms_types.h"
extern const CMS_Menu cmsx_menuVtxFFPV;

710
src/main/cms/cms_menu_vtx_smartaudio.c
View file

@ -1,710 +0,0 @@
/*
* 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/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#if defined(USE_CMS) && defined(USE_VTX_SMARTAUDIO)
#include "common/log.h"
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_vtx_smartaudio.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "io/vtx_string.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx.h"
// Interface to CMS
// Operational Model and RF modes (CMS)
#define SACMS_OPMODEL_UNDEF 0 // Not known yet
#define SACMS_OPMODEL_FREE 1 // Freestyle model: Power up transmitting
#define SACMS_OPMODEL_RACE 2 // Race model: Power up in pit mode
uint8_t saCmsOpmodel = SACMS_OPMODEL_UNDEF;
#define SACMS_TXMODE_NODEF 0
#define SACMS_TXMODE_PIT_OUTRANGE 1
#define SACMS_TXMODE_PIT_INRANGE 2
#define SACMS_TXMODE_ACTIVE 3
uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently used
uint8_t saCmsBand = 0;
uint8_t saCmsChan = 0;
uint8_t saCmsPower = 0;
// Frequency derived from channel table (used for reference in band/channel mode)
uint16_t saCmsFreqRef = 0;
uint16_t saCmsDeviceFreq = 0;
uint8_t saCmsDeviceStatus = 0;
uint8_t saCmsPower;
uint8_t saCmsPitFMode; // Undef(0), In-Range(1) or Out-Range(2)
uint8_t saCmsFselMode; // Channel(0) or User defined(1)
uint8_t saCmsFselModeNew; // Channel(0) or User defined(1)
uint16_t saCmsORFreq = 0; // POR frequency
uint16_t saCmsORFreqNew; // POR frequency
uint16_t saCmsUserFreq = 0; // User defined frequency
uint16_t saCmsUserFreqNew; // User defined frequency
static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self);
static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self);
static long saCmsConfigBandByGvar(displayPort_t *, const void *self);
static long saCmsConfigChanByGvar(displayPort_t *, const void *self);
static long saCmsConfigPowerByGvar(displayPort_t *, const void *self);
static bool saCmsUpdateCopiedState(void)
{
if (saDevice.version == 0)
return false;
if (saCmsDeviceStatus == 0 && saDevice.version != 0)
saCmsDeviceStatus = saDevice.version;
if (saCmsORFreq == 0 && saDevice.orfreq != 0)
saCmsORFreq = saDevice.orfreq;
if (saCmsUserFreq == 0 && saDevice.freq != 0)
saCmsUserFreq = saDevice.freq;
if (saDevice.version == 2) {
if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
saCmsPitFMode = 1;
else
saCmsPitFMode = 0;
}
return true;
}
static bool saCmsDrawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "- -- ---- ---";
// m bc ffff ppp
// 0123456789012
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
if (!saCmsUpdateCopiedState()) {
// VTX is not ready
return true;
}
buf[0] = "-FR"[saCmsOpmodel];
if (saCmsFselMode == 0) {
buf[2] = "ABEFR"[saDevice.channel / 8];
buf[3] = '1' + (saDevice.channel % 8);
} else {
buf[2] = 'U';
buf[3] = 'F';
}
if ((saDevice.mode & SA_MODE_GET_PITMODE)
&& (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE))
tfp_sprintf(&buf[5], "%4d", saDevice.orfreq);
else if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ)
tfp_sprintf(&buf[5], "%4d", saDevice.freq);
else
tfp_sprintf(&buf[5], "%4d",
vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]);
buf[9] = ' ';
if (saDevice.mode & SA_MODE_GET_PITMODE) {
buf[10] = 'P';
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
buf[11] = 'I';
} else {
buf[11] = 'O';
}
buf[12] = 'R';
buf[13] = 0;
} else {
tfp_sprintf(&buf[10], "%3d", (saDevice.version == 2) ? saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower);
}
return true;
}
void saCmsUpdate(void)
{
// XXX Take care of pit mode update somewhere???
if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) {
// This is a first valid response to GET_SETTINGS.
saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE;
saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0;
saCmsBand = vtxSettingsConfig()->band;
saCmsChan = vtxSettingsConfig()->channel;
saCmsFreqRef = vtxSettingsConfig()->freq;
saCmsDeviceFreq = saCmsFreqRef;
if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) {
saCmsRFState = SACMS_TXMODE_ACTIVE;
} else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
saCmsRFState = SACMS_TXMODE_PIT_INRANGE;
} else {
saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE;
}
saCmsPower = vtxSettingsConfig()->power;
// if user-freq mode then track possible change
if (saCmsFselMode && vtxSettingsConfig()->freq) {
saCmsUserFreq = vtxSettingsConfig()->freq;
}
saCmsFselModeNew = saCmsFselMode; //init mode for menu
}
saCmsUpdateCopiedState();
}
void saCmsResetOpmodel()
{
// trigger data refresh in 'saCmsUpdate()'
saCmsOpmodel = SACMS_OPMODEL_UNDEF;
}
static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 0) {
// Bounce back; not online yet
saCmsBand = 0;
return 0;
}
if (saCmsBand == 0) {
// Bouce back, no going back to undef state
saCmsBand = 1;
return 0;
}
if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
saCmsFreqRef = vtx58frequencyTable[saCmsBand - 1][saCmsChan - 1];
return 0;
}
static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 0) {
// Bounce back; not online yet
saCmsChan = 0;
return 0;
}
if (saCmsChan == 0) {
// Bounce back; no going back to undef state
saCmsChan = 1;
return 0;
}
if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred)
saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
saCmsFreqRef = vtx58frequencyTable[saCmsBand - 1][saCmsChan - 1];
return 0;
}
static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 0) {
// Bounce back; not online yet
saCmsPower = 0;
return 0;
}
if (saCmsPower == 0) {
// Bouce back; no going back to undef state
saCmsPower = 1;
return 0;
}
if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) {
vtxSettingsConfigMutable()->power = saCmsPower;
}
return 0;
}
static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 1) {
// V1 device doesn't support PIT mode; bounce back.
saCmsPitFMode = 0;
return 0;
}
LOG_D(VTX, "saCmsConfigPitFmodeByGbar: saCmsPitFMode %d", saCmsPitFMode);
if (saCmsPitFMode == 0) {
// Bounce back
saCmsPitFMode = 1;
return 0;
}
if (saCmsPitFMode == 1) {
saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
} else {
saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
}
return 0;
}
static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self); // Forward
static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 1) {
if (saCmsOpmodel != SACMS_OPMODEL_FREE)
saCmsOpmodel = SACMS_OPMODEL_FREE;
return 0;
}
uint8_t opmodel = saCmsOpmodel;
LOG_D(VTX, "saCmsConfigOpmodelByGvar: opmodel %d", opmodel);
if (opmodel == SACMS_OPMODEL_FREE) {
// VTX should power up transmitting.
// Turn off In-Range and Out-Range bits
saSetMode(0);
} else if (opmodel == SACMS_OPMODEL_RACE) {
// VTX should power up in pit mode.
// Default PitFMode is in-range to prevent users without
// out-range receivers from getting blinded.
saCmsPitFMode = 0;
saCmsConfigPitFModeByGvar(pDisp, self);
// Direct frequency mode is not available in RACE opmodel
saCmsFselModeNew = 0;
saCmsConfigFreqModeByGvar(pDisp, self);
} else {
// Trying to go back to unknown state; bounce back
saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1;
}
return 0;
}
#ifdef USE_EXTENDED_CMS_MENUS
static const char * const saCmsDeviceStatusNames[] = {
"OFFL",
"ONL V1",
"ONL V2",
};
static const OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames };
static const OSD_Entry saCmsMenuStatsEntries[] = {
OSD_LABEL_ENTRY("- SA STATS -"),
OSD_TAB_DYN_ENTRY("STATUS", &saCmsEntOnline),
OSD_UINT16_RO_ENTRY("BAUDRATE", &sa_smartbaud),
OSD_UINT16_RO_ENTRY("SENT", &saStat.pktsent),
OSD_UINT16_RO_ENTRY("RCVD", &saStat.pktrcvd),
OSD_UINT16_RO_ENTRY("BADPRE", &saStat.badpre),
OSD_UINT16_RO_ENTRY("BADLEN", &saStat.badlen),
OSD_UINT16_RO_ENTRY("CRCERR", &saStat.crc),
OSD_UINT16_RO_ENTRY("OOOERR", &saStat.ooopresp),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuStats = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSAST",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuStatsEntries
};
#endif /* USE_EXTENDED_CMS_MENUS */
static const OSD_TAB_t saCmsEntBand = { &saCmsBand, VTX_SMARTAUDIO_BAND_COUNT, vtx58BandNames };
static const OSD_TAB_t saCmsEntChan = { &saCmsChan, VTX_SMARTAUDIO_CHANNEL_COUNT, vtx58ChannelNames };
static const OSD_TAB_t saCmsEntPower = { &saCmsPower, VTX_SMARTAUDIO_POWER_COUNT, saPowerNames};
static const char * const saCmsOpmodelNames[] = {
"----",
"FREE",
"RACE",
};
static const char * const saCmsFselModeNames[] = {
"CHAN",
"USER"
};
static const char * const saCmsPitFModeNames[] = {
"---",
"PIR",
"POR"
};
static const OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
static long sacms_SetupTopMenu(const OSD_Entry *from); // Forward
static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
// if trying to do user frequency mode in RACE opmodel then
// revert because user-freq only available in FREE opmodel
if (saCmsFselModeNew != 0 && saCmsOpmodel != SACMS_OPMODEL_FREE) {
saCmsFselModeNew = 0;
}
// don't call 'saSetBandAndChannel()' / 'saSetFreq()' here,
// wait until SET / 'saCmsCommence()' is activated
sacms_SetupTopMenu(NULL);
return 0;
}
static long saCmsCommence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
const vtxSettingsConfig_t prevSettings = {
.band = vtxSettingsConfig()->band,
.channel = vtxSettingsConfig()->channel,
.freq = vtxSettingsConfig()->freq,
.power = vtxSettingsConfig()->power,
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
};
vtxSettingsConfig_t newSettings = prevSettings;
if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
// Race model
// Setup band, freq and power.
newSettings.band = saCmsBand;
newSettings.channel = saCmsChan;
newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
// If in pit mode, cancel it.
if (saCmsPitFMode == 0)
saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
else
saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE);
} else {
// Freestyle model
// Setup band and freq / user freq
if (saCmsFselModeNew == 0) {
newSettings.band = saCmsBand;
newSettings.channel = saCmsChan;
newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
} else {
saSetMode(0); //make sure FREE mode is setup
newSettings.band = 0;
newSettings.freq = saCmsUserFreq;
}
}
newSettings.power = saCmsPower;
if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) {
vtxSettingsConfigMutable()->band = newSettings.band;
vtxSettingsConfigMutable()->channel = newSettings.channel;
vtxSettingsConfigMutable()->power = newSettings.power;
vtxSettingsConfigMutable()->freq = newSettings.freq;
saveConfigAndNotify();
}
return MENU_CHAIN_BACK;
}
static long saCmsSetPORFreqOnEnter(const OSD_Entry *from)
{
UNUSED(from);
if (saDevice.version == 1)
return MENU_CHAIN_BACK;
saCmsORFreqNew = saCmsORFreq;
return 0;
}
static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
saSetPitFreq(saCmsORFreqNew);
return 0;
}
static char *saCmsORFreqGetString(void)
{
static char pbuf[5];
tfp_sprintf(pbuf, "%4d", saCmsORFreq);
return pbuf;
}
static char *saCmsUserFreqGetString(void)
{
static char pbuf[5];
tfp_sprintf(pbuf, "%4d", saCmsUserFreq);
return pbuf;
}
static long saCmsSetUserFreqOnEnter(const OSD_Entry *from)
{
UNUSED(from);
saCmsUserFreqNew = saCmsUserFreq;
return 0;
}
static long saCmsConfigUserFreq(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
saCmsUserFreq = saCmsUserFreqNew;
return MENU_CHAIN_BACK;
}
static const OSD_Entry saCmsMenuPORFreqEntries[] =
{
OSD_LABEL_ENTRY("- POR FREQ -"),
OSD_UINT16_RO_ENTRY("CUR FREQ", &saCmsORFreq),
OSD_UINT16_ENTRY("NEW FREQ", (&(const OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 })),
OSD_FUNC_CALL_ENTRY("SET", saCmsSetPORFreq),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuPORFreq =
{
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSAPOR",
.GUARD_type = OME_MENU,
#endif
.onEnter = saCmsSetPORFreqOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuPORFreqEntries,
};
static const OSD_Entry saCmsMenuUserFreqEntries[] =
{
OSD_LABEL_ENTRY("- USER FREQ -"),
OSD_UINT16_RO_ENTRY("CUR FREQ", &saCmsUserFreq),
OSD_UINT16_ENTRY("NEW FREQ", (&(const OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 })),
OSD_FUNC_CALL_ENTRY("SET", saCmsConfigUserFreq),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuUserFreq =
{
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSAUFQ",
.GUARD_type = OME_MENU,
#endif
.onEnter = saCmsSetUserFreqOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuUserFreqEntries,
};
static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselModeNew, 1, saCmsFselModeNames };
static const OSD_Entry saCmsMenuConfigEntries[] =
{
OSD_LABEL_ENTRY("- SA CONFIG -"),
{ "OP MODEL", {.func = saCmsConfigOpmodelByGvar}, &(const OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, OME_TAB, DYNAMIC },
{ "FSEL MODE", {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, OME_TAB, DYNAMIC },
OSD_TAB_CALLBACK_ENTRY("PIT FMODE", saCmsConfigPitFModeByGvar, &saCmsEntPitFMode),
{ "POR FREQ", {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OME_Submenu, OPTSTRING },
#ifdef USE_EXTENDED_CMS_MENUS
OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats),
#endif /* USE_EXTENDED_CMS_MENUS */
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuConfig = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSACFG",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuConfigEntries
};
static const OSD_Entry saCmsMenuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", saCmsCommence),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXCOM",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuCommenceEntries,
};
#pragma GCC diagnostic push
#if (__GNUC__ > 7)
// This is safe on 32bit platforms, suppress warning for saCmsUserFreqGetString
#pragma GCC diagnostic ignored "-Wcast-function-type"
#endif
static const OSD_Entry saCmsMenuFreqModeEntries[] =
{
OSD_LABEL_ENTRY("- SMARTAUDIO -"),
OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
{ "FREQ", {.menufunc = saCmsUserFreqGetString}, &saCmsMenuUserFreq, OME_Submenu, OPTSTRING },
OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower),
OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence),
OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig),
OSD_BACK_AND_END_ENTRY,
};
#pragma GCC diagnostic pop
static const OSD_Entry saCmsMenuChanModeEntries[] =
{
OSD_LABEL_ENTRY("- SMARTAUDIO -"),
OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
OSD_TAB_CALLBACK_ENTRY("BAND", saCmsConfigBandByGvar, &saCmsEntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", saCmsConfigChanByGvar, &saCmsEntChan),
OSD_UINT16_RO_ENTRY("(FREQ)", &saCmsFreqRef),
OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower),
OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence),
OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig),
OSD_BACK_AND_END_ENTRY,
};
static const OSD_Entry saCmsMenuOfflineEntries[] =
{
OSD_LABEL_ENTRY("- VTX SMARTAUDIO -"),
OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
#ifdef USE_EXTENDED_CMS_MENUS
OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats),
#endif /* USE_EXTENDED_CMS_MENUS */
OSD_BACK_AND_END_ENTRY,
};
CMS_Menu cmsx_menuVtxSmartAudio; // Forward
static long sacms_SetupTopMenu(const OSD_Entry *from)
{
UNUSED(from);
if (saCmsDeviceStatus) {
if (saCmsFselModeNew == 0)
cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
else
cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;
} else {
cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries;
}
return 0;
}
CMS_Menu cmsx_menuVtxSmartAudio = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXSA",
.GUARD_type = OME_MENU,
#endif
.onEnter = sacms_SetupTopMenu,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuOfflineEntries,
};
#endif // CMS

29
src/main/cms/cms_menu_vtx_smartaudio.h
View file

@ -1,29 +0,0 @@
/*
* 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 "cms/cms.h"
#include "cms/cms_types.h"
extern CMS_Menu cmsx_menuVtxSmartAudio;
void saCmsUpdate(void);
void saCmsResetOpmodel(void);

254
src/main/cms/cms_menu_vtx_tramp.c
View file

@ -1,254 +0,0 @@
/*
* 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/>.
*/
#include <ctype.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(USE_CMS) && defined(USE_VTX_TRAMP)
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "io/vtx_string.h"
#include "io/vtx_tramp.h"
#include "io/vtx.h"
static bool trampCmsDrawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "- -- ---- ----";
// m bc ffff tppp
// 01234567890123
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_TRAMP || !vtxCommonDeviceIsReady(vtxDevice)) {
return true;
}
buf[0] = '*';
buf[1] = ' ';
buf[2] = vtx58BandLetter[trampData.band];
buf[3] = vtx58ChannelNames[trampData.channel][0];
buf[4] = ' ';
if (trampData.curFreq)
tfp_sprintf(&buf[5], "%4d", trampData.curFreq);
else
tfp_sprintf(&buf[5], "----");
if (trampData.power) {
tfp_sprintf(&buf[9], " %c%3d", (trampData.power == trampData.configuredPower) ? ' ' : '*', trampData.power);
} else {
tfp_sprintf(&buf[9], " ----");
}
return true;
}
uint8_t trampCmsPitMode = 0;
uint8_t trampCmsBand = 1;
uint8_t trampCmsChan = 1;
uint16_t trampCmsFreqRef;
static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, VTX_TRAMP_BAND_COUNT, vtx58BandNames };
static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUNT, vtx58ChannelNames };
static uint8_t trampCmsPower = 1;
static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, VTX_TRAMP_POWER_COUNT, trampPowerNames };
static void trampCmsUpdateFreqRef(void)
{
if (trampCmsBand > 0 && trampCmsChan > 0)
trampCmsFreqRef = vtx58frequencyTable[trampCmsBand - 1][trampCmsChan - 1];
}
static long trampCmsConfigBand(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (trampCmsBand == 0)
// Bounce back
trampCmsBand = 1;
else
trampCmsUpdateFreqRef();
return 0;
}
static long trampCmsConfigChan(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (trampCmsChan == 0)
// Bounce back
trampCmsChan = 1;
else
trampCmsUpdateFreqRef();
return 0;
}
static long trampCmsConfigPower(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (trampCmsPower == 0)
// Bounce back
trampCmsPower = 1;
return 0;
}
static const char * const trampCmsPitModeNames[] = {
"---", "OFF", "ON "
};
static const OSD_TAB_t trampCmsEntPitMode = { &trampCmsPitMode, 2, trampCmsPitModeNames };
static long trampCmsSetPitMode(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (trampCmsPitMode == 0) {
// Bouce back
trampCmsPitMode = 1;
} else {
trampSetPitMode(trampCmsPitMode - 1);
}
return 0;
}
static long trampCmsCommence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
trampSetBandAndChannel(trampCmsBand, trampCmsChan);
trampSetRFPower(trampPowerTable[trampCmsPower-1]);
// If it fails, the user should retry later
trampCommitChanges();
// update'vtx_' settings
vtxSettingsConfigMutable()->band = trampCmsBand;
vtxSettingsConfigMutable()->channel = trampCmsChan;
vtxSettingsConfigMutable()->power = trampCmsPower;
vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(trampCmsBand, trampCmsChan);
saveConfigAndNotify();
return MENU_CHAIN_BACK;
}
static void trampCmsInitSettings(void)
{
if (trampData.band > 0) trampCmsBand = trampData.band;
if (trampData.channel > 0) trampCmsChan = trampData.channel;
trampCmsUpdateFreqRef();
trampCmsPitMode = trampData.pitMode + 1;
if (trampData.configuredPower > 0) {
for (uint8_t i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
if (trampData.configuredPower <= trampPowerTable[i]) {
trampCmsPower = i + 1;
break;
}
}
}
}
static long trampCmsOnEnter(const OSD_Entry *from)
{
UNUSED(from);
trampCmsInitSettings();
return 0;
}
static const OSD_Entry trampCmsMenuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", trampCmsCommence),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu trampCmsMenuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTRC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = trampCmsMenuCommenceEntries,
};
static const OSD_Entry trampMenuEntries[] =
{
OSD_LABEL_ENTRY("- TRAMP -"),
OSD_LABEL_FUNC_DYN_ENTRY("", trampCmsDrawStatusString),
OSD_TAB_CALLBACK_ENTRY("PIT", trampCmsSetPitMode, &trampCmsEntPitMode),
OSD_TAB_CALLBACK_ENTRY("BAND", trampCmsConfigBand, &trampCmsEntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", trampCmsConfigChan, &trampCmsEntChan),
OSD_UINT16_RO_ENTRY("(FREQ)", &trampCmsFreqRef),
OSD_TAB_CALLBACK_ENTRY("POWER", trampCmsConfigPower, &trampCmsEntPower),
OSD_INT16_RO_ENTRY("T(C)", &trampData.temperature),
OSD_SUBMENU_ENTRY("SET", &trampCmsMenuCommence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtxTramp = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTR",
.GUARD_type = OME_MENU,
#endif
.onEnter = trampCmsOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = trampMenuEntries,
};
#endif

23
src/main/cms/cms_menu_vtx_tramp.h
View file

@ -1,23 +0,0 @@
/*
* 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
#include "cms/cms.h"
#include "cms/cms_types.h"
extern const CMS_Menu cmsx_menuVtxTramp;

2
src/main/common/calibration.h
View file

@ -33,7 +33,7 @@
#include "common/vector.h"
typedef enum {
ZERO_CALIBRATION_NONE,
ZERO_CALIBRATION_NONE = 0,
ZERO_CALIBRATION_IN_PROGRESS,
ZERO_CALIBRATION_DONE,
ZERO_CALIBRATION_FAIL,

4
src/main/common/encoding.c
View file

@ -15,6 +15,10 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "encoding.h"
/**

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

@ -22,6 +22,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
@ -116,10 +118,9 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
return filter->state;
}
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
{
const float octaves = log2f((float)centerFreq / (float)cutoff) * 2;
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
}
void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)

2
src/main/common/filter.h
View file

@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
float biquadFilterApply(biquadFilter_t *filter, float sample);
float biquadFilterReset(biquadFilter_t *filter, float value);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

2
src/main/common/global_functions.c
View file

@ -142,7 +142,7 @@ float NOINLINE getThrottleScale(float globalThrottleScale) {
}
}
int16_t FAST_CODE getRcCommandOverride(int16_t command[], uint8_t axis) {
int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
int16_t outputValue = command[axis];
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {

39
src/main/common/logic_condition.c
View file

@ -41,6 +41,9 @@
#include "sensors/pitotmeter.h"
#include "flight/imu.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
@ -271,6 +274,42 @@ static int logicConditionGetFlightOperandValue(int operand) {
return constrain(attitude.values.pitch / 10, -180, 180);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
return ARMING_FLAG(ARMED) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
return (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) ? 1 : 0;
break;
default:
return 0;
break;

43
src/main/common/logic_condition.h
View file

@ -64,23 +64,32 @@ typedef enum logicOperandType_s {
} logicOperandType_e;
typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s
LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m
LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m
LOGIC_CONDITION_OPERAND_FLIGHT_RSSI,
LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10
LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10
LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100
LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS,
LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm
LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s
LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // %
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg
LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s // 0
LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m // 1
LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m // 2
LOGIC_CONDITION_OPERAND_FLIGHT_RSSI, // 3
LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10 // 4
LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10 // 5
LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100 // 6
LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh // 7
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS, // 8
LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s // 9
LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s // 10
LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s // 11
LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm // 12
LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s // 13
LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // % // 14
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg // 15
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg // 16
LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED, // 0/1 // 17
LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH, // 0/1 // 18
LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL, // 0/1 // 19
LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1 // 20
LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING, // 0/1 // 21
LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1 // 22
LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1 // 23
LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 24
LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 25
} logicFlightOperands_e;
typedef enum {

24
src/main/common/maths.c
View file

@ -25,6 +25,8 @@
#include "quaternion.h"
#include "platform.h"
FILE_COMPILE_FOR_SPEED
// http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
// Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
@ -159,7 +161,7 @@ int constrain(int amt, int low, int high)
return amt;
}
float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
float constrainf(float amt, float low, float high)
{
if (amt < low)
return low;
@ -473,7 +475,19 @@ static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
sensorCalibration_BackwardSubstitution(A, x, y);
}
void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
bool sensorCalibrationValidateResult(const float result[3])
{
// Validate that result is not INF and not NAN
for (int i = 0; i < 3; i++) {
if (isnan(result[i]) && isinf(result[i])) {
return false;
}
}
return true;
}
bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
{
float beta[4];
sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@ -481,9 +495,11 @@ void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float res
for (int i = 0; i < 3; i++) {
result[i] = beta[i] / 2;
}
return sensorCalibrationValidateResult(result);
}
void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
{
float beta[4];
sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@ -491,6 +507,8 @@ void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
for (int i = 0; i < 3; i++) {
result[i] = sqrtf(beta[i]);
}
return sensorCalibrationValidateResult(result);
}
float bellCurve(const float x, const float curveWidth)

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

@ -18,6 +18,7 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#ifndef sq
#define sq(x) ((x)*(x))
@ -122,8 +123,8 @@ typedef struct {
void sensorCalibrationResetState(sensorCalibrationState_t * state);
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target);
void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
int gcd(int num, int denom);
int32_t applyDeadband(int32_t value, int32_t deadband);

4
src/main/common/typeconversion.c
View file

@ -20,6 +20,8 @@
#include "build/build_config.h"
#include "maths.h"
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#ifdef REQUIRE_PRINTF_LONG_SUPPORT
@ -185,7 +187,7 @@ char *ftoa(float x, char *floatString)
dpLocation = strlen(intString2) - 3;
strncpy(floatString, intString2, dpLocation);
memcpy(floatString, intString2, dpLocation);
floatString[dpLocation] = '\0';
strcat(floatString, decimalPoint);
strcat(floatString, intString2 + dpLocation);

2
src/main/common/utils.h
View file

@ -109,4 +109,6 @@ void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("me
#define FALLTHROUGH do {} while(0)
#endif
#define UNREACHABLE() __builtin_unreachable()
#define ALIGNED(x) __attribute__ ((aligned(x)))

2
src/main/config/feature.c
View file

@ -34,7 +34,7 @@ bool featureConfigured(uint32_t mask)
return featureConfig()->enabledFeatures & mask;
}
bool FAST_CODE NOINLINE feature(uint32_t mask)
bool feature(uint32_t mask)
{
return activeFeaturesLatch & mask;
}

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

@ -42,7 +42,7 @@
//#define PG_PROFILE_SELECTION 23
#define PG_RX_CONFIG 24
#define PG_RC_CONTROLS_CONFIG 25
#define PG_MOTOR_3D_CONFIG 26
#define PG_REVERSIBLE_MOTORS_CONFIG 26
#define PG_LED_STRIP_CONFIG 27
//#define PG_COLOR_CONFIG 28
//#define PG_AIRPLANE_ALT_HOLD_CONFIG 29

2
src/main/drivers/barometer/barometer_bmp280.c
View file

@ -166,7 +166,7 @@ static bool deviceDetect(busDevice_t * busDev)
bool ack = busRead(busDev, BMP280_CHIP_ID_REG, &chipId);
if (ack && chipId == BMP280_DEFAULT_CHIP_ID) {
if ((ack && chipId == BMP280_DEFAULT_CHIP_ID) || (ack && chipId == BME280_DEFAULT_CHIP_ID)){
return true;
}
};

1
src/main/drivers/barometer/barometer_bmp280.h
View file

@ -19,6 +19,7 @@
#define BMP280_I2C_ADDR (0x76)
#define BMP280_DEFAULT_CHIP_ID (0x58)
#define BME280_DEFAULT_CHIP_ID (0x60)
#define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */
#define BMP280_RST_REG (0xE0) /* Softreset Register */

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

@ -317,9 +317,10 @@ void i2cInit(I2CDevice device)
/* Enable the Analog I2C Filter */
HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIO_I2C_ER, 0);
HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIO_I2C_EV, 0);
HAL_NVIC_EnableIRQ(i2cHardwareMap[device].ev_irq);
state->initialised = true;
}

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

@ -98,18 +98,13 @@ void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
{
NVIC_InitTypeDef NVIC_InitStructure;
dmaEnableClock(dma);
dma->irqHandlerCallback = callback;
dma->userParam = userParam;
NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(dma->irqNumber, priority);
NVIC_EnableIRQ(dma->irqNumber);
}
DMA_t dmaGetByRef(const DMA_Channel_TypeDef * ref)

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

@ -106,18 +106,13 @@ void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
{
NVIC_InitTypeDef NVIC_InitStructure;
dmaEnableClock(dma);
dma->irqHandlerCallback = callback;
dma->userParam = userParam;
NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(dma->irqNumber, priority);
NVIC_EnableIRQ(dma->irqNumber);
}
uint32_t dmaGetChannelByTag(dmaTag_t tag)

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

@ -110,7 +110,7 @@ void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t prior
dma->irqHandlerCallback = callback;
dma->userParam = userParam;
HAL_NVIC_SetPriority(dma->irqNumber, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
HAL_NVIC_SetPriority(dma->irqNumber, priority, 0);
HAL_NVIC_EnableIRQ(dma->irqNumber);
}

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

@ -90,7 +90,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf
if (extiGroupPriority[group] > irqPriority) {
extiGroupPriority[group] = irqPriority;
HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
HAL_NVIC_SetPriority(extiGroupIRQn[group], irqPriority, 0);
HAL_NVIC_EnableIRQ(extiGroupIRQn[group]);
}
}
@ -131,12 +131,8 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
if (extiGroupPriority[group] > irqPriority) {
extiGroupPriority[group] = irqPriority;
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = extiGroupIRQn[group];
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(extiGroupIRQn[group], irqPriority);
NVIC_EnableIRQ(extiGroupIRQn[group]);
}
}
#endif

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

@ -22,6 +22,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#ifdef USE_MAX7456
#include "common/bitarray.h"

54
src/main/drivers/nvic.h
View file

@ -2,43 +2,25 @@
#pragma once
// can't use 0
#define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1)
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increate slightly
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_GYRO_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
#define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART2 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART3 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART4 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART5 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART6 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART7 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART8 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_I2C_ER NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_I2C_EV NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0)
#define NVIC_PRIO_USB_WUP NVIC_BUILD_PRIORITY(1, 0)
#define NVIC_PRIO_SONAR_ECHO NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0)
// NVIC_SetPriority expects priority encoded according to priority grouping
// We allocate zero bits for sub-priority, therefore we have 16 priorities to use on STM32
// can't use 0
#define NVIC_PRIO_MAX 1
#define NVIC_PRIO_I2C_ER 2
#define NVIC_PRIO_I2C_EV 2
#define NVIC_PRIO_TIMER 3
#define NVIC_PRIO_TIMER_DMA 3
#define NVIC_PRIO_SDIO 3
#define NVIC_PRIO_GYRO_INT_EXTI 4
#define NVIC_PRIO_USB 5
#define NVIC_PRIO_SERIALUART 5
#define NVIC_PRIO_SONAR_EXTI 7
// Use all available bits for priority and zero bits to sub-priority
#ifdef USE_HAL_DRIVER
// utility macros to join/split priority
#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_2
#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING)))))<<4)&0xf0)
#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING))))>>4)
#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING))))>>4)
#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_4
#else
// utility macros to join/split priority
#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0)
#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_4
#endif

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

@ -117,7 +117,7 @@
#define SYM_3D_MPH 0x8A // 138 MPH 3D
#define SYM_RPM 0x8B // 139 RPM
// 0x8C // 140 -
#define SYM_WAYPOINT 0x8C // 140 Waypoint
// 0x8D // 141 -
// 0x8E // 142 -
// 0x8F // 143 -

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

@ -22,6 +22,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "build/debug.h"
#include "common/log.h"

8
src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
View file

@ -1411,12 +1411,8 @@ bool SD_Initialize_LL(DMA_Stream_TypeDef *dmaRef)
IOConfigGPIOAF(cmd, SDIO_CMD, GPIO_AF_SDIO);
// NVIC configuration for SDIO interrupts
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = SDIO_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(1);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(0);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(SDIO_IRQn, NVIC_PRIO_SDIO);
NVIC_EnableIRQ(SDIO_IRQn);
dma_stream = dmaRef;
RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;

4
src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
View file

@ -1329,10 +1329,8 @@ bool SD_Initialize_LL(DMA_Stream_TypeDef * dmaRef)
IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0);
IOConfigGPIOAF(cmd, SDMMC_CMD, GPIO_AF12_SDMMC1);
uint32_t PriorityGroup = NVIC_GetPriorityGrouping();
// NVIC configuration for SDIO interrupts
NVIC_SetPriority(SDMMC1_IRQn, NVIC_EncodePriority(PriorityGroup, 1, 0));
NVIC_SetPriority(SDMMC1_IRQn, NVIC_PRIO_SDIO);
NVIC_EnableIRQ(SDMMC1_IRQn);
dma_stream = dmaRef;

40
src/main/drivers/serial_uart_stm32f30x.c
View file

@ -169,7 +169,6 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui
#ifdef USE_UART1
uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t options)
{
NVIC_InitTypeDef NVIC_InitStructure;
uartPort_t *s;
static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
@ -191,11 +190,8 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1);
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(USART1_IRQn, NVIC_PRIO_SERIALUART);
NVIC_EnableIRQ(USART1_IRQn);
return s;
}
@ -204,7 +200,6 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
#ifdef USE_UART2
uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t options)
{
NVIC_InitTypeDef NVIC_InitStructure;
uartPort_t *s;
static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
@ -226,11 +221,8 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
serialUARTInit(IOGetByTag(IO_TAG(UART2_TX_PIN)), IOGetByTag(IO_TAG(UART2_RX_PIN)), mode, options, GPIO_AF_7, 2);
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART2);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART2);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(USART2_IRQn, NVIC_PRIO_SERIALUART);
NVIC_EnableIRQ(USART2_IRQn);
return s;
}
@ -239,7 +231,6 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
#ifdef USE_UART3
uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t options)
{
NVIC_InitTypeDef NVIC_InitStructure;
uartPort_t *s;
static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
@ -261,11 +252,8 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option
serialUARTInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOGetByTag(IO_TAG(UART3_RX_PIN)), mode, options, GPIO_AF_7, 3);
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART3);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART3);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(USART3_IRQn, NVIC_PRIO_SERIALUART);
NVIC_EnableIRQ(USART3_IRQn);
return s;
}
@ -277,7 +265,6 @@ uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t option
uartPort_t *s;
static volatile uint8_t rx4Buffer[UART4_RX_BUFFER_SIZE];
static volatile uint8_t tx4Buffer[UART4_TX_BUFFER_SIZE];
NVIC_InitTypeDef NVIC_InitStructure;
s = &uartPort4;
s->port.vTable = uartVTable;
@ -295,11 +282,8 @@ uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t option
serialUARTInit(IOGetByTag(IO_TAG(UART4_TX_PIN)), IOGetByTag(IO_TAG(UART4_RX_PIN)), mode, options, GPIO_AF_5, 4);
NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART4);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART4);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(UART4_IRQn, NVIC_PRIO_SERIALUART);
NVIC_EnableIRQ(UART4_IRQn);
return s;
}
@ -311,7 +295,6 @@ uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t option
uartPort_t *s;
static volatile uint8_t rx5Buffer[UART5_RX_BUFFER_SIZE];
static volatile uint8_t tx5Buffer[UART5_TX_BUFFER_SIZE];
NVIC_InitTypeDef NVIC_InitStructure;
s = &uartPort5;
s->port.vTable = uartVTable;
@ -329,11 +312,8 @@ uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t option
serialUARTInit(IOGetByTag(IO_TAG(UART5_TX_PIN)), IOGetByTag(IO_TAG(UART5_RX_PIN)), mode, options, GPIO_AF_5, 5);
NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART5);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART5);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(UART5_IRQn, NVIC_PRIO_SERIALUART);
NVIC_EnableIRQ(UART5_IRQn);
return s;
}

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

@ -60,7 +60,7 @@ static uartDevice_t uart1 =
#endif
.rcc_apb2 = RCC_APB2(USART1),
.irq = USART1_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART1
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -76,7 +76,7 @@ static uartDevice_t uart2 =
#endif
.rcc_apb1 = RCC_APB1(USART2),
.irq = USART2_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART2
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -92,7 +92,7 @@ static uartDevice_t uart3 =
#endif
.rcc_apb1 = RCC_APB1(USART3),
.irq = USART3_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART3
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -108,7 +108,7 @@ static uartDevice_t uart4 =
#endif
.rcc_apb1 = RCC_APB1(UART4),
.irq = UART4_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART4
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -124,7 +124,7 @@ static uartDevice_t uart5 =
#endif
.rcc_apb1 = RCC_APB1(UART5),
.irq = UART5_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART5
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -140,7 +140,7 @@ static uartDevice_t uart6 =
#endif
.rcc_apb2 = RCC_APB2(USART6),
.irq = USART6_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART6
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -153,7 +153,7 @@ static uartDevice_t uart7 =
.af = GPIO_AF_UART7,
.rcc_apb1 = RCC_APB1(UART7),
.irq = UART7_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART7
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -166,7 +166,7 @@ static uartDevice_t uart8 =
.af = GPIO_AF_UART8,
.rcc_apb1 = RCC_APB1(UART8),
.irq = UART8_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART8
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -256,7 +256,6 @@ void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins)
uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options)
{
uartPort_t *s;
NVIC_InitTypeDef NVIC_InitStructure;
uartDevice_t *uart = uartHardwareMap[device];
if (!uart) return NULL;
@ -304,11 +303,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
}
}
NVIC_InitStructure.NVIC_IRQChannel = uart->irq;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(uart->irqPriority);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(uart->irqPriority);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(uart->irq, uart->irqPriority);
NVIC_EnableIRQ(uart->irq);
return s;
}

18
src/main/drivers/serial_uart_stm32f7xx.c
View file

@ -64,7 +64,7 @@ static uartDevice_t uart1 =
#endif
.rcc_apb2 = RCC_APB2(USART1),
.irq = USART1_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART1
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -84,7 +84,7 @@ static uartDevice_t uart2 =
#endif
.rcc_apb1 = RCC_APB1(USART2),
.irq = USART2_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART2
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -104,7 +104,7 @@ static uartDevice_t uart3 =
#endif
.rcc_apb1 = RCC_APB1(USART3),
.irq = USART3_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART3
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -124,7 +124,7 @@ static uartDevice_t uart4 =
#endif
.rcc_apb1 = RCC_APB1(UART4),
.irq = UART4_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART4
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -144,7 +144,7 @@ static uartDevice_t uart5 =
#endif
.rcc_apb1 = RCC_APB1(UART5),
.irq = UART5_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART5
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -164,7 +164,7 @@ static uartDevice_t uart6 =
#endif
.rcc_apb2 = RCC_APB2(USART6),
.irq = USART6_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART6
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -184,7 +184,7 @@ static uartDevice_t uart7 =
#endif
.rcc_apb1 = RCC_APB1(UART7),
.irq = UART7_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART7
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
#ifdef USE_UART8
@ -203,7 +203,7 @@ static uartDevice_t uart8 =
#endif
.rcc_apb1 = RCC_APB1(UART8),
.irq = UART8_IRQn,
.irqPriority = NVIC_PRIO_SERIALUART8
.irqPriority = NVIC_PRIO_SERIALUART
};
#endif
@ -372,7 +372,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
}
}
HAL_NVIC_SetPriority(uart->irq, NVIC_PRIORITY_BASE(uart->irqPriority), NVIC_PRIORITY_SUB(uart->irqPriority));
HAL_NVIC_SetPriority(uart->irq, uart->irqPriority, 0);
HAL_NVIC_EnableIRQ(uart->irq);
return s;

12
src/main/drivers/system.c
View file

@ -48,8 +48,8 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
}
// cycles per microsecond
STATIC_UNIT_TESTED timeUs_t usTicks = 0;
// cycles per microsecond, this is deliberately uint32_t to avoid type conversions
STATIC_UNIT_TESTED uint32_t usTicks = 0;
// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
@ -132,7 +132,9 @@ timeUs_t microsISR(void)
pending = sysTickPending;
}
return ((timeUs_t)(ms + pending) * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks;
// XXX: Be careful to not trigger 64 bit division
const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
return ((timeUs_t)(ms + pending) * 1000LL) + ((timeUs_t)partial);
}
timeUs_t micros(void)
@ -152,7 +154,9 @@ timeUs_t micros(void)
cycle_cnt = SysTick->VAL;
} while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
return ((timeUs_t)ms * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks;
// XXX: Be careful to not trigger 64 bit division
const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
return ((timeUs_t)ms * 1000LL) + ((timeUs_t)partial);
}
// Return system uptime in milliseconds (rollover in 49 days)

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

@ -62,12 +62,12 @@ void impl_timerInitContext(timHardwareContext_t * timCtx)
void impl_timerNVICConfigure(TCH_t * tch, int irqPriority)
{
if (tch->timCtx->timDef->irq) {
HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, irqPriority, 0);
HAL_NVIC_EnableIRQ(tch->timCtx->timDef->irq);
}
if (tch->timCtx->timDef->secondIrq) {
HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, irqPriority, 0);
HAL_NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq);
}
}
@ -380,7 +380,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
init.PeriphBurst = LL_DMA_PBURST_SINGLE;
dmaInit(tch->dma, OWNER_TIMER, 0);
dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch);
dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
LL_DMA_Init(tch->dma->dma, streamLL, &init);

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

@ -44,20 +44,14 @@ void impl_timerInitContext(timHardwareContext_t * timCtx)
void impl_timerNVICConfigure(TCH_t * tch, int irqPriority)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
if (tch->timCtx->timDef->irq) {
NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->irq;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(tch->timCtx->timDef->irq, irqPriority);
NVIC_EnableIRQ(tch->timCtx->timDef->irq);
}
if (tch->timCtx->timDef->secondIrq) {
NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->secondIrq;
NVIC_Init(&NVIC_InitStructure);
NVIC_SetPriority(tch->timCtx->timDef->secondIrq, irqPriority);
NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq);
}
}
@ -311,7 +305,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
TIM_Cmd(timer, ENABLE);
dmaInit(tch->dma, OWNER_TIMER, 0);
dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch);
dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
DMA_DeInit(tch->dma->ref);
DMA_Cmd(tch->dma->ref, DISABLE);

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

@ -112,7 +112,7 @@ uint8_t mscStart(void)
// NVIC configuration for SYSTick
NVIC_DisableIRQ(SysTick_IRQn);
NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
NVIC_SetPriority(SysTick_IRQn, 0);
NVIC_EnableIRQ(SysTick_IRQn);
return 0;

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

@ -110,7 +110,7 @@ uint8_t mscStart(void)
// NVIC configuration for SYSTick
NVIC_DisableIRQ(SysTick_IRQn);
NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
NVIC_SetPriority(SysTick_IRQn, 0);
NVIC_EnableIRQ(SysTick_IRQn);
return 0;

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

@ -103,13 +103,6 @@ void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
}
}
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
{
if (vtxDevice && vtxDevice->vTable->setFrequency) {
vtxDevice->vTable->setFrequency(vtxDevice, frequency);
}
}
bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
if (vtxDevice && vtxDevice->vTable->getBandAndChannel) {
@ -150,3 +143,35 @@ bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t
}
return false;
}
bool vtxCommonGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
if (vtxDevice && vtxDevice->vTable->getPower) {
return vtxDevice->vTable->getPower(vtxDevice, pIndex, pPowerMw);
}
return false;
}
bool vtxCommonGetOsdInfo(vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
bool ret = false;
if (vtxDevice && vtxDevice->vTable->getOsdInfo) {
ret = vtxDevice->vTable->getOsdInfo(vtxDevice, pOsdInfo);
}
// Make sure we provide sane results even in case API fails
if (!ret) {
pOsdInfo->band = 0;
pOsdInfo->channel = 0;
pOsdInfo->frequency = 0;
pOsdInfo->powerIndex = 0;
pOsdInfo->powerMilliwatt = 0;
pOsdInfo->bandLetter = '-';
pOsdInfo->bandName = "-";
pOsdInfo->channelName = "-";
pOsdInfo->powerIndexLetter = '0';
}
return ret;
}

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

@ -30,19 +30,12 @@
#define VTX_SETTINGS_DEFAULT_BAND 4
#define VTX_SETTINGS_DEFAULT_CHANNEL 1
#define VTX_SETTINGS_DEFAULT_FREQ 5740
#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0
#define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL 1
#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0
#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting
#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting
#if defined(USE_VTX_RTC6705)
#include "drivers/vtx_rtc6705.h"
#endif
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
#define VTX_SETTINGS_POWER_COUNT 5
@ -51,14 +44,7 @@
#define VTX_SETTINGS_MIN_USER_FREQ 5000
#define VTX_SETTINGS_MAX_USER_FREQ 5999
#define VTX_SETTINGS_FREQCMD
#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1)
#elif defined(USE_VTX_RTC6705)
#define VTX_SETTINGS_POWER_COUNT VTX_RTC6705_POWER_COUNT
#define VTX_SETTINGS_DEFAULT_POWER VTX_RTC6705_DEFAULT_POWER
#define VTX_SETTINGS_MIN_POWER VTX_RTC6705_MIN_POWER
#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1)
#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1)
#endif
@ -68,7 +54,7 @@
typedef enum {
VTXDEV_UNSUPPORTED = 0, // reserved for MSP
VTXDEV_RTC6705 = 1,
VTXDEV_RTC6705 = 1, // deprecated
// 2 reserved
VTXDEV_SMARTAUDIO = 3,
VTXDEV_TRAMP = 4,
@ -82,23 +68,32 @@ typedef struct vtxDeviceCapability_s {
uint8_t bandCount;
uint8_t channelCount;
uint8_t powerCount;
char **bandNames;
char **channelNames;
char **powerNames;
} vtxDeviceCapability_t;
typedef struct vtxDeviceOsdInfo_s {
int band;
int channel;
int frequency;
int powerIndex;
int powerMilliwatt;
char bandLetter;
const char * bandName;
const char * channelName;
char powerIndexLetter;
} vtxDeviceOsdInfo_t;
typedef struct vtxDevice_s {
const struct vtxVTable_s * const vTable;
vtxDeviceCapability_t capability;
uint16_t *frequencyTable; // Array of [bandCount][channelCount]
char **bandNames; // char *bandNames[bandCount]
char **channelNames; // char *channelNames[channelCount]
char **powerNames; // char *powerNames[powerCount]
uint8_t band; // Band = 1, 1-based
uint8_t channel; // CH1 = 1, 1-based
uint8_t powerIndex; // Lowest/Off = 0
uint8_t pitMode; // 0 = non-PIT, 1 = PIT
} vtxDevice_t;
// {set,get}BandAndChannel: band and channel are 1 origin
@ -113,12 +108,14 @@ typedef struct vtxVTable_s {
void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level);
void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff);
void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq);
bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool (*getPower)(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw);
bool (*getOsdInfo)(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo);
} vtxVTable_t;
// 3.1.0
@ -137,9 +134,10 @@ bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice);
void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index);
void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff);
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency);
bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability);
bool vtxCommonGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw);
bool vtxCommonGetOsdInfo(vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo);

264
src/main/drivers/vtx_rtc6705.c
View file

@ -1,264 +0,0 @@
/*
* 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/>.
*/
/*
* Author: Giles Burgess (giles@multiflite.co.uk)
*
* This source code is provided as is and can be used/modified so long
* as this header is maintained with the file at all times.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI)
#include "common/maths.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/vtx_rtc6705.h"
#define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400
#define RTC6705_SET_A1 0x8F3031 //5865
#define RTC6705_SET_A2 0x8EB1B1 //5845
#define RTC6705_SET_A3 0x8E3331 //5825
#define RTC6705_SET_A4 0x8DB4B1 //5805
#define RTC6705_SET_A5 0x8D3631 //5785
#define RTC6705_SET_A6 0x8CB7B1 //5765
#define RTC6705_SET_A7 0x8C4131 //5745
#define RTC6705_SET_A8 0x8BC2B1 //5725
#define RTC6705_SET_B1 0x8BF3B1 //5733
#define RTC6705_SET_B2 0x8C6711 //5752
#define RTC6705_SET_B3 0x8CE271 //5771
#define RTC6705_SET_B4 0x8D55D1 //5790
#define RTC6705_SET_B5 0x8DD131 //5809
#define RTC6705_SET_B6 0x8E4491 //5828
#define RTC6705_SET_B7 0x8EB7F1 //5847
#define RTC6705_SET_B8 0x8F3351 //5866
#define RTC6705_SET_E1 0x8B4431 //5705
#define RTC6705_SET_E2 0x8AC5B1 //5685
#define RTC6705_SET_E3 0x8A4731 //5665
#define RTC6705_SET_E4 0x89D0B1 //5645
#define RTC6705_SET_E5 0x8FA6B1 //5885
#define RTC6705_SET_E6 0x902531 //5905
#define RTC6705_SET_E7 0x90A3B1 //5925
#define RTC6705_SET_E8 0x912231 //5945
#define RTC6705_SET_F1 0x8C2191 //5740
#define RTC6705_SET_F2 0x8CA011 //5760
#define RTC6705_SET_F3 0x8D1691 //5780
#define RTC6705_SET_F4 0x8D9511 //5800
#define RTC6705_SET_F5 0x8E1391 //5820
#define RTC6705_SET_F6 0x8E9211 //5840
#define RTC6705_SET_F7 0x8F1091 //5860
#define RTC6705_SET_F8 0x8F8711 //5880
#define RTC6705_SET_R1 0x8A2151 //5658
#define RTC6705_SET_R2 0x8B04F1 //5695
#define RTC6705_SET_R3 0x8BF091 //5732
#define RTC6705_SET_R4 0x8CD431 //5769
#define RTC6705_SET_R5 0x8DB7D1 //5806
#define RTC6705_SET_R6 0x8EA371 //5843
#define RTC6705_SET_R7 0x8F8711 //5880
#define RTC6705_SET_R8 0x9072B1 //5917
#define RTC6705_SET_R 400 //Reference clock
#define RTC6705_SET_FDIV 1024 //128*(fosc/1000000)
#define RTC6705_SET_NDIV 16 //Remainder divider to get 'A' part of equation
#define RTC6705_SET_WRITE 0x11 //10001b to write to register
#define RTC6705_SET_DIVMULT 1000000 //Division value (to fit into a uint32_t) (Hz to MHz)
#ifdef RTC6705_POWER_PIN
static IO_t vtxPowerPin = IO_NONE;
#endif
static IO_t vtxCSPin = IO_NONE;
#define DISABLE_RTC6705() IOHi(vtxCSPin)
#ifdef USE_RTC6705_CLK_HACK
static IO_t vtxCLKPin = IO_NONE;
// HACK for missing pull up on CLK line - drive the CLK high *before* enabling the CS pin.
#define ENABLE_RTC6705() {IOHi(vtxCLKPin); delayMicroseconds(5); IOLo(vtxCSPin); }
#else
#define ENABLE_RTC6705() IOLo(vtxCSPin)
#endif
#define DP_5G_MASK 0x7000 // b111000000000000
#define PA5G_BS_MASK 0x0E00 // b000111000000000
#define PA5G_PW_MASK 0x0180 // b000000110000000
#define PD_Q5G_MASK 0x0040 // b000000001000000
#define QI_5G_MASK 0x0038 // b000000000111000
#define PA_BS_MASK 0x0007 // b000000000000111
#define PA_CONTROL_DEFAULT 0x4FBD
#define RTC6705_RW_CONTROL_BIT (1 << 4)
#define RTC6705_ADDRESS (0x07)
#define ENABLE_VTX_POWER() IOLo(vtxPowerPin)
#define DISABLE_VTX_POWER() IOHi(vtxPowerPin)
// Define variables
static const uint32_t channelArray[VTX_RTC6705_BAND_COUNT][VTX_RTC6705_CHANNEL_COUNT] = {
{ RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
{ RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 },
{ RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 },
{ RTC6705_SET_F1, RTC6705_SET_F2, RTC6705_SET_F3, RTC6705_SET_F4, RTC6705_SET_F5, RTC6705_SET_F6, RTC6705_SET_F7, RTC6705_SET_F8 },
{ RTC6705_SET_R1, RTC6705_SET_R2, RTC6705_SET_R3, RTC6705_SET_R4, RTC6705_SET_R5, RTC6705_SET_R6, RTC6705_SET_R7, RTC6705_SET_R8 },
};
/**
* Reverse a uint32_t (LSB to MSB)
* This is easier for when generating the frequency to then
* reverse the bits afterwards
*/
static uint32_t reverse32(uint32_t in)
{
uint32_t out = 0;
for (uint8_t i = 0 ; i < 32 ; i++)
{
out |= ((in>>i) & 1)<<(31-i);
}
return out;
}
/**
* Start chip if available
*/
void rtc6705IOInit(void)
{
#ifdef RTC6705_POWER_PIN
vtxPowerPin = IOGetByTag(IO_TAG(RTC6705_POWER_PIN));
IOInit(vtxPowerPin, OWNER_VTX, RESOURCE_OUTPUT, 0);
DISABLE_VTX_POWER();
IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
#endif
#ifdef USE_RTC6705_CLK_HACK
vtxCLKPin = IOGetByTag(IO_TAG(RTC6705_CLK_PIN));
// we assume the CLK pin will have been initialised by the SPI code.
#endif
vtxCSPin = IOGetByTag(IO_TAG(RTC6705_CS_PIN));
IOInit(vtxCSPin, OWNER_VTX, RESOURCE_OUTPUT, 0);
DISABLE_RTC6705();
// GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode.
// Note: It's critical to ensure that incorrect signals are not sent to the VTX.
IOConfigGPIO(vtxCSPin, IOCFG_OUT_PP);
}
/**
* Transfer a 25bit packet to RTC6705
* This will just send it as a 32bit packet LSB meaning
* extra 0's get truncated on RTC6705 end
*/
static void rtc6705Transfer(uint32_t command)
{
command = reverse32(command);
ENABLE_RTC6705();
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 24) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 16) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 8) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 0) & 0xFF);
delayMicroseconds(2);
DISABLE_RTC6705();
delayMicroseconds(2);
}
/**
* Set a band and channel
*/
void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
{
band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1);
channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1);
spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
rtc6705Transfer(RTC6705_SET_HEAD);
rtc6705Transfer(channelArray[band][channel]);
}
/**
* Set a freq in mhz
* Formula derived from datasheet
*/
void rtc6705SetFreq(uint16_t frequency)
{
frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);
uint32_t val_hex = 0;
uint32_t val_a = ((((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
uint32_t val_n = (((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)
val_hex |= RTC6705_SET_WRITE;
val_hex |= (val_a << 5);
val_hex |= (val_n << 12);
spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
rtc6705Transfer(RTC6705_SET_HEAD);
delayMicroseconds(10);
rtc6705Transfer(val_hex);
}
void rtc6705SetRFPower(uint8_t rf_power)
{
rf_power = constrain(rf_power, 0, VTX_RTC6705_POWER_COUNT - 1);
spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write
val_hex |= RTC6705_ADDRESS; // address
uint32_t data = rf_power == 0 ? (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)) : PA_CONTROL_DEFAULT;
val_hex |= data << 5; // 4 address bits and 1 rw bit.
rtc6705Transfer(val_hex);
}
void rtc6705Disable(void)
{
#ifdef RTC6705_POWER_PIN
DISABLE_VTX_POWER();
#endif
}
void rtc6705Enable(void)
{
#ifdef RTC6705_POWER_PIN
ENABLE_VTX_POWER();
#endif
}
#endif

50
src/main/drivers/vtx_rtc6705.h
View file

@ -1,50 +0,0 @@
/*
* 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/>.
*/
/*
* Author: Giles Burgess (giles@multiflite.co.uk)
*
* This source code is provided as is and can be used/modified so long
* as this header is maintained with the file at all times.
*/
#pragma once
#include <stdint.h>
#define VTX_RTC6705_BAND_COUNT 5
#define VTX_RTC6705_CHANNEL_COUNT 8
#define VTX_RTC6705_POWER_COUNT 3
#define VTX_RTC6705_DEFAULT_POWER 1
#if defined(RTC6705_POWER_PIN)
#define VTX_RTC6705_MIN_POWER 0
#else
#define VTX_RTC6705_MIN_POWER 1
#endif
#define VTX_RTC6705_FREQ_MIN 5600
#define VTX_RTC6705_FREQ_MAX 5950
#define VTX_RTC6705_BOOT_DELAY 350 // milliseconds
void rtc6705IOInit(void);
void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel);
void rtc6705SetFreq(const uint16_t freq);
void rtc6705SetRFPower(const uint8_t rf_power);
void rtc6705Disable(void);
void rtc6705Enable(void);

156
src/main/drivers/vtx_rtc6705_soft_spi.c
View file

@ -1,156 +0,0 @@
/*
* 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 "platform.h"
#if defined(USE_VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI)
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "light_led.h"
#include "vtx_rtc6705.h"
#define DP_5G_MASK 0x7000
#define PA5G_BS_MASK 0x0E00
#define PA5G_PW_MASK 0x0180
#define PD_Q5G_MASK 0x0040
#define QI_5G_MASK 0x0038
#define PA_BS_MASK 0x0007
#define PA_CONTROL_DEFAULT 0x4FBD
#define RTC6705_SPICLK_ON IOHi(rtc6705ClkPin)
#define RTC6705_SPICLK_OFF IOLo(rtc6705ClkPin)
#define RTC6705_SPIDATA_ON IOHi(rtc6705DataPin)
#define RTC6705_SPIDATA_OFF IOLo(rtc6705DataPin)
#define RTC6705_SPILE_ON IOHi(rtc6705LePin)
#define RTC6705_SPILE_OFF IOLo(rtc6705LePin)
const uint16_t vtx_freq[] =
{
5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // Boacam A
5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // Boscam B
5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // Boscam E
5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // FatShark
5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // RaceBand
};
static IO_t rtc6705DataPin = IO_NONE;
static IO_t rtc6705LePin = IO_NONE;
static IO_t rtc6705ClkPin = IO_NONE;
void rtc6705IOInit(void)
{
rtc6705DataPin = IOGetByTag(IO_TAG(RTC6705_SPIDATA_PIN));
rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN));
rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN));
IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP);
IOInit(rtc6705LePin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP);
IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP);
}
static void rtc6705_write_register(uint8_t addr, uint32_t data)
{
uint8_t i;
RTC6705_SPILE_OFF;
delay(1);
// send address
for (i=0; i<4; i++) {
if ((addr >> i) & 1)
RTC6705_SPIDATA_ON;
else
RTC6705_SPIDATA_OFF;
RTC6705_SPICLK_ON;
delay(1);
RTC6705_SPICLK_OFF;
delay(1);
}
// Write bit
RTC6705_SPIDATA_ON;
RTC6705_SPICLK_ON;
delay(1);
RTC6705_SPICLK_OFF;
delay(1);
for (i=0; i<20; i++) {
if ((data >> i) & 1)
RTC6705_SPIDATA_ON;
else
RTC6705_SPIDATA_OFF;
RTC6705_SPICLK_ON;
delay(1);
RTC6705_SPICLK_OFF;
delay(1);
}
RTC6705_SPILE_ON;
}
void rtc6705SetFreq(uint16_t channel_freq)
{
uint32_t freq = (uint32_t)channel_freq * 1000;
uint32_t N, A;
freq /= 40;
N = freq / 64;
A = freq % 64;
rtc6705_write_register(0, 400);
rtc6705_write_register(1, (N << 7) | A);
}
void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel)
{
// band and channel are 1-based, not 0-based
// example for raceband/ch8:
// (5 - 1) * 8 + (8 - 1)
// 4 * 8 + 7
// 32 + 7 = 39
uint8_t freqIndex = ((band - 1) * RTC6705_BAND_COUNT) + (channel - 1);
uint16_t freq = vtx_freq[freqIndex];
rtc6705SetFreq(freq);
}
void rtc6705SetRFPower(const uint8_t rf_power)
{
rtc6705_write_register(7, (rf_power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK))));
}
void rtc6705Disable(void)
{
}
void rtc6705Enable(void)
{
}
#endif

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

@ -68,6 +68,7 @@ extern uint8_t __config_end;
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/usb_msc.h"
#include "drivers/vtx_common.h"
#include "fc/fc_core.h"
#include "fc/cli.h"
@ -146,8 +147,8 @@ static bool commandBatchError = false;
// sync this with features_e
static const char * const featureNames[] = {
"THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
"DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "RPM_FILTERS",
"", "TELEMETRY", "CURRENT_METER", "3D", "",
"", "SOFTSERIAL", "GPS", "RPM_FILTERS",
"", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
"BLACKBOX", "", "TRANSPONDER", "AIRMODE",
"SUPEREXPO", "VTX", "", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE",
@ -1279,7 +1280,7 @@ static void cliTempSensor(char *cmdline)
static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
{
cliPrintLinef("#wp %d %svalid", posControl.waypointCount, posControl.waypointListValid ? "" : "in"); //int8_t bool
const char *format = "wp %u %u %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1; uint8_t flag
const char *format = "wp %u %u %d %d %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1 int16_t p2 int16_t p3; uint8_t flag
for (uint8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
bool equalsDefault = false;
if (defaultNavWaypoint) {
@ -1288,6 +1289,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
&& navWaypoint[i].lon == defaultNavWaypoint[i].lon
&& navWaypoint[i].alt == defaultNavWaypoint[i].alt
&& navWaypoint[i].p1 == defaultNavWaypoint[i].p1
&& navWaypoint[i].p2 == defaultNavWaypoint[i].p2
&& navWaypoint[i].p3 == defaultNavWaypoint[i].p3
&& navWaypoint[i].flag == defaultNavWaypoint[i].flag;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
@ -1296,6 +1299,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
defaultNavWaypoint[i].lon,
defaultNavWaypoint[i].alt,
defaultNavWaypoint[i].p1,
defaultNavWaypoint[i].p2,
defaultNavWaypoint[i].p3,
defaultNavWaypoint[i].flag
);
}
@ -1306,6 +1311,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
navWaypoint[i].lon,
navWaypoint[i].alt,
navWaypoint[i].p1,
navWaypoint[i].p2,
navWaypoint[i].p3,
navWaypoint[i].flag
);
}
@ -1322,7 +1329,7 @@ static void cliWaypoints(char *cmdline)
} else if (sl_strcasecmp(cmdline, "save") == 0) {
posControl.waypointListValid = false;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break;
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND)) break;
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.waypointCount = i + 1;
posControl.waypointListValid = true;
@ -1335,7 +1342,7 @@ static void cliWaypoints(char *cmdline)
cliShowParseError();
}
} else {
int16_t i, p1;
int16_t i, p1,p2=0,p3=0,tmp;
uint8_t action, flag;
int32_t lat, lon, alt;
uint8_t validArgumentCount = 0;
@ -1369,12 +1376,29 @@ static void cliWaypoints(char *cmdline)
}
ptr = nextArg(ptr);
if (ptr) {
flag = fastA2I(ptr);
tmp = fastA2I(ptr);
validArgumentCount++;
}
if (validArgumentCount < 4) {
/* We support pre-2.5 6 values (... p1,flags) or
* 2.5 and later, 8 values (... p1,p2,p3,flags)
*/
ptr = nextArg(ptr);
if (ptr) {
p2 = tmp;
p3 = fastA2I(ptr);
validArgumentCount++;
ptr = nextArg(ptr);
if (ptr) {
flag = fastA2I(ptr);
validArgumentCount++;
}
} else {
flag = tmp;
}
if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
cliShowParseError();
} else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
} else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
cliShowParseError();
} else {
posControl.waypointList[i].action = action;
@ -1382,6 +1406,8 @@ static void cliWaypoints(char *cmdline)
posControl.waypointList[i].lon = lon;
posControl.waypointList[i].alt = alt;
posControl.waypointList[i].p1 = p1;
posControl.waypointList[i].p2 = p2;
posControl.waypointList[i].p3 = p3;
posControl.waypointList[i].flag = flag;
}
} else {
@ -1979,7 +2005,7 @@ static void cliGlobalFunctions(char *cmdline) {
if (
i >= 0 && i < MAX_GLOBAL_FUNCTIONS &&
args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
args[CONDITION_ID] >= 0 && args[CONDITION_ID] < MAX_LOGIC_CONDITIONS &&
args[CONDITION_ID] >= -1 && args[CONDITION_ID] < MAX_LOGIC_CONDITIONS &&
args[ACTION] >= 0 && args[ACTION] < GLOBAL_FUNCTION_ACTION_LAST &&
args[VALUE_TYPE] >= 0 && args[VALUE_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[VALUE_VALUE] >= -1000000 && args[VALUE_VALUE] <= 1000000 &&
@ -3047,6 +3073,29 @@ static void cliStatus(char *cmdline)
cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);
#endif
#if defined(USE_VTX_CONTROL) && !defined(CLI_MINIMAL_VERBOSITY)
cliPrint("VTX: ");
if (vtxCommonDeviceIsReady(vtxCommonDevice())) {
vtxDeviceOsdInfo_t osdInfo;
vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
cliPrintf("band: %c, chan: %s, power: %c", osdInfo.bandLetter, osdInfo.channelName, osdInfo.powerIndexLetter);
if (osdInfo.powerMilliwatt) {
cliPrintf(" (%d mW)", osdInfo.powerMilliwatt);
}
if (osdInfo.frequency) {
cliPrintf(", freq: %d MHz", osdInfo.frequency);
}
}
else {
cliPrint("not detected");
}
cliPrintLinefeed();
#endif
// If we are blocked by PWM init - provide more information
if (getPwmInitError() != PWM_INIT_ERROR_NONE) {
cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());
@ -3218,13 +3267,13 @@ static void printConfig(const char *cmdline, bool doDiff)
//printResource(dumpMask, &defaultConfig);
cliPrintHashLine("mixer");
cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n");
cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n");
printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0));
// print custom servo mixer if exists
cliPrintHashLine("servo mix");
cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n");
cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n");
printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0));
// print servo parameters

20
src/main/fc/config.c
View file

@ -163,7 +163,7 @@ __attribute__((weak)) void targetConfiguration(void)
uint32_t getLooptime(void) {
return gyro.targetLooptime;
}
}
void validateAndFixConfig(void)
{
@ -181,13 +181,7 @@ void validateAndFixConfig(void)
}
// Disable unused features
featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);
#if defined(DISABLE_RX_PWM_FEATURE) || !defined(USE_RX_PWM)
if (rxConfig()->receiverType == RX_TYPE_PWM) {
rxConfigMutable()->receiverType = RX_TYPE_NONE;
}
#endif
featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);
#if !defined(USE_RX_PPM)
if (rxConfig()->receiverType == RX_TYPE_PPM) {
@ -196,16 +190,6 @@ void validateAndFixConfig(void)
#endif
if (rxConfig()->receiverType == RX_TYPE_PWM) {
#if defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
// led strip needs the same ports
featureClear(FEATURE_LED_STRIP);
#endif
// software serial needs free PWM ports
featureClear(FEATURE_SOFTSERIAL);
}
#if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) {
const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);

4
src/main/fc/config.h
View file

@ -41,14 +41,14 @@ typedef enum {
FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command
FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_DYNAMIC_FILTERS = 1 << 5, // was FEATURE_SERVO_TILT
FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS
FEATURE_SOFTSERIAL = 1 << 6,
FEATURE_GPS = 1 << 7,
FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE
FEATURE_UNUSED_4 = 1 << 9, // was FEATURE_SONAR
FEATURE_TELEMETRY = 1 << 10,
FEATURE_CURRENT_METER = 1 << 11,
FEATURE_3D = 1 << 12,
FEATURE_REVERSIBLE_MOTORS = 1 << 12,
FEATURE_UNUSED_5 = 1 << 13, // RX_PARALLEL_PWM
FEATURE_UNUSED_6 = 1 << 14, // RX_MSP
FEATURE_RSSI_ADC = 1 << 15,

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

@ -21,6 +21,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "blackbox/blackbox.h"
#include "build/debug.h"
@ -200,7 +202,7 @@ static void updateArmingStatus(void)
/* CHECK: Throttle */
if (!armingConfig()->fixed_wing_auto_arm) {
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
if (calculateThrottleStatus() != THROTTLE_LOW) {
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
@ -475,6 +477,8 @@ void tryArm(void)
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
//It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
logicConditionReset();
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
@ -515,7 +519,7 @@ void processRx(timeUs_t currentTimeUs)
calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
disarm(DISARM_SWITCH_3D);
}
@ -529,10 +533,10 @@ void processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
// When armed and motors aren't spinning, do beeps periodically
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)) {
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
static bool armedBeeperOn = false;
if (throttleStatus == THROTTLE_LOW) {
@ -633,7 +637,7 @@ void processRx(timeUs_t currentTimeUs)
#endif
// Handle passthrough mode
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough
(!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating
ENABLE_FLIGHT_MODE(MANUAL_MODE);
@ -649,13 +653,13 @@ void processRx(timeUs_t currentTimeUs)
/* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */
pidResetErrorAccumulators();
}
else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
else if (STATE(FIXED_WING_LEGACY) || 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 ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
ENABLE_STATE(ANTI_WINDUP);
}
else {
@ -753,7 +757,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
flightTime += cycleTime;
updateAccExtremes();
}
@ -782,7 +786,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
#endif
// Apply throttle tilt compensation
if (!STATE(FIXED_WING)) {
if (!STATE(FIXED_WING_LEGACY)) {
int16_t thrTiltCompStrength = 0;
if (navigationRequiresThrottleTiltCompensation()) {

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

@ -74,7 +74,6 @@
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/io_pca9685.h"
#include "drivers/vtx_rtc6705.h"
#include "drivers/vtx_common.h"
#ifdef USE_USB_MSC
#include "drivers/usb_msc.h"
@ -253,10 +252,10 @@ void init(void)
#if defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(feature(FEATURE_SOFTSERIAL),
(rxConfig()->receiverType == RX_TYPE_PWM) || (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
(rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(feature(FEATURE_SOFTSERIAL),
(rxConfig()->receiverType == RX_TYPE_PWM) || (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
(rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
@ -285,7 +284,10 @@ void init(void)
// Some sanity checking
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
featureClear(FEATURE_REVERSIBLE_MOTORS);
}
if (!STATE(ALTITUDE_CONTROL)) {
featureClear(FEATURE_AIRMODE);
}
// Initialize motor and servo outpus
@ -296,37 +298,6 @@ void init(void)
ENABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
}
/*
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
pwm_params.flyingPlatformType = mixerConfig()->platformType;
pwm_params.useParallelPWM = (rxConfig()->receiverType == RX_TYPE_PWM);
pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM);
pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL);
pwm_params.useServoOutputs = isMixerUsingServos();
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servoPwmRate;
pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE);
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
pwmRxInit(systemConfig()->pwmRxInputFilteringMode);
#endif
#ifdef USE_PWM_SERVO_DRIVER
// If external PWM driver is enabled, for example PCA9685, disable internal
// servo handling mechanism, since external device will do that
if (feature(FEATURE_PWM_SERVO_DRIVER)) {
pwm_params.useServoOutputs = false;
}
#endif
*/
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
@ -538,7 +509,7 @@ void init(void)
rxInit();
#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
#if defined(USE_OSD)
displayPort_t *osdDisplayPort = NULL;
#endif
@ -565,13 +536,6 @@ void init(void)
}
#endif
#if defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)
// If OSD is not active, then register MSP_DISPLAYPORT as a CMS device.
if (!osdDisplayPort) {
cmsDisplayPortRegister(displayPortMspInit());
}
#endif
#ifdef USE_UAV_INTERCONNECT
uavInterconnectBusInit();
#endif

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

@ -1116,16 +1116,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_3D:
sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
sbufWriteU16(dst, flight3DConfig()->neutral3d);
sbufWriteU16(dst, reversibleMotorsConfig()->deadband_low);
sbufWriteU16(dst, reversibleMotorsConfig()->deadband_high);
sbufWriteU16(dst, reversibleMotorsConfig()->neutral);
break;
case MSP_RC_DEADBAND:
sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
sbufWriteU16(dst, rcControlsConfig()->deadband3d_throttle);
sbufWriteU16(dst, rcControlsConfig()->mid_throttle_deadband);
break;
case MSP_SENSOR_ALIGNMENT:
@ -1425,7 +1425,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP2_INAV_MIXER:
sbufWriteU8(dst, mixerConfig()->yaw_motor_direction);
sbufWriteU8(dst, mixerConfig()->motorDirectionInverted);
sbufWriteU16(dst, 0);
sbufWriteU8(dst, mixerConfig()->platformType);
sbufWriteU8(dst, mixerConfig()->hasFlaps);
@ -1964,7 +1964,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#ifdef USE_GLOBAL_FUNCTIONS
case MSP2_INAV_SET_GLOBAL_FUNCTIONS:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 14) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
if ((dataSize == 10) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
globalFunctionsMutable(tmp_u8)->enabled = sbufReadU8(src);
globalFunctionsMutable(tmp_u8)->conditionId = sbufReadU8(src);
globalFunctionsMutable(tmp_u8)->action = sbufReadU8(src);
@ -1988,9 +1988,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_3D:
if (dataSize >= 6) {
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
flight3DConfigMutable()->neutral3d = sbufReadU16(src);
reversibleMotorsConfigMutable()->deadband_low = sbufReadU16(src);
reversibleMotorsConfigMutable()->deadband_high = sbufReadU16(src);
reversibleMotorsConfigMutable()->neutral = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
@ -2000,7 +2000,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
rcControlsConfigMutable()->deadband3d_throttle = sbufReadU16(src);
rcControlsConfigMutable()->mid_throttle_deadband = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
@ -2383,11 +2383,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
const uint8_t newChannel = (newFrequency % 8) + 1;
vtxSettingsConfigMutable()->band = newBand;
vtxSettingsConfigMutable()->channel = newChannel;
vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
} else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { //value is frequency in MHz. Ignore it if it's invalid
vtxSettingsConfigMutable()->band = 0;
vtxSettingsConfigMutable()->channel = 0;
vtxSettingsConfigMutable()->freq = newFrequency;
}
if (sbufBytesRemaining(src) > 1) {
@ -2755,7 +2750,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
case MSP2_INAV_SET_MIXER:
mixerConfigMutable()->yaw_motor_direction = sbufReadU8(src);
mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src);
sbufReadU16(src); // Was yaw_jump_prevention_limit
mixerConfigMutable()->platformType = sbufReadU8(src);
mixerConfigMutable()->hasFlaps = sbufReadU8(src);

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

@ -164,13 +164,13 @@ void initActiveBoxIds(void)
activeBoxIdCount = 0;
activeBoxIds[activeBoxIdCount++] = BOXARM;
if (sensors(SENSOR_ACC)) {
if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST;
}
if (!feature(FEATURE_AIRMODE)) {
if (!feature(FEATURE_AIRMODE) && STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
}
@ -181,35 +181,39 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
}
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
if (STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
}
//Camstab mode is enabled always
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
#ifdef USE_GPS
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
if (STATE(ALTITUDE_CONTROL) && (sensors(SENSOR_BARO) || (STATE(AIRPLANE) && feature(FEATURE_GPS)))) {
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
}
const bool navReadyQuads = !STATE(FIXED_WING) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
if (STATE(FIXED_WING)) {
if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) {
if (!STATE(ROVER) && !STATE(BOAT)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
}
if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN;
}
}
if (navReadyQuads || navReadyPlanes) {
if (navReadyMultirotor || navReadyOther) {
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
if (feature(FEATURE_GPS)) {
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
if (STATE(FIXED_WING)) {
if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
}
}
@ -223,8 +227,11 @@ void initActiveBoxIds(void)
#endif
if (STATE(FIXED_WING)) {
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
activeBoxIds[activeBoxIdCount++] = BOXMANUAL;
}
if (STATE(AIRPLANE)) {
if (!feature(FEATURE_FW_LAUNCH)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
}

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

@ -74,9 +74,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONT
PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = 5,
.yaw_deadband = 5,
.pos_hold_deadband = 20,
.pos_hold_deadband = 10,
.alt_hold_deadband = 50,
.deadband3d_throttle = 50,
.mid_throttle_deadband = 50,
.airmodeHandlingType = STICK_CENTER,
.airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD,
);
@ -99,12 +99,19 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
}
throttleStatus_e calculateThrottleStatus(void)
throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
{
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle;
if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + deadband3d_throttle)))
int value;
if (type == THROTTLE_STATUS_TYPE_RC) {
value = rxGetChannelValue(THROTTLE);
} else {
value = rcCommand[THROTTLE];
}
const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
return THROTTLE_LOW;
else if (!feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
return THROTTLE_LOW;
return THROTTLE_HIGH;
@ -183,7 +190,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
emergencyArmingUpdate(armingSwitchIsActive);
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
tryArm();

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

@ -45,6 +45,11 @@ typedef enum {
THROTTLE_HIGH
} throttleStatus_e;
typedef enum {
THROTTLE_STATUS_TYPE_RC = 0,
THROTTLE_STATUS_TYPE_COMMAND
} throttleStatusType_e;
typedef enum {
NOT_CENTERED = 0,
CENTERED
@ -80,7 +85,7 @@ typedef struct rcControlsConfig_s {
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC
uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered
uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation
} rcControlsConfig_t;
@ -100,7 +105,7 @@ bool checkStickPosition(stickPositions_e stickPos);
bool areSticksInApModePosition(uint16_t ap_mode);
bool areSticksDeflectedMoreThanPosHoldDeadband(void);
throttleStatus_e calculateThrottleStatus(void);
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);

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

@ -104,9 +104,9 @@ static void processAirmodeMultirotor(void) {
void processAirmode(void) {
if (STATE(FIXED_WING)) {
if (STATE(AIRPLANE)) {
processAirmodeAirplane();
} else {
} else if (STATE(MULTIROTOR)) {
processAirmodeMultirotor();
}

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

@ -80,7 +80,7 @@ uint32_t disableFlightMode(flightModeFlags_e mask)
return flightModeFlags;
}
bool FAST_CODE NOINLINE sensors(uint32_t mask)
bool sensors(uint32_t mask)
{
return enabledSensors & mask;
}

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

@ -110,7 +110,7 @@ typedef enum {
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
FIXED_WING_LEGACY = (1 << 4), // No new code should use this state. Use AIRPLANE, MULTIROTOR, ROVER, BOAT, ALTITUDE_CONTROL and MOVE_FORWARD_ONLY states
ANTI_WINDUP = (1 << 5),
FLAPERON_AVAILABLE = (1 << 6),
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
@ -123,6 +123,14 @@ typedef enum {
NAV_EXTRA_ARMING_SAFETY_BYPASSED = (1 << 14), // nav_extra_arming_safey was bypassed. Keep it until power cycle.
AIRMODE_ACTIVE = (1 << 15),
ESC_SENSOR_ENABLED = (1 << 16),
AIRPLANE = (1 << 17),
MULTIROTOR = (1 << 18),
ROVER = (1 << 19),
BOAT = (1 << 20),
ALTITUDE_CONTROL = (1 << 21), //It means it can fly
MOVE_FORWARD_ONLY = (1 << 22),
SET_REVERSIBLE_MOTORS_FORWARD = (1 << 23),
FW_HEADING_USE_YAW = (1 << 24),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

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

@ -22,7 +22,7 @@ tables:
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
enum: pitotSensor_e
- name: receiver_type
values: ["NONE", "PWM", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"]
@ -81,7 +81,7 @@ tables:
values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"ERPM", "RPM_FILTER", "RPM_FREQ"]
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -186,24 +186,23 @@ groups:
- name: gyro_stage2_lowpass_type
field: gyro_stage2_lowpass_type
table: filter_type
- name: dyn_notch_width_percent
field: dyn_notch_width_percent
- name: dynamic_gyro_notch_enabled
field: dynamicGyroNotchEnabled
condition: USE_DYNAMIC_FILTERS
min: 0
max: 20
- name: dyn_notch_range
field: dyn_notch_range
type: bool
- name: dynamic_gyro_notch_range
field: dynamicGyroNotchRange
condition: USE_DYNAMIC_FILTERS
table: dynamicFilterRangeTable
- name: dyn_notch_q
field: dyn_notch_q
- name: dynamic_gyro_notch_q
field: dynamicGyroNotchQ
condition: USE_DYNAMIC_FILTERS
min: 1
max: 1000
- name: dyn_notch_min_hz
field: dyn_notch_min_hz
- name: dynamic_gyro_notch_min_hz
field: dynamicGyroNotchMinHz
condition: USE_DYNAMIC_FILTERS
min: 60
min: 30
max: 1000
- name: gyro_to_use
condition: USE_DUAL_GYRO
@ -685,9 +684,9 @@ groups:
- name: PG_MIXER_CONFIG
type: mixerConfig_t
members:
- name: yaw_motor_direction
min: -1
max: 1
- name: motor_direction_inverted
field: motorDirectionInverted
type: bool
- name: platform_type
field: platformType
type: uint8_t
@ -704,19 +703,19 @@ groups:
min: 0
max: 450
- name: PG_MOTOR_3D_CONFIG
type: flight3DConfig_t
- name: PG_REVERSIBLE_MOTORS_CONFIG
type: reversibleMotorsConfig_t
members:
- name: 3d_deadband_low
field: deadband3d_low
field: deadband_low
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: 3d_deadband_high
field: deadband3d_high
field: deadband_high
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: 3d_neutral
field: neutral3d
field: neutral
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
@ -878,9 +877,6 @@ groups:
- name: rpm_gyro_filter_enabled
field: gyro_filter_enabled
type: bool
- name: rpm_dterm_filter_enabled
field: dterm_filter_enabled
type: bool
- name: rpm_gyro_harmonics
field: gyro_harmonics
type: uint8_t
@ -889,28 +885,13 @@ groups:
- name: rpm_gyro_min_hz
field: gyro_min_hz
type: uint8_t
min: 50
min: 30
max: 200
- name: rpm_gyro_q
field: gyro_q
type: uint16_t
min: 1
max: 3000
- name: dterm_gyro_harmonics
field: dterm_harmonics
type: uint8_t
min: 1
max: 3
- name: rpm_dterm_min_hz
field: dterm_min_hz
type: uint8_t
min: 50
max: 200
- name: rpm_dterm_q
field: dterm_q
type: uint16_t
min: 1
max: 3000
- name: PG_GPS_CONFIG
type: gpsConfig_t
condition: USE_GPS
@ -952,13 +933,13 @@ groups:
min: 0
max: 100
- name: pos_hold_deadband
min: 10
min: 2
max: 250
- name: alt_hold_deadband
min: 10
max: 250
- name: 3d_deadband_throttle
field: deadband3d_throttle
field: mid_throttle_deadband
min: 0
max: 200
- name: mc_airmode_type
@ -1238,6 +1219,25 @@ groups:
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_p
field: bank_fw.pid[PID_POS_HEADING].P
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_i
field: bank_fw.pid[PID_POS_HEADING].I
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_d
field: bank_fw.pid[PID_POS_HEADING].D
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_pidsum_limit
field: navFwPosHdgPidsumLimit
min: PID_SUM_LIMIT_MIN
max: PID_SUM_LIMIT_MAX
- name: mc_iterm_relax_type
field: iterm_relax_type
table: iterm_relax_type
@ -1654,6 +1654,13 @@ groups:
- name: nav_fw_allow_manual_thr_increase
field: fw.allow_manual_thr_increase
type: bool
- name: nav_use_fw_yaw_control
field: fw.useFwNavYawControl
type: bool
- name: nav_fw_yaw_deadband
field: fw.yawControlDeadband
min: 0
max: 90
- name: PG_TELEMETRY_CONFIG
type: telemetryConfig_t
@ -1691,8 +1698,8 @@ groups:
field: hottAlarmSoundInterval
min: 0
max: 120
- name: telemetry_uart_unidir
field: uartUnidirectional
- name: telemetry_halfduplex
field: halfDuplex
type: bool
- name: smartport_fuel_unit
field: smartportFuelUnit
@ -1931,8 +1938,8 @@ groups:
max: 4
- name: osd_hud_margin_v
field: hud_margin_v
min: 0
max: 4
min: 1
max: 3
- name: osd_hud_homing
field: hud_homing
type: bool
@ -1955,6 +1962,10 @@ groups:
field: hud_radar_nearest
min: 0
max: 4000
- name: osd_hud_wp_disp
field: hud_wp_disp
min: 0
max: 3
- name: osd_left_sidebar_scroll
field: left_sidebar_scroll
table: osd_sidebar_scroll
@ -2080,16 +2091,10 @@ groups:
field: lowPowerDisarm
table: vtx_low_power_disarm
type: uint8_t
- name: vtx_freq
field: freq
min: VTX_SETTINGS_MIN_FREQUENCY_MHZ
max: VTX_SETTINGS_MAX_FREQUENCY_MHZ
condition: VTX_SETTINGS_FREQCMD
- name: vtx_pit_mode_freq
field: pitModeFreq
min: VTX_SETTINGS_MIN_FREQUENCY_MHZ
max: VTX_SETTINGS_MAX_FREQUENCY_MHZ
condition: VTX_SETTINGS_FREQCMD
- name: vtx_pit_mode_chan
field: pitModeChan
min: VTX_SETTINGS_MIN_CHANNEL
max: VTX_SETTINGS_MAX_CHANNEL
- name: PG_PINIOBOX_CONFIG
type: pinioBoxConfig_t

86
src/main/flight/dynamic_gyro_notch.c Normal file
View file

@ -0,0 +1,86 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include "platform.h"
#ifdef USE_DYNAMIC_FILTERS
#include <stdint.h>
#include "dynamic_gyro_notch.h"
#include "fc/config.h"
#include "build/debug.h"
#include "sensors/gyro.h"
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
state->filtersApplyFn = nullFilterApply;
state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f;
state->enabled = gyroConfig()->dynamicGyroNotchEnabled;
state->looptime = getLooptime();
if (state->enabled) {
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
/*
* Step 1 - init all filters even if they will not be used further down the road
*/
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
}
state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) {
state->frequency[axis] = frequency;
DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency);
if (state->enabled) {
biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
}
}
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) {
float output = input;
/*
* We always apply all filters. If a filter dimension is disabled, one of
* the function pointers will be a null apply function
*/
output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output);
output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output);
output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output);
return output;
}
#endif

50
src/main/flight/dynamic_gyro_notch.h Normal file
View file

@ -0,0 +1,50 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include <stdint.h>
#include "common/axis.h"
#include "common/filter.h"
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
typedef struct dynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT];
float dynNotchQ;
float dynNotch1Ctr;
float dynNotch2Ctr;
uint32_t looptime;
uint8_t enabled;
/*
* Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type
*/
biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
filterApplyFnPtr filtersApplyFn;
} dynamicGyroNotchState_t;
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state);
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency);
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input);

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

@ -258,7 +258,7 @@ void failsafeApplyControlInput(void)
{
// Prepare FAILSAFE_CHANNEL_AUTO values for rcCommand
int16_t autoRcCommand[4];
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
@ -287,7 +287,7 @@ void failsafeApplyControlInput(void)
break;
case THROTTLE:
rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : getThrottleIdleValue();
rcCommand[idx] = feature(FEATURE_REVERSIBLE_MOTORS) ? PWM_RANGE_MIDDLE : getThrottleIdleValue();
break;
}
break;
@ -397,7 +397,7 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus()) {
if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (!receivingRxDataAndNotFailsafeMode) {

120
src/main/flight/gyroanalyse.c
View file

@ -26,6 +26,7 @@
#include <stdint.h>
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#ifdef USE_DYNAMIC_FILTERS
@ -35,6 +36,7 @@
#include "common/maths.h"
#include "common/time.h"
#include "common/utils.h"
#include "config/feature.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/time.h"
@ -55,68 +57,40 @@
// we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
#define DYN_NOTCH_OSD_MIN_THROTTLE 20
void gyroDataAnalyseStateInit(
gyroAnalyseState_t *state,
uint16_t minFrequency,
uint8_t range,
uint32_t targetLooptimeUs
) {
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
state->minFrequency = minFrequency;
static uint16_t EXTENDED_FASTRAM fftSamplingRateHz;
static float EXTENDED_FASTRAM fftResolution;
static uint8_t EXTENDED_FASTRAM fftStartBin;
static uint16_t EXTENDED_FASTRAM dynNotchMaxCtrHz;
static uint8_t dynamicFilterRange;
static float EXTENDED_FASTRAM dynNotchQ;
static float EXTENDED_FASTRAM dynNotch1Ctr;
static float EXTENDED_FASTRAM dynNotch2Ctr;
static uint16_t EXTENDED_FASTRAM dynNotchMinHz;
static bool EXTENDED_FASTRAM dualNotch = true;
static uint16_t EXTENDED_FASTRAM dynNotchMaxFFT;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
static EXTENDED_FASTRAM float hanningWindow[FFT_WINDOW_SIZE];
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
dynamicFilterRange = gyroConfig()->dyn_notch_range;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
if (gyroConfig()->dyn_notch_width_percent == 0) {
dualNotch = false;
if (range == DYN_NOTCH_RANGE_HIGH) {
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
}
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
}
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
else if (range == DYN_NOTCH_RANGE_MEDIUM) {
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
}
// If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
state->fftSamplingRateHz = MIN((gyroLoopRateHz / 3), state->fftSamplingRateHz);
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
state->fftResolution = (float)state->fftSamplingRateHz / FFT_WINDOW_SIZE;
fftStartBin = dynNotchMinHz / lrintf(fftResolution);
state->fftStartBin = state->minFrequency / lrintf(state->fftResolution);
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
state->maxFrequency = state->fftSamplingRateHz / 2; //Nyquist
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
state->hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
}
}
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
// *** can this next line be removed ??? ***
gyroDataAnalyseInit(targetLooptimeUs);
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
state->maxSampleCount = samplingFrequency / state->fftSamplingRateHz;
state->maxSampleCountRcp = 1.f / state->maxSampleCount;
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
@ -124,11 +98,11 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
const float looptime = MAX(1000000u / state->fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value
state->centerFreq[axis] = dynNotchMaxCtrHz;
state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
state->centerFreq[axis] = state->maxFrequency;
state->prevCenterFreq[axis] = state->maxFrequency;
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
}
}
@ -138,13 +112,15 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float
state->oversampledGyroAccumulator[axis] += sample;
}
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
void gyroDataAnalyse(gyroAnalyseState_t *state)
{
state->filterUpdateExecute = false; //This will be changed to true only if new data is present
// samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples
state->sampleCount++;
@ -169,7 +145,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
// calculate FFT and update filters
if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
gyroDataAnalyseUpdate(state);
--state->updateTicks;
}
}
@ -183,7 +159,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t
/*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
{
enum {
STEP_ARM_CFFT_F32,
@ -245,7 +221,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
uint8_t binStart = 0;
uint8_t binMax = 0;
//for bins after initial decline, identify start bin and max bin
for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) {
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
if (!fftIncreased) {
binStart = i; // first up-step bin
@ -282,24 +258,20 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
}
}
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
float centerFreq = dynNotchMaxCtrHz;
float centerFreq = state->maxFrequency;
float fftMeanIndex = 0;
// idx was shifted by 1 to start at 1, not 0
if (fftSum > 0) {
fftMeanIndex = (fftWeightedSum / fftSum) - 1;
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
centerFreq = fftMeanIndex * fftResolution;
centerFreq = fftMeanIndex * state->fftResolution;
} else {
centerFreq = state->prevCenterFreq[state->updateAxis];
}
centerFreq = fmax(centerFreq, dynNotchMinHz);
centerFreq = fmax(centerFreq, state->minFrequency);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
state->centerFreq[state->updateAxis] = centerFreq;
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
// Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
break;
}
case STEP_UPDATE_FILTERS:
@ -307,13 +279,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// 7us
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
if (dualNotch) {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
} else {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
}
/*
* Filters will be updated inside dynamicGyroNotchFiltersUpdate()
*/
state->filterUpdateExecute = true;
state->filterUpdateAxis = state->updateAxis;
state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
}
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
@ -326,9 +297,9 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// apply hanning window to gyro samples and store result in fftData
// hanning starts and ends with 0, could be skipped for minor speed improvement
const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &state->hanningWindow[0], &state->fftData[0], ringBufIdx);
if (state->circularBufferIdx > 0) {
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &state->hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
}
}
}
@ -336,13 +307,4 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
state->updateStep = (state->updateStep + 1) % STEP_COUNT;
}
uint16_t getMaxFFT(void) {
return dynNotchMaxFFT;
}
void resetMaxFFT(void) {
dynNotchMaxFFT = 0;
}
#endif // USE_DYNAMIC_FILTERS

22
src/main/flight/gyroanalyse.h
View file

@ -51,13 +51,27 @@ typedef struct gyroAnalyseState_s {
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
uint16_t centerFreq[XYZ_AXIS_COUNT];
uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
bool filterUpdateExecute;
uint8_t filterUpdateAxis;
uint16_t filterUpdateFrequency;
uint16_t fftSamplingRateHz;
uint8_t fftStartBin;
float fftResolution;
uint16_t minFrequency;
uint16_t maxFrequency;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
float hanningWindow[FFT_WINDOW_SIZE];
} gyroAnalyseState_t;
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
void gyroDataAnalyseStateInit(
gyroAnalyseState_t *state,
uint16_t minFrequency,
uint8_t range,
uint32_t targetLooptimeUs
);
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse);
#endif

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

@ -57,7 +57,7 @@ void hilUpdateControlState(void)
{
// FIXME: There must be a cleaner way to to this
// If HIL active, store PID outout into hilState and disable motor control
if (FLIGHT_MODE(MANUAL_MODE) || !STATE(FIXED_WING)) {
if (FLIGHT_MODE(MANUAL_MODE) || !STATE(AIRPLANE)) {
hilToSIM.pidCommand[ROLL] = rcCommand[ROLL];
hilToSIM.pidCommand[PITCH] = rcCommand[PITCH];
hilToSIM.pidCommand[YAW] = rcCommand[YAW];

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

@ -23,6 +23,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "blackbox/blackbox.h"
#include "build/build_config.h"
@ -481,7 +483,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
const float nearness = ABS(100 - (accMagnitudeSq * 100));
const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f;
// Experiment: if rotation rate on a FIXED_WING is higher than a threshold - centrifugal force messes up too much and we
// Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we
// should not use measured accel for AHRS comp
// Centrifugal acceleration AccelC = Omega^2 * R = Speed^2 / R
// Omega = Speed / R
@ -499,7 +501,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
// Default - don't apply rate/ignore scaling
float accWeight_RateIgnore = 1.0f;
if (ARMING_FLAG(ARMED) && STATE(FIXED_WING) && imuConfig()->acc_ignore_rate) {
if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) {
const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
@ -532,7 +534,7 @@ static void imuCalculateEstimatedAttitude(float dT)
bool useCOG = false;
#if defined(USE_GPS)
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
bool canUseCOG = isGPSHeadingValid();
// Prefer compass (if available)
@ -631,7 +633,7 @@ void imuCheckVibrationLevels(void)
// DEBUG_VIBE values 4-7 are used by NAV estimator
}
void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)
void imuUpdateAttitude(timeUs_t currentTimeUs)
{
/* Calculate dT */
static timeUs_t previousIMUUpdateTimeUs;
@ -670,7 +672,7 @@ bool isImuReady(void)
bool isImuHeadingValid(void)
{
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING) && gpsHeadingInitialized);
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
}
float calculateCosTiltAngle(void)

301
src/main/flight/mixer.c
View file

@ -21,6 +21,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#include "build/debug.h"
#include "common/axis.h"
@ -61,20 +63,27 @@ static float mixerScale = 1.0f;
static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static EXTENDED_FASTRAM uint8_t motorCount = 0;
EXTENDED_FASTRAM int mixerThrottleCommand;
static EXTENDED_FASTRAM int throttleIdleValue = 0;
static EXTENDED_FASTRAM int throttleIdleValue = 0;
static EXTENDED_FASTRAM int motorValueWhenStopped = 0;
static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
static EXTENDED_FASTRAM int throttleRangeMin = 0;
static EXTENDED_FASTRAM int throttleRangeMax = 0;
static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
.deadband3d_low = 1406,
.deadband3d_high = 1514,
.neutral3d = 1460
PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.deadband_low = 1406,
.deadband_high = 1514,
.neutral = 1460
);
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3);
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.yaw_motor_direction = 1,
.motorDirectionInverted = 0,
.platformType = PLATFORM_MULTIROTOR,
.hasFlaps = false,
.appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
@ -131,7 +140,7 @@ static void computeMotorCount(void)
}
}
uint8_t FAST_CODE NOINLINE getMotorCount(void) {
uint8_t getMotorCount(void) {
return motorCount;
}
@ -147,14 +156,36 @@ bool mixerIsOutputSaturated(void)
void mixerUpdateStateFlags(void)
{
// set flag that we're on something with wings
DISABLE_STATE(FIXED_WING_LEGACY);
DISABLE_STATE(MULTIROTOR);
DISABLE_STATE(ROVER);
DISABLE_STATE(BOAT);
DISABLE_STATE(AIRPLANE);
DISABLE_STATE(MOVE_FORWARD_ONLY);
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
ENABLE_STATE(FIXED_WING);
ENABLE_STATE(FIXED_WING_LEGACY);
ENABLE_STATE(AIRPLANE);
ENABLE_STATE(ALTITUDE_CONTROL);
ENABLE_STATE(MOVE_FORWARD_ONLY);
} if (mixerConfig()->platformType == PLATFORM_ROVER) {
ENABLE_STATE(ROVER);
ENABLE_STATE(FIXED_WING_LEGACY);
ENABLE_STATE(MOVE_FORWARD_ONLY);
} if (mixerConfig()->platformType == PLATFORM_BOAT) {
ENABLE_STATE(BOAT);
ENABLE_STATE(FIXED_WING_LEGACY);
ENABLE_STATE(MOVE_FORWARD_ONLY);
} else if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
ENABLE_STATE(MULTIROTOR);
ENABLE_STATE(ALTITUDE_CONTROL);
} else if (mixerConfig()->platformType == PLATFORM_TRICOPTER) {
ENABLE_STATE(MULTIROTOR);
ENABLE_STATE(ALTITUDE_CONTROL);
} else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) {
DISABLE_STATE(FIXED_WING);
} else {
DISABLE_STATE(FIXED_WING);
}
ENABLE_STATE(MULTIROTOR);
ENABLE_STATE(ALTITUDE_CONTROL);
}
if (mixerConfig()->hasFlaps) {
ENABLE_STATE(FLAPERON_AVAILABLE);
@ -172,7 +203,7 @@ void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
@ -211,10 +242,13 @@ void mixerInit(void)
computeMotorCount();
loadPrimaryMotorMixer();
// in 3D mode, mixer gain has to be halved
if (feature(FEATURE_3D)) {
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
mixerScale = 0.5f;
}
throttleDeadbandLow = PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband;
throttleDeadbandHigh = PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband;
mixerResetDisarmedMotors();
if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
@ -222,17 +256,73 @@ void mixerInit(void)
} else {
motorRateLimitingApplyFn = nullMotorRateLimiting;
}
if (mixerConfig()->motorDirectionInverted) {
motorYawMultiplier = -1;
} else {
motorYawMultiplier = 1;
}
}
void mixerResetDisarmedMotors(void)
{
int motorZeroCommand;
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
motorZeroCommand = reversibleMotorsConfig()->neutral;
throttleRangeMin = throttleDeadbandHigh;
throttleRangeMax = motorConfig()->maxthrottle;
} else {
motorZeroCommand = motorConfig()->mincommand;
throttleRangeMin = getThrottleIdleValue();
throttleRangeMax = motorConfig()->maxthrottle;
}
reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
if (feature(FEATURE_MOTOR_STOP)) {
motorValueWhenStopped = motorZeroCommand;
} else {
motorValueWhenStopped = getThrottleIdleValue();
}
// set disarmed motor values
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motor_disarmed[i] = motorZeroCommand;
}
}
void FAST_CODE NOINLINE writeMotors(void)
#ifdef USE_DSHOT
static uint16_t handleOutputScaling(
int16_t input, // Input value from the mixer
int16_t stopThreshold, // Threshold value to check if motor should be rotating or not
int16_t onStopValue, // Value sent to the ESC when min rotation is required - on motor_stop it is STOP command, without motor_stop it's a value that keeps rotation
int16_t inputScaleMin, // Input range - min value
int16_t inputScaleMax, // Input range - max value
int16_t outputScaleMin, // Output range - min value
int16_t outputScaleMax, // Output range - max value
bool moveForward // If motor should be rotating FORWARD or BACKWARD
)
{
int value;
if (moveForward && input < stopThreshold) {
//Send motor stop command
value = onStopValue;
}
else if (!moveForward && input > stopThreshold) {
//Send motor stop command
value = onStopValue;
}
else {
//Scale input to protocol output values
value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax);
value = constrain(value, outputScaleMin, outputScaleMax);
}
return value;
}
#endif
void FAST_CODE writeMotors(void)
{
for (int i = 0; i < motorCount; i++) {
uint16_t motorValue;
@ -241,31 +331,73 @@ void FAST_CODE NOINLINE writeMotors(void)
// If we use DSHOT we need to convert motorValue to DSHOT ranges
if (isMotorProtocolDigital()) {
if (feature(FEATURE_3D)) {
if (motor[i] >= throttleIdleValue && motor[i] <= flight3DConfig()->deadband3d_low) {
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
}
else if (motor[i] >= flight3DConfig()->deadband3d_high && motor[i] <= motorConfig()->maxthrottle) {
motorValue = scaleRangef(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
}
else {
motorValue = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
motorValue = handleOutputScaling(
motor[i],
throttleRangeMin,
DSHOT_DISARM_COMMAND,
throttleRangeMin,
throttleRangeMax,
DSHOT_3D_DEADBAND_HIGH,
DSHOT_MAX_THROTTLE,
true
);
} else {
motorValue = handleOutputScaling(
motor[i],
throttleRangeMax,
DSHOT_DISARM_COMMAND,
throttleRangeMin,
throttleRangeMax,
DSHOT_MIN_THROTTLE,
DSHOT_3D_DEADBAND_LOW,
false
);
}
}
else {
if (motor[i] < throttleIdleValue) { // motor disarmed
motorValue = DSHOT_DISARM_COMMAND;
}
else {
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
motorValue = handleOutputScaling(
motor[i],
throttleIdleValue,
DSHOT_DISARM_COMMAND,
motorConfig()->mincommand,
motorConfig()->maxthrottle,
DSHOT_MIN_THROTTLE,
DSHOT_MAX_THROTTLE,
true
);
}
}
else {
motorValue = motor[i];
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
motorValue = handleOutputScaling(
motor[i],
throttleRangeMin,
motor[i],
throttleRangeMin,
throttleRangeMax,
reversibleMotorsConfig()->deadband_high,
motorConfig()->maxthrottle,
true
);
} else {
motorValue = handleOutputScaling(
motor[i],
throttleRangeMax,
motor[i],
throttleRangeMin,
throttleRangeMax,
motorConfig()->mincommand,
reversibleMotorsConfig()->deadband_low,
false
);
}
} else {
motorValue = motor[i];
}
}
#else
// We don't define USE_DSHOT
@ -287,7 +419,7 @@ void writeAllMotors(int16_t mc)
void stopMotors(void)
{
writeAllMotors(feature(FEATURE_3D) ? flight3DConfig()->neutral3d : motorConfig()->mincommand);
writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);
delay(50); // give the timers and ESCs a chance to react.
}
@ -297,11 +429,24 @@ void stopPwmAllMotors(void)
pwmShutdownPulsesForAllMotors(motorCount);
}
void FAST_CODE NOINLINE mixTable(const float dT)
static int getReversibleMotorsThrottleDeadband(void)
{
int directionValue;
if (reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
directionValue = reversibleMotorsConfig()->deadband_low;
} else {
directionValue = reversibleMotorsConfig()->deadband_high;
}
return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
}
void FAST_CODE mixTable(const float dT)
{
int16_t input[3]; // RPY, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING) && FLIGHT_MODE(MANUAL_MODE)) {
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
// Direct passthru from RX
input[ROLL] = rcCommand[ROLL];
input[PITCH] = rcCommand[PITCH];
@ -323,7 +468,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
rpyMix[i] =
(input[PITCH] * currentMixer[i].pitch +
input[ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * input[YAW] * currentMixer[i].yaw) * mixerScale;
-motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
@ -332,38 +477,42 @@ void FAST_CODE NOINLINE mixTable(const float dT)
int16_t rpyMixRange = rpyMixMax - rpyMixMin;
int16_t throttleRange;
int16_t throttleMin, throttleMax;
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// Find min and max throttle based on condition.
#ifdef USE_GLOBAL_FUNCTIONS
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
throttleMin = throttleIdleValue;
throttleMax = motorConfig()->maxthrottle;
throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle;
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax);
} else
#endif
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig()->deadband3d_low;
throttleMin = throttleIdleValue;
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
throttleMax = motorConfig()->maxthrottle;
throttleMin = flight3DConfig()->deadband3d_high;
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
mixerThrottleCommand = throttleMax = flight3DConfig()->deadband3d_low;
throttleMin = throttleIdleValue;
} else { // Deadband handling from positive to negative
throttleMax = motorConfig()->maxthrottle;
mixerThrottleCommand = throttleMin = flight3DConfig()->deadband3d_high;
if (rcCommand[THROTTLE] >= (throttleDeadbandHigh) || STATE(SET_REVERSIBLE_MOTORS_FORWARD)) {
/*
* Throttle is above deadband, FORWARD direction
*/
reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
throttleRangeMax = motorConfig()->maxthrottle;
throttleRangeMin = throttleDeadbandHigh;
DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
} else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
/*
* Throttle is below deadband, BACKWARD direction
*/
reversibleMotorsThrottleState = MOTOR_DIRECTION_BACKWARD;
throttleRangeMax = throttleDeadbandLow;
throttleRangeMin = motorConfig()->mincommand;
}
motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
} else {
mixerThrottleCommand = rcCommand[THROTTLE];
throttleMin = throttleIdleValue;
throttleMax = motorConfig()->maxthrottle;
throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle;
// Throttle scaling to limit max throttle when battery is full
#ifdef USE_GLOBAL_FUNCTIONS
@ -377,6 +526,9 @@ void FAST_CODE NOINLINE mixTable(const float dT)
}
}
throttleMin = throttleRangeMin;
throttleMax = throttleRangeMax;
throttleRange = throttleMax - throttleMin;
#define THROTTLE_CLIPPING_FACTOR 0.33f
@ -397,28 +549,19 @@ void FAST_CODE NOINLINE mixTable(const float dT)
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
if (ARMING_FLAG(ARMED)) {
const motorStatus_e currentMotorStatus = getMotorStatus();
for (int i = 0; i < motorCount; i++) {
motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
} else if (feature(FEATURE_3D)) {
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) {
motor[i] = constrain(motor[i], throttleIdleValue, flight3DConfig()->deadband3d_low);
} else {
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
}
} else {
motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);
motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
}
// Motor stop handling
if (ARMING_FLAG(ARMED) && (getMotorStatus() != MOTOR_RUNNING)) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
} else {
motor[i] = throttleIdleValue;
}
if (currentMotorStatus != MOTOR_RUNNING) {
motor[i] = motorValueWhenStopped;
}
}
} else {
@ -437,8 +580,8 @@ motorStatus_e getMotorStatus(void)
return MOTOR_STOPPED_AUTO;
}
if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) {
if ((STATE(FIXED_WING) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
return MOTOR_STOPPED_USER;
}
}
@ -450,4 +593,4 @@ void loadPrimaryMotorMixer(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
currentMixer[i] = *primaryMotorMixer(i);
}
}
}

20
src/main/flight/mixer.h
View file

@ -63,7 +63,7 @@ typedef struct motorMixer_s {
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer);
typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
int8_t motorDirectionInverted;
uint8_t platformType;
bool hasFlaps;
int16_t appliedMixerPreset;
@ -72,13 +72,13 @@ typedef struct mixerConfig_s {
PG_DECLARE(mixerConfig_t, mixerConfig);
typedef struct flight3DConfig_s {
uint16_t deadband3d_low; // min 3d value
uint16_t deadband3d_high; // max 3d value
uint16_t neutral3d; // center 3d value
} flight3DConfig_t;
typedef struct reversibleMotorsConfig_s {
uint16_t deadband_low; // min 3d value
uint16_t deadband_high; // max 3d value
uint16_t neutral; // center 3d value
} reversibleMotorsConfig_t;
PG_DECLARE(flight3DConfig_t, flight3DConfig);
PG_DECLARE(reversibleMotorsConfig_t, reversibleMotorsConfig);
typedef struct motorConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
@ -102,6 +102,12 @@ typedef enum {
MOTOR_RUNNING
} motorStatus_e;
typedef enum {
MOTOR_DIRECTION_FORWARD,
MOTOR_DIRECTION_BACKWARD,
MOTOR_DIRECTION_DEADBAND
} reversibleMotorsThrottleState_e;
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand;

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

@ -21,6 +21,8 @@
#include <platform.h>
FILE_COMPILE_FOR_SPEED
#include "build/build_config.h"
#include "build/debug.h"
@ -118,7 +120,6 @@ STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM uint8_t itermRelax;
static EXTENDED_FASTRAM uint8_t itermRelaxType;
static EXTENDED_FASTRAM float itermRelaxSetpointThreshold;
#ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf;
@ -186,6 +187,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.I = 50, // NAV_VEL_Z_I * 20
.D = 10, // NAV_VEL_Z_D * 100
.FF = 0,
},
[PID_POS_HEADING] = {
.P = 0,
.I = 0,
.D = 0,
.FF = 0
}
}
},
@ -213,6 +220,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.I = 5, // FW_POS_XY_I * 100
.D = 8, // FW_POS_XY_D * 100
.FF = 0,
},
[PID_POS_HEADING] = {
.P = 30,
.I = 2,
.D = 0,
.FF = 0
}
}
},
@ -256,6 +269,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.antigravityAccelerator = 1.0f,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
.pidControllerType = PID_TYPE_AUTO,
.navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
);
bool pidInitFilters(void)
@ -530,7 +544,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
@ -562,7 +576,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
}
/* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
@ -584,7 +598,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
}
static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateError, float dT) {
static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
float newPTerm = rateError * pidState->kP;
return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
@ -643,7 +657,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
if (itermRelaxType == ITERM_RELAX_SETPOINT) {
*itermErrorRate *= itermRelaxFactor;
@ -707,10 +721,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
// Apply D-term notch
deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
#ifdef USE_RPM_FILTER
deltaFiltered = rpmFilterDtermApply((uint8_t)axis, deltaFiltered);
#endif
// Apply additional lowpass
deltaFiltered = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, deltaFiltered);
deltaFiltered = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, deltaFiltered);
@ -861,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
targetRates.x = 0.0f;
targetRates.y = 0.0f;
if (STATE(FIXED_WING)) {
if (STATE(AIRPLANE)) {
if (calculateCosTiltAngle() >= 0.173648f) {
// Ideal banked turn follow the equations:
// forward_vel^2 / radius = Gravity * tan(roll_angle)
@ -905,7 +915,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
// Replace YAW on quads - add it in on airplanes
if (STATE(FIXED_WING)) {
if (STATE(AIRPLANE)) {
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
}
else {
@ -932,7 +942,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
}
void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
void checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
@ -945,7 +955,7 @@ void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
}
void FAST_CODE NOINLINE pidController(float dT)
void FAST_CODE pidController(float dT)
{
if (!pidFiltersConfigured) {
return;
@ -1018,7 +1028,7 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType;
}
if (STATE(FIXED_WING)) {
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
return PID_TYPE_NONE;
}
@ -1042,7 +1052,6 @@ void pidInit(void)
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
@ -1072,7 +1081,11 @@ void pidInit(void)
}
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
if (
mixerConfig()->platformType == PLATFORM_AIRPLANE ||
mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER
) {
usedPidControllerType = PID_TYPE_PIFF;
} else {
usedPidControllerType = PID_TYPE_PID;
@ -1108,9 +1121,9 @@ void pidInit(void)
}
}
const pidBank_t FAST_CODE NOINLINE * pidBank(void) {
const pidBank_t * pidBank(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
}
pidBank_t FAST_CODE NOINLINE * pidBankMutable(void) {
pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
}

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

@ -65,6 +65,7 @@ typedef enum {
PID_LEVEL, // + +
PID_HEADING, // + +
PID_VEL_Z, // + n/a
PID_POS_HEADING,// n/a +
PID_ITEM_COUNT
} pidIndex_e;
@ -148,6 +149,8 @@ typedef struct pidProfile_s {
float antigravityGain;
float antigravityAccelerator;
uint8_t antigravityCutoff;
uint16_t navFwPosHdgPidsumLimit;
} pidProfile_t;
typedef struct pidAutotuneConfig_s {

40
src/main/flight/rpm_filter.c
View file

@ -22,6 +22,8 @@
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include "platform.h"
#include "flight/rpm_filter.h"
#include "config/parameter_group.h"
@ -43,17 +45,13 @@
#define RPM_FILTER_RPM_LPF_HZ 150
#define RPM_FILTER_HARMONICS 3
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1);
PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
.gyro_filter_enabled = 0,
.dterm_filter_enabled = 0,
.gyro_harmonics = 1,
.gyro_min_hz = 100,
.gyro_q = 500,
.dterm_harmonics = 1,
.dterm_min_hz = 100,
.dterm_q = 500, );
.gyro_q = 500, );
typedef struct
{
@ -70,11 +68,8 @@ typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor,
static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS];
static EXTENDED_FASTRAM float erpmToHz;
static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters;
static EXTENDED_FASTRAM rpmFilterBank_t dtermRpmFilters;
static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn;
static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmDtermApplyFn;
static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn;
static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmDtermUpdateFn;
float nullRpmFilterApply(rpmFilterBank_t *filter, uint8_t axis, float input)
{
@ -89,7 +84,7 @@ void nullRpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseF
UNUSED(baseFrequency);
}
float FAST_CODE rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
float rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
{
float output = input;
@ -141,10 +136,9 @@ static void rpmFilterInit(rpmFilterBank_t *filter, uint16_t q, uint8_t minHz, ui
void disableRpmFilters(void) {
rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
rpmDtermApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
}
void FAST_CODE NOINLINE rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
{
@ -172,7 +166,6 @@ void rpmFiltersInit(void)
erpmToHz = ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2) / RPM_TO_HZ;
rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
if (rpmFilterConfig()->gyro_filter_enabled)
{
@ -184,20 +177,9 @@ void rpmFiltersInit(void)
rpmGyroApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
}
if (rpmFilterConfig()->dterm_filter_enabled)
{
rpmFilterInit(
&dtermRpmFilters,
rpmFilterConfig()->dterm_q,
rpmFilterConfig()->dterm_min_hz,
rpmFilterConfig()->dterm_harmonics);
rpmDtermApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
}
}
void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
void rpmFilterUpdateTask(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
@ -215,18 +197,12 @@ void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
}
rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
rpmDtermUpdateFn(&dtermRpmFilters, i, baseFrequency);
}
}
float FAST_CODE rpmFilterGyroApply(uint8_t axis, float input)
float rpmFilterGyroApply(uint8_t axis, float input)
{
return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
}
float FAST_CODE rpmFilterDtermApply(uint8_t axis, float input)
{
return rpmDtermApplyFn(&dtermRpmFilters, axis, input);
}
#endif

3
src/main/flight/rpm_filter.h
View file

@ -49,5 +49,4 @@ PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
void disableRpmFilters(void);
void rpmFiltersInit(void);
void rpmFilterUpdateTask(timeUs_t currentTimeUs);
float rpmFilterGyroApply(uint8_t axis, float input);
float rpmFilterDtermApply(uint8_t axis, float input);
float rpmFilterGyroApply(uint8_t axis, float input);

4
src/main/flight/rth_estimator.c
View file

@ -146,10 +146,10 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee
// returns Wh
static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
// Fixed wing only for now
if (!STATE(FIXED_WING))
if (!STATE(FIXED_WING_LEGACY))
return -1;
if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid()
if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING_LEGACY)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid()
#ifdef USE_WIND_ESTIMATOR
&& isEstimatedWindSpeedValid()
#endif

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

@ -248,7 +248,7 @@ void servoMixer(float dT)
input[INPUT_STABILIZED_YAW] = axisPID[YAW];
// Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter
if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) &&
if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) &&
(mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
@ -451,17 +451,17 @@ void processServoAutotrim(void)
}
}
bool FAST_CODE NOINLINE isServoOutputEnabled(void)
bool isServoOutputEnabled(void)
{
return servoOutputEnabled;
}
void NOINLINE setServoOutputEnabled(bool flag)
void setServoOutputEnabled(bool flag)
{
servoOutputEnabled = flag;
}
bool FAST_CODE NOINLINE isMixerUsingServos(void)
bool isMixerUsingServos(void)
{
return mixerUsesServos;
}

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

@ -79,7 +79,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
{
static timeUs_t lastUpdateUs = 0;
if (!STATE(FIXED_WING) ||
if (!STATE(FIXED_WING_LEGACY) ||
!isGPSHeadingValid() ||
!gpsSol.flags.validVelNE ||
!gpsSol.flags.validVelD) {

20
src/main/io/dashboard.c
View file

@ -418,14 +418,6 @@ static void showStatusPage(void)
i2c_OLED_send_string(lineBuffer);
}
#endif
rowIndex++;
char rollTrim[7], pitchTrim[7];
formatTrimDegrees(rollTrim, boardAlignment()->rollDeciDegrees);
formatTrimDegrees(pitchTrim, boardAlignment()->pitchDeciDegrees);
tfp_sprintf(lineBuffer, "Acc: %sR, %sP", rollTrim, pitchTrim );
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
void dashboardUpdate(timeUs_t currentTimeUs)
@ -551,16 +543,4 @@ void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
nextPageAt = futureMicros;
}
void formatTrimDegrees ( char *formattedTrim, int16_t trimValue ) {
char trim[6];
tfp_sprintf(trim, "%d", trimValue);
int x = strlen(trim)-1;
strncpy(formattedTrim,trim,x);
formattedTrim[x] = '\0';
if (trimValue !=0) {
strcat(formattedTrim,".");
}
strcat(formattedTrim,trim+x);
}
#endif

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

@ -25,6 +25,4 @@ void dashboardInit(void);
void dashboardUpdate(timeUs_t currentTimeUs);
void dashboardSetPage(pageId_e newPageId);
void dashboardSetNextPageChangeAt(timeUs_t futureMicros);
void formatTrimDegrees ( char formattedTrim[7], int16_t trimValue );
void dashboardSetNextPageChangeAt(timeUs_t futureMicros);

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

@ -31,6 +31,8 @@
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#ifdef USE_OSD
#include "build/debug.h"
@ -196,7 +198,7 @@ static bool osdDisplayHasCanvas;
#define AH_SIDEBAR_WIDTH_POS 7
#define AH_SIDEBAR_HEIGHT_POS 3
PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9);
PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10);
static int digitCount(int32_t value)
{
@ -530,14 +532,6 @@ static uint16_t osdConvertRSSI(void)
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
}
static void osdGetVTXPowerChar(char *buff)
{
buff[0] = '-';
buff[1] = '\0';
uint8_t powerIndex = 0;
if (vtxCommonGetPowerIndex(vtxCommonDevice(), &powerIndex)) buff[0] = '0' + powerIndex;
}
/**
* Displays a temperature postfixed with a symbol depending on the current unit system
* @param label to display
@ -659,6 +653,8 @@ static const char * osdArmingDisabledReasonMessage(void)
return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST");
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR");
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR("JUMP WAYPOINT MISCONFIGURED");
}
#endif
break;
@ -793,11 +789,12 @@ static const char * navigationStateMessage(void)
// Used by HOLD flight modes. No information to add.
break;
case MW_NAV_STATE_HOLD_TIMED:
// Not used anymore
// TODO: Maybe we can display a count down
return OSD_MESSAGE_STR("HOLDING WAYPOINT");
break;
case MW_NAV_STATE_WP_ENROUTE:
// TODO: Show WP number
return OSD_MESSAGE_STR("EN ROUTE TO WAYPOINT");
return OSD_MESSAGE_STR("TO WP");
case MW_NAV_STATE_PROCESS_NEXT:
return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
case MW_NAV_STATE_DO_JUMP:
@ -811,7 +808,7 @@ static const char * navigationStateMessage(void)
case MW_NAV_STATE_LAND_IN_PROGRESS:
return OSD_MESSAGE_STR("LANDING");
case MW_NAV_STATE_HOVER_ABOVE_HOME:
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
}
return OSD_MESSAGE_STR("HOVERING");
@ -1644,13 +1641,14 @@ static bool osdDrawSingleElement(uint8_t item)
p = "3CRS";
else if (FLIGHT_MODE(NAV_CRUISE_MODE))
p = "CRS ";
else if (FLIGHT_MODE(NAV_WP_MODE))
p = " WP ";
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
// If navigationRequiresAngleMode() returns false when ALTHOLD is active,
// it means it can be combined with ANGLE, HORIZON, ACRO, etc...
// and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
p = " AH ";
} else if (FLIGHT_MODE(NAV_WP_MODE))
p = " WP ";
}
else if (FLIGHT_MODE(ANGLE_MODE))
p = "ANGL";
else if (FLIGHT_MODE(HORIZON_MODE))
@ -1681,34 +1679,26 @@ static bool osdDrawSingleElement(uint8_t item)
}
case OSD_VTX_CHANNEL:
#if defined(VTX)
// FIXME: This doesn't actually work. It's for boards with
// builtin VTX.
tfp_sprintf(buff, "CH:%2d", current_vtx_channel % CHANNELS_PER_BAND + 1);
#else
{
uint8_t band = 0;
uint8_t channel = 0;
char bandChr = '-';
const char *channelStr = "-";
if (vtxCommonGetBandAndChannel(vtxCommonDevice(), &band, &channel)) {
bandChr = vtx58BandLetter[band];
channelStr = vtx58ChannelNames[channel];
}
tfp_sprintf(buff, "CH:%c%s:", bandChr, channelStr);
vtxDeviceOsdInfo_t osdInfo;
vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName);
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
osdGetVTXPowerChar(buff);
tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr);
return true;
}
#endif
break;
case OSD_VTX_POWER:
{
osdGetVTXPowerChar(buff);
vtxDeviceOsdInfo_t osdInfo;
vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
return true;
@ -1725,14 +1715,18 @@ static bool osdDrawSingleElement(uint8_t item)
if (STATE(GPS_FIX) && isImuHeadingValid()) {
if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0) {
if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
osdHudClear();
}
// -------- POI : Home point
if (osdConfig()->hud_homepoint) { // Display the home point (H)
osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, 5, SYM_HOME);
osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0);
}
// -------- POI : Nearby aircrafts from ESP32 radar
if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar
for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) {
if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped
@ -1743,7 +1737,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) {
radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In °
radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100;
osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, radar_pois[i].heading, radar_pois[i].lq, 65 + i);
osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq);
}
}
}
@ -1755,6 +1749,29 @@ static bool osdDrawSingleElement(uint8_t item)
}
}
}
// -------- POI : Next waypoints from navigation
if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints
gpsLocation_t wp2;
int j;
tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
j = posControl.activeWaypointIndex + i;
if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {
wp2.lat = posControl.waypointList[j].lat;
wp2.lon = posControl.waypointList[j].lon;
wp2.alt = posControl.waypointList[j].alt;
fpVector3_t poi;
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
}
}
}
}
return true;
@ -2113,7 +2130,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
} else if (STATE(FIXED_WING) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = "AUTOLAUNCH";
} else {
if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
@ -2727,9 +2744,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->hud_homing = 0;
osdConfig->hud_homepoint = 0;
osdConfig->hud_radar_disp = 0;
osdConfig->hud_radar_range_min = 10;
osdConfig->hud_radar_range_min = 3;
osdConfig->hud_radar_range_max = 4000;
osdConfig->hud_radar_nearest = 0;
osdConfig->hud_wp_disp = 0;
osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
osdConfig->sidebar_scroll_arrows = 0;

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

@ -231,6 +231,7 @@ typedef struct osdConfig_s {
uint16_t hud_radar_range_min;
uint16_t hud_radar_range_max;
uint16_t hud_radar_nearest;
uint8_t hud_wp_disp;
uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e

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