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

Merge branch 'development' into dzikuvx-nav-cruise-improvements

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-05 15:10:49 +01:00
commit ae774017da
112 changed files with 1552 additions and 2772 deletions
Download patch file Download diff file Expand all files Collapse all files

22
docs/Cli.md
View file

@ -290,7 +290,6 @@ A shorter form is also supported to enable and disable functions using `serial <
| pos_hold_deadband | 20 | Stick deadband in [r/c points], applied after r/c deadband and expo |
| alt_hold_deadband | 50 | Defines the deadband of throttle during alt_hold [r/c points] |
| yaw_motor_direction | 1 | Use if you need to inverse yaw motor direction. |
| yaw_jump_prevention_limit | 200 | Prevent yaw jumps during yaw stops and rapid YAW input. To disable set to 500. Adjust this if your aircraft 'skids out'. Higher values increases YAW authority but can cause roll/pitch instability in case of underpowered UAVs. Lower values makes yaw adjustments more gentle but can cause UAV unable to keep heading |
| tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
| servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
| servo_center_pulse | 1500 | Servo midpoint |
@ -364,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. |
@ -420,14 +417,23 @@ A shorter form is also supported to enable and disable functions using `serial <
| acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
| acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value |
| dterm_lpf_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. |
| 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` |
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |
| 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_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 |
| 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` |
| iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |
| rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. |
| rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. |

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

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.

127
docs/development/serial_printf_debugging.md Normal file
View file

@ -0,0 +1,127 @@
# Serial printf style debugging
## Overview
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.
For example, on a VCP port.
```
serial 20 32769 115200 115200 0 115200
```
If the port is shared, it will be resused with extant settings; if the port is not shared it is opened at 921600 baud.
There are two run time settings that control the verbosity, the most verbose settings being:
```
log_level = DEBUG
Allowed values: ERROR, WARNING, INFO, VERBOSE, DEBUG
log_topics = 0
Allowed range: 0 - 4294967295
```
The use of level and topics is described in the following sections.
## LOG LEVELS
Log levels are defined in `src/main/common/log.h`, at the time of writing these include (in ascending order):
* ERROR
* WARNING
* INFO
* VERBOSE
* DEBUG
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 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
```
log_level = INFO
```
then only `ERROR`, `WARNING` and `INFO` levels will be output.
## Log Topic
Log topics are defined in `src/main/common/log.h`, at the time of writing:
* SYSTEM
* GYRO
* BARO
* PITOT
* PWM
* TIMER
* IMU
* TEMPERATURE
* POS_ESTIMATOR
* VTX
* OSD
Topics are stored as masks (SYSTEM=1 ... OSD=1024) and may be used to unconditionally display log messages.
If the CLI `log_topics` is non-zero, then all topics matching the mask will be displayed regardless of `log_level`. Setting `log_topics` to 4294967295 (all bits set) will display all log messages regardless of run time level (but still constrained by compile time settings), so F3 will still only display ERROR level messages.
## Code usage
A set of macros `LOG_S()` (log system) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
```
// LOG_D(topic, fmt, ...)
LOG_D(SYSTEM, "This is %s topic debug message, value %d", "system", 42);
```
It is also possible to dump a hex representation of arbitrary data:
```
// LOG_BUF_D(topic, buf, size)
struct {...} tstruct;
...
LOG_BUF_D(TEMPERATURE, &tstruct, sizeof(tstruct));
```
## Output Support
Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messages (`MSP_DEBUGMSG`). It is possible to use any serial terminal to display these messages, however it is advisable to use an application that understands `MSP_DEBUGMSG` in order to maintain readability (in a raw serial terminal the MSP message envelope may result in the display of strange characters). `MSP_DEBUGMSG` aware applications include:
* msp-tool https://github.com/fiam/msp-tool
* mwp https://github.com/stronnag/mwptools
* inav Configurator
For example, with the final lines of `src/main/fc/fc_init.c` set to:
```
LOG_E(SYSTEM, "Init is complete");
systemState |= SYSTEM_STATE_READY;
```
and the following CLI settings:
```
serial 20 32769 115200 115200 0 115200
set log_level = DEBUG
set log_topics = 4294967295
```
The output will be formatted as follows:
```
# msp-tool
[DEBUG] [ 3.967] Init is complete
# mwp (stderr log file)
2020-02-02T19:09:02+0000 DEBUG:[ 3.968] Init is complete
```
The numeric value in square brackets is the FC uptime in seconds.

2
make/release.mk
View file

@ -25,7 +25,7 @@ RELEASE_TARGETS += NOX WINGFC
RELEASE_TARGETS += OMNIBUSF4V6
RELEASE_TARGETS += MAMBAF405
RELEASE_TARGETS += MAMBAF405 MAMBAF405US MAMBAF722
RELEASE_TARGETS += IFLIGHTF7_TWING IFLIGHTF4_TWING

5
make/source.mk
View file

@ -98,6 +98,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 \
@ -161,9 +162,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 \

9
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

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;

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

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

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

@ -271,6 +271,10 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE),
#endif
#ifdef USE_ESC_SENSOR
OSD_ELEMENT_ENTRY("ESC RPM", OSD_ESC_RPM),
#endif
OSD_BACK_AND_END_ENTRY,
};

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

@ -15,132 +15,247 @@
* 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 = vtxSettingsConfig()->lowPowerDisarm ? 2 : 1;
}
}
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;
vtxSettingsConfigMutable()->lowPowerDisarm = (vtxPitMode == 2 ? 1 : 0);
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/global_functions.c
View file

@ -38,7 +38,7 @@
#include "common/axis.h"
PG_REGISTER_ARRAY(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
PG_REGISTER_ARRAY_WITH_RESET_FN(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
EXTENDED_FASTRAM uint64_t globalFunctionsFlags = 0;
EXTENDED_FASTRAM globalFunctionState_t globalFunctionsStates[MAX_GLOBAL_FUNCTIONS];

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

@ -40,26 +40,10 @@
#include "sensors/pitotmeter.h"
#include "flight/imu.h"
PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
void pgResetFn_logicConditions(logicCondition_t *instance)
{
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
RESET_CONFIG(logicCondition_t, &instance[i],
.enabled = 0,
.operation = LOGIC_CONDITION_TRUE,
.operandA = {
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
.value = 0
},
.operandB = {
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
.value = 0
},
.flags = 0
);
}
}
PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
@ -251,6 +235,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

@ -56,23 +56,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 {

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

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

@ -49,9 +49,9 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
}
// cycles per microsecond
STATIC_UNIT_TESTED timeUs_t usTicks = 0;
STATIC_UNIT_TESTED EXTENDED_FASTRAM timeUs_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 EXTENDED_FASTRAM volatile timeMs_t sysTickUptime = 0;
STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
// cached value of RCC->CSR
uint32_t cachedRccCsrValue;
@ -76,7 +76,7 @@ void cycleCounterInit(void)
// SysTick
static volatile int sysTickPending = 0;
static EXTENDED_FASTRAM volatile int sysTickPending = 0;
void SysTick_Handler(void)
{

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

46
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
@ -53,13 +46,6 @@
#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)
#endif
// check value for MSP_SET_VTX_CONFIG to determine if value is encoded
@ -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

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

@ -67,6 +67,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"
@ -145,8 +146,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",
@ -1321,7 +1322,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)) break;
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.waypointCount = i + 1;
posControl.waypointListValid = true;
@ -1907,7 +1908,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 &&
@ -2975,6 +2976,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());
@ -3146,13 +3170,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

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

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

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

@ -515,7 +515,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);
}
@ -532,7 +532,7 @@ void processRx(timeUs_t currentTimeUs)
const throttleStatus_e throttleStatus = calculateThrottleStatus();
// 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 +633,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 +649,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 +753,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 +782,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()) {

41
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

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

@ -153,26 +153,74 @@ typedef enum {
MSP_FLASHFS_BIT_SUPPORTED = 2
} mspFlashfsFlags_e;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#define ESC_4WAY 0xff
typedef enum {
MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
static uint8_t escMode;
static uint8_t escPortIndex;
MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
} mspPassthroughType_e;
static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
static uint8_t mspPassthroughMode;
static uint8_t mspPassthroughArgument;
static serialPort_t *mspFindPassthroughSerialPort(void)
{
serialPortUsage_t *portUsage = NULL;
switch (mspPassthroughMode) {
case MSP_PASSTHROUGH_SERIAL_ID:
{
portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument);
break;
}
case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
{
const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument);
if (portConfig) {
portUsage = findSerialPortUsageByIdentifier(portConfig->identifier);
}
break;
}
}
return portUsage ? portUsage->serialPort : NULL;
}
static void mspSerialPassthroughFn(serialPort_t *serialPort)
{
serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
if (passthroughPort && serialPort) {
serialPassthrough(passthroughPort, serialPort, NULL, NULL);
}
}
static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
{
const unsigned int dataSize = sbufBytesRemaining(src);
if (dataSize == 0) {
// Legacy format
escMode = ESC_4WAY;
mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY;
} else {
escMode = sbufReadU8(src);
escPortIndex = sbufReadU8(src);
mspPassthroughMode = sbufReadU8(src);
if (!sbufReadU8Safe(&mspPassthroughArgument, src)) {
mspPassthroughArgument = 0;
}
}
switch (escMode) {
case ESC_4WAY:
switch (mspPassthroughMode) {
case MSP_PASSTHROUGH_SERIAL_ID:
case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
if (mspFindPassthroughSerialPort()) {
if (mspPostProcessFn) {
*mspPostProcessFn = mspSerialPassthroughFn;
}
sbufWriteU8(dst, 1);
} else {
sbufWriteU8(dst, 0);
}
break;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
case MSP_PASSTHROUGH_ESC_4WAY:
// get channel number
// switch all motor lines HI
// reply with the count of ESC found
@ -182,14 +230,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
*mspPostProcessFn = esc4wayProcess;
}
break;
#endif
default:
sbufWriteU8(dst, 0);
}
}
#endif
static void mspRebootFn(serialPort_t *serialPort)
{
UNUSED(serialPort);
@ -1064,16 +1110,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:
@ -1373,7 +1419,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);
@ -1936,9 +1982,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;
@ -1948,7 +1994,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;
@ -2331,11 +2377,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) {
@ -2703,7 +2744,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);
@ -3177,11 +3218,9 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = mspProcessSensorCommand(cmdMSP, src);
} else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
ret = MSP_RESULT_ACK;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
} else if (cmdMSP == MSP_SET_4WAY_IF) {
mspFc4waySerialCommand(dst, src, mspPostProcessFn);
} else if (cmdMSP == MSP_SET_PASSTHROUGH) {
mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
ret = MSP_RESULT_ACK;
#endif
} else {
if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) {
ret = mspFcProcessInCommand(cmdMSP, src);

27
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;
}
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) {
if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) {
if (!STATE(ROVER) && !STATE(BOAT)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
if (STATE(FIXED_WING)) {
}
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;
}

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

@ -76,7 +76,7 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.yaw_deadband = 5,
.pos_hold_deadband = 20,
.alt_hold_deadband = 50,
.deadband3d_throttle = 50,
.mid_throttle_deadband = 50,
.airmodeHandlingType = STICK_CENTER,
.airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD,
);
@ -101,10 +101,10 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
throttleStatus_e calculateThrottleStatus(void)
{
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)))
const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
return THROTTLE_LOW;
else if (!feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
return THROTTLE_LOW;
return THROTTLE_HIGH;
@ -183,7 +183,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();

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

@ -80,7 +80,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;

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

9
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,13 @@ 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),
FW_HEADING_USE_YAW = (1 << 23),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

103
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,22 +186,21 @@ 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
max: 1000
@ -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
@ -896,21 +892,6 @@ groups:
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
@ -958,7 +939,7 @@ groups:
min: 10
max: 250
- name: 3d_deadband_throttle
field: deadband3d_throttle
field: mid_throttle_deadband
min: 0
max: 200
- name: mc_airmode_type
@ -1250,6 +1231,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: pidSumLimitYaw
min: PID_SUM_LIMIT_MIN
max: PID_SUM_LIMIT_MAX
- name: mc_iterm_relax_type
field: iterm_relax_type
table: iterm_relax_type
@ -1666,6 +1666,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
@ -1703,8 +1710,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
@ -2092,16 +2099,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);

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

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

@ -35,6 +35,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 +56,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 +97,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 +111,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 +144,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 +158,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 +220,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 +257,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 +278,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 +296,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 +306,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

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

@ -51,13 +51,44 @@ typedef struct gyroAnalyseState_s {
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
uint16_t centerFreq[XYZ_AXIS_COUNT];
uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
/*
* Extended Dynamic Filters are 3x3 filter matrix
* In this approach, we assume that vibration peak on one axis
* can be also detected on other axises, but with lower amplitude
* that causes this freqency not to be attenuated.
*
* This approach is similiar to the approach on RPM filter when motor base
* frequency is attenuated on every axis even tho it might not be appearing
* in gyro traces
*
* extendedDynamicFilter[GYRO_AXIS][ANALYZED_AXIS]
*
*/
biquadFilter_t extendedDynamicFilter[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
filterApplyFnPtr extendedDynamicFilterApplyFn;
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];

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

@ -481,7 +481,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 +499,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 +532,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)
@ -670,7 +670,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)

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

@ -62,19 +62,21 @@ 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 motorValueWhenStopped = 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
@ -147,13 +149,35 @@ 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) {
@ -172,7 +196,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,7 +235,7 @@ 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;
}
@ -222,13 +246,27 @@ void mixerInit(void)
} else {
motorRateLimitingApplyFn = nullMotorRateLimiting;
}
if (mixerConfig()->motorDirectionInverted) {
motorYawMultiplier = -1;
} else {
motorYawMultiplier = 1;
}
}
void mixerResetDisarmedMotors(void)
{
const int motorZeroCommand = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
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;
}
}
@ -241,13 +279,13 @@ 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);
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (motor[i] >= throttleIdleValue && motor[i] <= reversibleMotorsConfig()->deadband_low) {
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, reversibleMotorsConfig()->deadband_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);
else if (motor[i] >= reversibleMotorsConfig()->deadband_high && motor[i] <= motorConfig()->maxthrottle) {
motorValue = scaleRangef(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
}
else {
@ -287,7 +325,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.
}
@ -301,7 +339,7 @@ void FAST_CODE NOINLINE 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 +361,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];
@ -342,23 +380,23 @@ void FAST_CODE NOINLINE mixTable(const float dT)
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax);
} else
#endif
if (feature(FEATURE_3D)) {
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig()->deadband3d_low;
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
throttleMax = reversibleMotorsConfig()->deadband_low;
throttleMin = throttleIdleValue;
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband)) { // Positive handling
throttleMax = motorConfig()->maxthrottle;
throttleMin = flight3DConfig()->deadband3d_high;
throttleMin = reversibleMotorsConfig()->deadband_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;
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Deadband handling from negative to positive
mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
throttleMin = throttleIdleValue;
} else { // Deadband handling from positive to negative
throttleMax = motorConfig()->maxthrottle;
mixerThrottleCommand = throttleMin = flight3DConfig()->deadband3d_high;
mixerThrottleCommand = throttleMin = reversibleMotorsConfig()->deadband_high;
}
} else {
mixerThrottleCommand = rcCommand[THROTTLE];
@ -397,28 +435,25 @@ 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 if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)) {
motor[i] = constrain(motor[i], throttleIdleValue, reversibleMotorsConfig()->deadband_low);
} else {
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
motor[i] = constrain(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
}
} else {
motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);
}
// 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 +472,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() == THROTTLE_LOW) {
if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
return MOTOR_STOPPED_USER;
}
}

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

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

@ -118,7 +118,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 +185,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 +218,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
}
}
},
@ -259,6 +270,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)
@ -533,7 +545,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]);
@ -646,7 +658,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;
@ -710,10 +722,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);
@ -864,7 +872,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)
@ -908,7 +916,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 {
@ -1021,7 +1029,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;
}
@ -1045,7 +1053,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);
@ -1075,7 +1082,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;

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;
@ -150,6 +151,8 @@ typedef struct pidProfile_s {
float antigravityGain;
float antigravityAccelerator;
uint8_t antigravityCutoff;
int navFwPosHdgPidsumLimit;
} pidProfile_t;
typedef struct pidAutotuneConfig_s {

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

@ -43,17 +43,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 +66,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)
{
@ -141,7 +134,6 @@ 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)
@ -172,7 +164,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,17 +175,6 @@ 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)
@ -215,7 +195,6 @@ void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
}
rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
rpmDtermUpdateFn(&dtermRpmFilters, i, baseFrequency);
}
}
@ -224,9 +203,4 @@ float FAST_CODE 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

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

@ -50,4 +50,3 @@ 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);

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

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

@ -247,7 +247,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;
}

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

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

@ -530,14 +530,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 +651,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;
@ -811,7 +805,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");
@ -1681,34 +1675,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;
@ -2113,7 +2099,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()) {

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

@ -116,7 +116,7 @@ void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, cons
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
displayCanvasCtmRotate(canvas, -DEGREES_TO_RADIANS(degrees));
displayCanvasCtmRotate(canvas, -DEGREES_TO_RADIANS(180 + degrees));
displayCanvasCtmTranslate(canvas, px + canvas->gridElementWidth / 2, py + canvas->gridElementHeight / 2);
displayCanvasFillStrokeTriangle(canvas, 0, 6, 5, -6, -5, -6);
displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);

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

@ -397,8 +397,11 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
break;
case DJI_MSP_NAME:
for (const char * name = systemConfig()->name; *name; name++) {
sbufWriteU8(dst, *name++);
{
const char * name = systemConfig()->name;
int len = strlen(name);
if (len > 12) len = 12;
sbufWriteData(dst, name, len);
}
break;

166
src/main/io/vtx.c
View file

@ -42,14 +42,13 @@
#include "io/vtx_string.h"
#include "io/vtx_control.h"
PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 1);
PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
.band = VTX_SETTINGS_DEFAULT_BAND,
.channel = VTX_SETTINGS_DEFAULT_CHANNEL,
.power = VTX_SETTINGS_DEFAULT_POWER,
.freq = VTX_SETTINGS_DEFAULT_FREQ,
.pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ,
.pitModeChan = VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL,
.lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF,
);
@ -63,51 +62,17 @@ typedef enum {
void vtxInit(void)
{
bool settingsUpdated = false;
// sync frequency in parameter group when band/channel are specified
const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) {
vtxSettingsConfigMutable()->freq = freq;
settingsUpdated = true;
}
#if defined(VTX_SETTINGS_FREQCMD)
// constrain pit mode frequency
if (vtxSettingsConfig()->pitModeFreq) {
const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_SETTINGS_MIN_USER_FREQ);
if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) {
vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq;
settingsUpdated = true;
}
}
#endif
if (settingsUpdated) {
saveConfigAndNotify();
}
}
static vtxSettingsConfig_t vtxGetSettings(void)
static vtxSettingsConfig_t * vtxGetRuntimeSettings(void)
{
vtxSettingsConfig_t settings = {
.band = vtxSettingsConfig()->band,
.channel = vtxSettingsConfig()->channel,
.power = vtxSettingsConfig()->power,
.freq = vtxSettingsConfig()->freq,
.pitModeFreq = vtxSettingsConfig()->pitModeFreq,
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
};
static vtxSettingsConfig_t settings;
#if 0
#if defined(VTX_SETTINGS_FREQCMD)
if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) {
settings.band = 0;
settings.freq = settings.pitModeFreq;
settings.power = VTX_SETTINGS_DEFAULT_POWER;
}
#endif
#endif
settings.band = vtxSettingsConfig()->band;
settings.channel = vtxSettingsConfig()->channel;
settings.power = vtxSettingsConfig()->power;
settings.pitModeChan = vtxSettingsConfig()->pitModeChan;
settings.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm;
if (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) ||
@ -116,56 +81,46 @@ static vtxSettingsConfig_t vtxGetSettings(void)
settings.power = VTX_SETTINGS_DEFAULT_POWER;
}
return settings;
return &settings;
}
static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice)
static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{
// Shortcut for undefined band
if (!runtimeSettings->band) {
return false;
}
if(!ARMING_FLAG(ARMED)) {
uint8_t vtxBand;
uint8_t vtxChan;
if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxBand != settings.band || vtxChan != settings.channel) {
vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel);
return true;
if (!vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
return false;
}
if (vtxBand != runtimeSettings->band || vtxChan != runtimeSettings->channel) {
vtxCommonSetBandAndChannel(vtxDevice, runtimeSettings->band, runtimeSettings->channel);
return true;
}
}
return false;
}
#if defined(VTX_SETTINGS_FREQCMD)
static bool vtxProcessFrequency(vtxDevice_t *vtxDevice)
{
if(!ARMING_FLAG(ARMED)) {
uint16_t vtxFreq;
if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) {
const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxFreq != settings.freq) {
vtxCommonSetFrequency(vtxDevice, settings.freq);
return true;
}
}
}
return false;
}
#endif
static bool vtxProcessPower(vtxDevice_t *vtxDevice)
static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{
uint8_t vtxPower;
if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxPower != settings.power) {
vtxCommonSetPowerByIndex(vtxDevice, settings.power);
return true;
if (!vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
return false;
}
if (vtxPower != runtimeSettings->power) {
vtxCommonSetPowerByIndex(vtxDevice, runtimeSettings->power);
return true;
}
return false;
}
static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{
uint8_t pitOnOff;
@ -173,25 +128,10 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
static bool prevPmSwitchState = false;
if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
// Not supported on INAV yet. It might not be that useful.
#if 0
currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
#endif
if (currPmSwitchState != prevPmSwitchState) {
prevPmSwitchState = currPmSwitchState;
if (currPmSwitchState) {
#if defined(VTX_SETTINGS_FREQCMD)
if (vtxSettingsConfig()->pitModeFreq) {
return false;
}
#endif
#if 0
if (isModeActivationConditionPresent(BOXVTXPITMODE)) {
#endif
if (0) {
if (!pitOnOff) {
vtxCommonSetPitMode(vtxDevice, true);
@ -209,22 +149,18 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
return false;
}
static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice)
static bool vtxProcessCheckParameters(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{
const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings();
vtxSettingsConfig_t vtxState = vtxSettingsState;
uint8_t vtxBand;
uint8_t vtxChan;
uint8_t vtxPower;
if (vtxSettingsState.band) {
vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel);
#if defined(VTX_SETTINGS_FREQCMD)
} else {
vtxCommonGetFrequency(vtxDevice, &vtxState.freq);
#endif
}
vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan);
vtxCommonGetPowerIndex(vtxDevice, &vtxState.power);
return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t));
return (runtimeSettings->band && runtimeSettings->band != vtxBand) ||
(runtimeSettings->channel != vtxChan) ||
(runtimeSettings->power != vtxPower);
}
void vtxUpdate(timeUs_t currentTimeUs)
@ -240,36 +176,32 @@ void vtxUpdate(timeUs_t currentTimeUs)
// Check input sources for config updates
vtxControlInputPoll();
const uint8_t startingSchedule = currentSchedule;
// Build runtime settings
const vtxSettingsConfig_t * runtimeSettings = vtxGetRuntimeSettings();
bool vtxUpdatePending = false;
do {
switch (currentSchedule) {
case VTX_PARAM_POWER:
vtxUpdatePending = vtxProcessPower(vtxDevice);
vtxUpdatePending = vtxProcessPower(vtxDevice, runtimeSettings);
break;
case VTX_PARAM_BANDCHAN:
if (vtxGetSettings().band) {
vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice);
#if defined(VTX_SETTINGS_FREQCMD)
} else {
vtxUpdatePending = vtxProcessFrequency(vtxDevice);
#endif
}
vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice, runtimeSettings);
break;
case VTX_PARAM_PITMODE:
vtxUpdatePending = vtxProcessPitMode(vtxDevice);
vtxUpdatePending = vtxProcessPitMode(vtxDevice, runtimeSettings);
break;
case VTX_PARAM_CONFIRM:
vtxUpdatePending = vtxProcessStateUpdate(vtxDevice);
vtxUpdatePending = vtxProcessCheckParameters(vtxDevice, runtimeSettings);
break;
default:
break;
}
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
} while (!vtxUpdatePending && currentSchedule != startingSchedule);
if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
vtxCommonProcess(vtxDevice, currentTimeUs);
}
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
}
}

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

@ -38,8 +38,7 @@ typedef struct vtxSettingsConfig_s {
uint8_t band; // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Racebande
uint8_t channel; // 1-8
uint8_t power; // 0 = lowest
uint16_t freq; // sets freq in MHz if band=0
uint16_t pitModeFreq; // sets out-of-range pitmode frequency
uint16_t pitModeChan; // sets out-of-range pitmode frequency
uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e
} vtxSettingsConfig_t;

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

@ -42,7 +42,6 @@
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2);
PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
// .vtxChannelActivationConditions = { 0 },
.halfDuplex = true,
);

9
src/main/io/vtx_control.h
View file

@ -33,15 +33,6 @@ typedef struct vtxConfig_s {
uint8_t halfDuplex;
} vtxConfig_t;
typedef struct vtxRunState_s {
int pitMode;
int band;
int channel;
int frequency;
int powerIndex;
int powerMilliwatt;
} vtxRunState_t;
PG_DECLARE(vtxConfig_t, vtxConfig);
void vtxControlInit(void);

80
src/main/io/vtx_ffpv24g.c
View file

@ -41,8 +41,6 @@
#include "scheduler/protothreads.h"
//#include "cms/cms_menu_vtx_ffpv24g.h"
#include "io/vtx.h"
#include "io/vtx_ffpv24g.h"
#include "io/vtx_control.h"
@ -85,7 +83,6 @@ typedef struct {
// Requested VTX state
struct {
bool setByFrequency;
int band;
int channel;
unsigned freq;
@ -108,9 +105,9 @@ typedef struct {
/*****************************************************************************/
const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = {
"--------",
"FFPV 2.4 A",
"FFPV 2.4 B",
"-----",
"A 2.4",
"B 2.4",
};
const char * ffpvBandLetters = "-AB";
@ -353,18 +350,10 @@ static bool impl_DevSetFreq(uint16_t freq)
return true;
}
static void impl_SetFreq(vtxDevice_t * vtxDevice, uint16_t freq)
static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
{
UNUSED(vtxDevice);
if (impl_DevSetFreq(freq)) {
// Keep track that we set frequency directly
vtxState.request.setByFrequency = true;
}
}
void ffpvSetBandAndChannel(uint8_t band, uint8_t channel)
{
// Validate band and channel
if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) {
return;
@ -372,20 +361,12 @@ void ffpvSetBandAndChannel(uint8_t band, uint8_t channel)
if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) {
// Keep track of band/channel data
vtxState.request.setByFrequency = false;
vtxState.request.band = band;
vtxState.request.channel = channel;
}
}
static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
{
UNUSED(vtxDevice);
ffpvSetBandAndChannel(band, channel);
}
void ffpvSetRFPowerByIndex(uint16_t index)
static void ffpvSetRFPowerByIndex(uint16_t index)
{
// Validate index
if (index < 1 || index > VTX_FFPV_POWER_COUNT) {
@ -422,7 +403,7 @@ static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand,
}
// if in user-freq mode then report band as zero
*pBand = vtxState.request.setByFrequency ? 0 : vtxState.request.band;
*pBand = vtxState.request.band;
*pChannel = vtxState.request.channel;
return true;
}
@ -459,27 +440,33 @@ static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true;
}
vtxRunState_t * ffpvGetRuntimeState(void)
static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
static vtxRunState_t state;
if (!impl_IsReady(vtxDevice)) {
return false;
}
if (vtxState.ready) {
state.pitMode = 0;
state.band = vtxState.request.band;
state.channel = vtxState.request.channel;
state.frequency = vtxState.request.freq;
state.powerIndex = vtxState.request.powerIndex;
state.powerMilliwatt = vtxState.request.power;
*pIndex = vtxState.request.powerIndex;
*pPowerMw = vtxState.request.power;
return true;
}
else {
state.pitMode = 0;
state.band = 1;
state.channel = 1;
state.frequency = ffpvFrequencyTable[0][0];
state.powerIndex = 1;
state.powerMilliwatt = 25;
static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
if (!impl_IsReady(vtxDevice)) {
return false;
}
return &state;
pOsdInfo->band = vtxState.request.band;
pOsdInfo->channel = vtxState.request.channel;
pOsdInfo->frequency = vtxState.request.freq;
pOsdInfo->powerIndex = vtxState.request.powerIndex;
pOsdInfo->powerMilliwatt = vtxState.request.power;
pOsdInfo->bandName = ffpvBandNames[vtxState.request.band];
pOsdInfo->bandLetter = ffpvBandLetters[vtxState.request.band];
pOsdInfo->channelName = ffpvChannelNames[vtxState.request.channel];
pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
return true;
}
/*****************************************************************************/
@ -490,11 +477,12 @@ static const vtxVTable_t impl_vtxVTable = {
.setBandAndChannel = impl_SetBandAndChannel,
.setPowerByIndex = impl_SetPowerByIndex,
.setPitMode = impl_SetPitMode,
.setFrequency = impl_SetFreq,
.getBandAndChannel = impl_GetBandAndChannel,
.getPowerIndex = impl_GetPowerIndex,
.getPitMode = impl_GetPitMode,
.getFrequency = impl_GetFreq,
.getPower = impl_GetPower,
.getOsdInfo = impl_GetOsdInfo,
};
static vtxDevice_t impl_vtxDevice = {
@ -502,9 +490,9 @@ static vtxDevice_t impl_vtxDevice = {
.capability.bandCount = VTX_FFPV_BAND_COUNT,
.capability.channelCount = VTX_FFPV_CHANNEL_COUNT,
.capability.powerCount = VTX_FFPV_POWER_COUNT,
.bandNames = (char **)ffpvBandNames,
.channelNames = (char **)ffpvChannelNames,
.powerNames = (char **)ffpvPowerNames,
.capability.bandNames = (char **)ffpvBandNames,
.capability.channelNames = (char **)ffpvChannelNames,
.capability.powerNames = (char **)ffpvPowerNames,
};
bool vtxFuriousFPVInit(void)

10
src/main/io/vtx_ffpv24g.h
View file

@ -38,14 +38,4 @@
#define VTX_FFPV_CHANNEL_COUNT 8
#define VTX_FFPV_POWER_COUNT 4
extern const char * ffpvBandLetters;
extern const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1];
extern const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1];
extern const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1];
extern const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT];
bool vtxFuriousFPVInit(void);
void ffpvSetBandAndChannel(uint8_t band, uint8_t channel);
void ffpvSetRFPowerByIndex(uint16_t index);
vtxRunState_t * ffpvGetRuntimeState(void);

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

@ -32,7 +32,6 @@
#include "build/debug.h"
#include "cms/cms.h"
#include "cms/cms_menu_vtx_smartaudio.h"
#include "common/log.h"
#include "common/maths.h"
@ -67,9 +66,9 @@ static vtxDevice_t vtxSmartAudio = {
.capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
.capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
.bandNames = (char **)vtx58BandNames,
.channelNames = (char **)vtx58ChannelNames,
.powerNames = (char **)saPowerNames,
.capability.bandNames = (char **)vtx58BandNames,
.capability.channelNames = (char **)vtx58ChannelNames,
.capability.powerNames = (char **)saPowerNames,
};
// SmartAudio command and response codes
@ -332,7 +331,7 @@ static void saProcessResponse(uint8_t *buf, int len)
if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
#ifdef USE_CMS //if changes then trigger saCms update
saCmsResetOpmodel();
//saCmsResetOpmodel();
#endif
// Debug
saPrintSettings();
@ -341,7 +340,7 @@ static void saProcessResponse(uint8_t *buf, int len)
#ifdef USE_CMS
// Export current device status for CMS
saCmsUpdate();
//saCmsUpdate();
#endif
}
@ -540,11 +539,6 @@ static void saGetSettings(void)
saQueueCmd(bufGetSettings, 5);
}
static bool saValidateFreq(uint16_t freq)
{
return (freq >= VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ && freq <= VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ);
}
static void saDoDevSetFreq(uint16_t freq)
{
static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
@ -802,15 +796,6 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
return;
}
static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{
UNUSED(vtxDevice);
if (saValidateFreq(freq)) {
saSetMode(0); //need to be in FREE mode to set freq
saSetFreq(freq);
}
}
static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
if (!vtxSAIsReady(vtxDevice)) {
@ -857,6 +842,50 @@ static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true;
}
static bool vtxSAGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
uint8_t powerIndex;
if (!vtxSAGetPowerIndex(vtxDevice, &powerIndex)) {
return false;
}
*pIndex = powerIndex;
*pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex-1].rfpower : 0;
return true;
}
static bool vtxSAGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
uint8_t powerIndex;
uint16_t powerMw;
uint16_t freq;
uint8_t band, channel;
if (!vtxSAGetBandAndChannel(vtxDevice, &band, &channel)) {
return false;
}
if (!vtxSAGetFreq(vtxDevice, &freq)) {
return false;
}
if (!vtxSAGetPower(vtxDevice, &powerIndex, &powerMw)) {
return false;
}
pOsdInfo->band = band;
pOsdInfo->channel = channel;
pOsdInfo->frequency = freq;
pOsdInfo->powerIndex = powerIndex;
pOsdInfo->powerMilliwatt = powerMw;
pOsdInfo->bandLetter = vtx58BandNames[band][0];
pOsdInfo->bandName = vtx58BandNames[band];
pOsdInfo->channelName = vtx58ChannelNames[channel];
pOsdInfo->powerIndexLetter = '0' + powerIndex;
return true;
}
static const vtxVTable_t saVTable = {
.process = vtxSAProcess,
.getDeviceType = vtxSAGetDeviceType,
@ -864,11 +893,12 @@ static const vtxVTable_t saVTable = {
.setBandAndChannel = vtxSASetBandAndChannel,
.setPowerByIndex = vtxSASetPowerByIndex,
.setPitMode = vtxSASetPitMode,
.setFrequency = vtxSASetFreq,
.getBandAndChannel = vtxSAGetBandAndChannel,
.getPowerIndex = vtxSAGetPowerIndex,
.getPitMode = vtxSAGetPitMode,
.getFrequency = vtxSAGetFreq,
.getPower = vtxSAGetPower,
.getOsdInfo = vtxSAGetOsdInfo,
};

5
src/main/io/vtx_string.c
View file

@ -27,6 +27,7 @@
#define VTX_STRING_BAND_COUNT 5
#define VTX_STRING_CHAN_COUNT 8
#define VTX_STRING_POWER_COUNT 5
const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] =
{
@ -52,6 +53,10 @@ const char * const vtx58ChannelNames[VTX_STRING_CHAN_COUNT + 1] = {
"-", "1", "2", "3", "4", "5", "6", "7", "8",
};
const char * const vtx58DefaultPowerNames[VTX_STRING_POWER_COUNT + 1] = {
"---", "PL1", "PL2", "PL3", "PL4", "PL5"
};
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
{
int8_t band;

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

@ -5,6 +5,7 @@
extern const uint16_t vtx58frequencyTable[5][8];
extern const char * const vtx58BandNames[];
extern const char * const vtx58ChannelNames[];
extern const char * const vtx58DefaultPowerNames[];
extern const char vtx58BandLetter[];
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel);

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

@ -34,8 +34,6 @@
#include "common/maths.h"
#include "common/utils.h"
#include "cms/cms_menu_vtx_tramp.h"
#include "drivers/vtx_common.h"
#include "io/serial.h"
@ -44,7 +42,6 @@
#include "io/vtx.h"
#include "io/vtx_string.h"
#if defined(USE_CMS)
const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
25, 100, 200, 400, 600
};
@ -52,7 +49,6 @@ const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
"---", "25 ", "100", "200", "400", "600"
};
#endif
static const vtxVTable_t trampVTable; // forward
static vtxDevice_t vtxTramp = {
@ -60,9 +56,9 @@ static vtxDevice_t vtxTramp = {
.capability.bandCount = VTX_TRAMP_BAND_COUNT,
.capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
.capability.powerCount = VTX_TRAMP_POWER_COUNT,
.bandNames = (char **)vtx58BandNames,
.channelNames = (char **)vtx58ChannelNames,
.powerNames = (char **)trampPowerNames,
.capability.bandNames = (char **)vtx58BandNames,
.capability.channelNames = (char **)vtx58ChannelNames,
.capability.powerNames = (char **)trampPowerNames,
};
static serialPort_t *trampSerialPort = NULL;
@ -126,11 +122,6 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
trampWriteBuf(trampReqBuffer);
}
static bool trampValidateFreq(uint16_t freq)
{
return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
}
static void trampDevSetFreq(uint16_t freq)
{
trampConfFreq = freq;
@ -511,15 +502,6 @@ static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
trampSetPitMode(onoff);
}
static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{
UNUSED(vtxDevice);
if (trampValidateFreq(freq)) {
trampSetFreq(freq);
trampCommitChanges();
}
}
static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
if (!vtxTrampIsReady(vtxDevice)) {
@ -570,6 +552,40 @@ static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true;
}
static bool vtxTrampGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
uint8_t powerIndex;
if (!vtxTrampGetPowerIndex(vtxDevice, &powerIndex)) {
return false;
}
*pIndex = trampData.configuredPower ? powerIndex : 0;
*pPowerMw = trampData.configuredPower;
return true;
}
static bool vtxTrampGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
uint8_t powerIndex;
uint16_t powerMw;
if (!vtxTrampGetPower(vtxDevice, &powerIndex, &powerMw)) {
return false;
}
pOsdInfo->band = trampData.setByFreqFlag ? 0 : trampData.band;
pOsdInfo->channel = trampData.channel;
pOsdInfo->frequency = trampData.curFreq;
pOsdInfo->powerIndex = powerIndex;
pOsdInfo->powerMilliwatt = powerMw;
pOsdInfo->bandLetter = vtx58BandNames[pOsdInfo->band][0];
pOsdInfo->bandName = vtx58BandNames[pOsdInfo->band];
pOsdInfo->channelName = vtx58ChannelNames[pOsdInfo->channel];
pOsdInfo->powerIndexLetter = '0' + powerIndex;
return true;
}
static const vtxVTable_t trampVTable = {
.process = vtxTrampProcess,
.getDeviceType = vtxTrampGetDeviceType,
@ -577,11 +593,12 @@ static const vtxVTable_t trampVTable = {
.setBandAndChannel = vtxTrampSetBandAndChannel,
.setPowerByIndex = vtxTrampSetPowerByIndex,
.setPitMode = vtxTrampSetPitMode,
.setFrequency = vtxTrampSetFreq,
.getBandAndChannel = vtxTrampGetBandAndChannel,
.getPowerIndex = vtxTrampGetPowerIndex,
.getPitMode = vtxTrampGetPitMode,
.getFrequency = vtxTrampGetFreq,
.getPower = vtxTrampGetPower,
.getOsdInfo = vtxTrampGetOsdInfo,
};

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

@ -321,7 +321,7 @@
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...)
#define MSP_RTC 246 //out message Gets the RTC clock (returns: secs(i32) millis(u16) - (0,0) if time is not known)
#define MSP_SET_RTC 247 //in message Sets the RTC clock (args: secs(i32) millis(u16))

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

@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -165,7 +165,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_climb_angle = 18, // 18 degrees
.launch_max_angle = 45, // 45 deg
.cruise_yaw_rate = 20, // 20dps
.allow_manual_thr_increase = false
.allow_manual_thr_increase = false,
.useFwNavYawControl = 0,
.yawControlDeadband = 0,
}
);
@ -617,12 +619,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 0,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
@ -841,7 +844,7 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void)
static bool navTerrainFollowingRequested(void)
{
// Terrain following not supported on FIXED WING aircraft yet
return !STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXSURFACE);
return !STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXSURFACE);
}
/*************************************************************************************************/
@ -939,7 +942,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
{
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
DEBUG_SET(DEBUG_CRUISE, 0, 1);
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration.
@ -1018,7 +1021,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState)
{
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
@ -1049,7 +1052,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
// Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -1078,7 +1081,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
else {
fpVector3_t targetHoldPos;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
} else {
@ -1115,14 +1118,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
// If we have valid pos sensor OR we are configured to ignore GPS loss
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
}
@ -1130,7 +1133,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
posControl.rthState.rthInitialDistance = posControl.homeDistance;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
@ -1144,12 +1147,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
/* For multi-rotors execute sanity check during initial ascent as well */
if (!STATE(FIXED_WING) && !validateRTHSanityChecker()) {
if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
@ -1222,7 +1225,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
// Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@ -1367,20 +1370,46 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
/* A helper function to do waypoint-specific action */
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// JUMP is the last waypoint and we reached the last jump, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished JUMP, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p2--;
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_RTH:
default:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
};
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
@ -1389,9 +1418,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
default:
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
@ -1407,6 +1435,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
}
break;
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
@ -1426,13 +1457,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
else {
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
{
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1440,10 +1479,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
default:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
@ -1673,7 +1711,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
case RTH_HOME_ENROUTE_PROPORTIONAL:
{
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
if (rthTotalDistanceToTravel >= 100) {
float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
@ -1786,6 +1824,11 @@ float navPidApply3(
}
}
/*
* Limit both output and Iterm to limit windup
*/
pid->integrator = constrain(pid->integrator, outMin, outMax);
return outValConstrained;
}
@ -2063,7 +2106,7 @@ static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWayp
// We consider waypoint reached if within specified radius
posControl.wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING) && isWaypointHome) {
if (STATE(FIXED_WING_LEGACY) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|| (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
@ -2322,7 +2365,7 @@ uint32_t getTotalTravelDistance(void)
*-----------------------------------------------------------*/
void calculateInitialHoldPosition(fpVector3_t * pos)
{
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
calculateFixedWingInitialHoldPosition(pos);
}
else {
@ -2379,7 +2422,7 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc
*-----------------------------------------------------------*/
void resetLandingDetector(void)
{
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
resetFixedWingLandingDetector();
}
else {
@ -2391,7 +2434,7 @@ bool isLandingDetected(void)
{
bool landingDetected;
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
landingDetected = isFixedWingLandingDetected();
}
else {
@ -2417,7 +2460,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
posControl.desiredState.pos.z = altitudeToUse;
}
else {
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
@ -2440,7 +2483,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
// Set terrain following flag
posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingAltitudeController();
}
else {
@ -2450,7 +2493,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
static void setupAltitudeController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
setupFixedWingAltitudeController();
}
else {
@ -2460,7 +2503,7 @@ static void setupAltitudeController(void)
static bool adjustAltitudeFromRCInput(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return adjustFixedWingAltitudeFromRCInput();
}
else {
@ -2473,7 +2516,7 @@ static bool adjustAltitudeFromRCInput(void)
*-----------------------------------------------------------*/
static void resetHeadingController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingHeadingController();
}
else {
@ -2483,7 +2526,7 @@ static void resetHeadingController(void)
static bool adjustHeadingFromRCInput(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return adjustFixedWingHeadingFromRCInput();
}
else {
@ -2496,7 +2539,7 @@ static bool adjustHeadingFromRCInput(void)
*-----------------------------------------------------------*/
static void resetPositionController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingPositionController();
}
else {
@ -2509,7 +2552,7 @@ static bool adjustPositionFromRCInput(void)
{
bool retValue;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
retValue = adjustFixedWingPositionFromRCInput();
}
else {
@ -2607,9 +2650,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
}
// WP #1 - #15 - common waypoints - pre-programmed mission
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_RTH) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;
@ -2777,7 +2820,7 @@ static void processNavigationRCAdjustments(void)
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
}
else {
if (!STATE(FIXED_WING)) {
if (!STATE(FIXED_WING_LEGACY)) {
resetMulticopterBrakingMode();
}
}
@ -2829,7 +2872,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
/* Process controllers */
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
applyFixedWingNavigationController(navStateFlags, currentTimeUs);
}
else {
@ -2902,7 +2945,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
bool canActivateNavigation = canActivateNavigationModes();
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
if (canActivateLaunchMode) {
canActivateLaunchMode = false;
@ -2985,7 +3028,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
*-----------------------------------------------------------*/
bool navigationRequiresThrottleTiltCompensation(void)
{
return !STATE(FIXED_WING) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
}
/*-----------------------------------------------------------
@ -2994,7 +3037,7 @@ bool navigationRequiresThrottleTiltCompensation(void)
bool navigationRequiresAngleMode(void)
{
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
}
/*-----------------------------------------------------------
@ -3003,7 +3046,7 @@ bool navigationRequiresAngleMode(void)
bool navigationRequiresTurnAssistance(void)
{
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// For airplanes turn assistant is always required when controlling position
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
}
@ -3018,7 +3061,7 @@ bool navigationRequiresTurnAssistance(void)
int8_t navigationGetHeadingControlState(void)
{
// For airplanes report as manual heading control
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return NAV_HEADING_CONTROL_MANUAL;
}
@ -3043,7 +3086,7 @@ bool navigationTerrainFollowingEnabled(void)
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
if (usedBypass) {
@ -3083,6 +3126,17 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
}
// Don't allow arming if any of JUMP waypoint has invalid settings
if (posControl.waypointCount > 0) {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
}
}
}
}
return NAV_ARMING_BLOCKER_NONE;
}
@ -3217,6 +3271,13 @@ void navigationUsePIDs(void)
0.0f,
NAV_DTERM_CUT_HZ
);
navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
0.0f,
2.0f
);
}
void navigationInit(void)
@ -3247,6 +3308,16 @@ void navigationInit(void)
/* Use system config */
navigationUsePIDs();
if (
mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER ||
navConfig()->fw.useFwNavYawControl
) {
ENABLE_STATE(FW_HEADING_USE_YAW);
} else {
DISABLE_STATE(FW_HEADING_USE_YAW);
}
}
/*-----------------------------------------------------------

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

@ -101,6 +101,7 @@ typedef enum {
NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1,
NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2,
NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3,
NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4,
} navArmingBlocker_e;
typedef struct positionEstimationConfig_s {
@ -216,6 +217,8 @@ typedef struct navConfig_s {
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase;
bool useFwNavYawControl;
uint8_t yawControlDeadband;
} fw;
} navConfig_t;
@ -231,6 +234,7 @@ typedef struct gpsOrigin_s {
typedef enum {
NAV_WP_ACTION_WAYPOINT = 0x01,
NAV_WP_ACTION_JUMP = 0x06,
NAV_WP_ACTION_RTH = 0x04
} navWaypointActions_e;
@ -308,6 +312,7 @@ typedef struct navigationPIDControllers_s {
/* Fixed-wing PIDs */
pidController_t fw_alt;
pidController_t fw_nav;
pidController_t fw_heading;
} navigationPIDControllers_t;
/* MultiWii-compatible params for telemetry */

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

@ -61,6 +61,7 @@
static bool isPitchAdjustmentValid = false;
static bool isRollAdjustmentValid = false;
static bool isYawAdjustmentValid = false;
static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
@ -209,8 +210,11 @@ void resetFixedWingPositionController(void)
virtualDesiredPosition.z = 0;
navPidReset(&posControl.pids.fw_nav);
navPidReset(&posControl.pids.fw_heading);
posControl.rcAdjustment[ROLL] = 0;
posControl.rcAdjustment[YAW] = 0;
isRollAdjustmentValid = false;
isYawAdjustmentValid = false;
pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
}
@ -292,7 +296,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// We have virtual position target, calculate heading error
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
// Calculate NAV heading error
/*
* Calculate NAV heading error
* Units are centidegrees
*/
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
// Forced turn direction
@ -333,6 +340,41 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
/*
* Yaw adjustment
* It is working in relative mode and we aim to keep error at zero
*/
if (STATE(FW_HEADING_USE_YAW)) {
static float limit = 0.0f;
if (limit == 0.0f) {
limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
}
const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
float yawAdjustment = navPidApply2(
&posControl.pids.fw_heading,
0,
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
US2S(deltaMicros),
-limit,
limit,
yawPidFlags
) / 100.0f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
posControl.rcAdjustment[YAW] = yawAdjustment;
} else {
posControl.rcAdjustment[YAW] = 0;
}
}
void applyFixedWingPositionController(timeUs_t currentTimeUs)
@ -376,10 +418,12 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
}
isRollAdjustmentValid = true;
isYawAdjustmentValid = true;
}
else {
// No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
isRollAdjustmentValid = false;
isYawAdjustmentValid = false;
}
}
@ -448,6 +492,10 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
}
if (isYawAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rcCommand[YAW] = posControl.rcAdjustment[YAW];
}
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH >0 dive, <0 climb
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));

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

@ -581,7 +581,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
return true;
}
else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) {
else if (STATE(FIXED_WING_LEGACY) && (ctx->newFlags & EST_GPS_Z_VALID)) {
// If baro is not available - use GPS Z for correction on a plane
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {

2
src/main/rx/msp_override.c
View file

@ -75,7 +75,7 @@ void mspOverrideInit(void)
mspRcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
}
mspRcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
mspRcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
mspRcChannels[THROTTLE].data = mspRcChannels[THROTTLE].raw;
// Initialize ARM switch to OFF position when arming via switch is defined

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

@ -104,7 +104,7 @@ static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
rxRuntimeConfig_t rxRuntimeConfig;
static uint8_t rcSampleIndex = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 8);
PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 9);
#ifndef RX_SPI_DEFAULT_PROTOCOL
#define RX_SPI_DEFAULT_PROTOCOL 0
@ -262,7 +262,7 @@ void rxInit(void)
rcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
}
rcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
rcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
rcChannels[THROTTLE].data = rcChannels[THROTTLE].raw;
// Initialize ARM switch to OFF position when arming via switch is defined
@ -327,7 +327,6 @@ void rxInit(void)
default:
case RX_TYPE_NONE:
case RX_TYPE_PWM:
rxConfigMutable()->receiverType = RX_TYPE_NONE;
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;

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

@ -59,12 +59,11 @@ typedef enum {
typedef enum {
RX_TYPE_NONE = 0,
RX_TYPE_PWM = 1,
RX_TYPE_PPM = 2,
RX_TYPE_SERIAL = 3,
RX_TYPE_MSP = 4,
RX_TYPE_SPI = 5,
RX_TYPE_UIB = 6
RX_TYPE_PPM = 1,
RX_TYPE_SERIAL = 2,
RX_TYPE_MSP = 3,
RX_TYPE_SPI = 4,
RX_TYPE_UIB = 5
} rxReceiverType_e;
typedef enum {

2
src/main/sensors/esc_sensor.h
View file

@ -29,7 +29,7 @@ typedef struct {
int8_t temperature;
int16_t voltage;
int32_t current;
int16_t rpm;
uint32_t rpm;
} escSensorData_t;
typedef struct escSensorConfig_s {

75
src/main/sensors/gyro.c
View file

@ -69,6 +69,7 @@
#include "flight/gyroanalyse.h"
#include "flight/rpm_filter.h"
#include "flight/dynamic_gyro_notch.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@ -95,17 +96,12 @@ STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
#ifdef USE_DYNAMIC_FILTERS
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn2;
static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros
@ -122,10 +118,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_2 = 1,
.gyro_stage2_lowpass_hz = 0,
.gyro_stage2_lowpass_type = FILTER_BIQUAD,
.dyn_notch_width_percent = 8,
.dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM,
.dyn_notch_q = 120,
.dyn_notch_min_hz = 150,
.dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
.dynamicGyroNotchQ = 120,
.dynamicGyroNotchMinHz = 150,
.dynamicGyroNotchEnabled = 0
);
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@ -265,33 +261,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
return gyroHardware;
}
#ifdef USE_DYNAMIC_FILTERS
bool isDynamicFilterActive(void)
{
return feature(FEATURE_DYNAMIC_FILTERS);
}
static void gyroInitFilterDynamicNotch(void)
{
notchFilterDynApplyFn = nullFilterApply;
notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) {
notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
if(gyroConfig()->dyn_notch_width_percent != 0) {
notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
}
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
biquadFilterInit(&notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
}
}
}
#endif
bool gyroInit(void)
{
memset(&gyro, 0, sizeof(gyro));
@ -322,8 +291,13 @@ bool gyroInit(void)
gyroInitFilters();
#ifdef USE_DYNAMIC_FILTERS
gyroInitFilterDynamicNotch();
gyroDataAnalyseStateInit(&gyroAnalyseState, getLooptime());
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
gyroDataAnalyseStateInit(
&gyroAnalyseState,
gyroConfig()->dynamicGyroNotchMinHz,
gyroConfig()->dynamicGyroNotchRange,
getLooptime()
);
#endif
return true;
}
@ -468,18 +442,27 @@ void FAST_CODE NOINLINE gyroUpdate()
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
#ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) {
if (dynamicGyroNotchState.enabled) {
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf);
gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf);
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis, gyroADCf);
gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis + 3, gyroADCf);
}
#endif
gyro.gyroADCf[axis] = gyroADCf;
}
#ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2);
if (dynamicGyroNotchState.enabled) {
gyroDataAnalyse(&gyroAnalyseState);
if (gyroAnalyseState.filterUpdateExecute) {
dynamicGyroNotchFiltersUpdate(
&dynamicGyroNotchState,
gyroAnalyseState.filterUpdateAxis,
gyroAnalyseState.filterUpdateFrequency
);
}
}
#endif

8
src/main/sensors/gyro.h
View file

@ -71,10 +71,10 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_cutoff_2;
uint16_t gyro_stage2_lowpass_hz;
uint8_t gyro_stage2_lowpass_type;
uint8_t dyn_notch_width_percent;
uint8_t dyn_notch_range;
uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz;
uint8_t dynamicGyroNotchRange;
uint16_t dynamicGyroNotchQ;
uint16_t dynamicGyroNotchMinHz;
uint8_t dynamicGyroNotchEnabled;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);

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

@ -27,8 +27,8 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM
// Motors
DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1

3
src/main/target/BETAFLIGHTF4/target.h
View file

@ -180,3 +180,6 @@
#define MAX_PWM_OUTPUT_PORTS 4
#define PCA9685_I2C_BUS BUS_I2C2
#define USE_DSHOT
#define USE_ESC_SENSOR

9
src/main/target/F4BY/target.h
View file

@ -35,6 +35,9 @@
#define MPU6000_CS_PIN PA4
#define MPU6000_SPI_BUS BUS_SPI1
#define ICM20689_CS_PIN PA4
#define ICM20689_SPI_BUS BUS_SPI1
#define USE_ACC
#define USE_ACC_MPU6000
@ -44,6 +47,12 @@
#define USE_GYRO_MPU6000
#define GYRO_MPU6000_ALIGN CW90_DEG
#define USE_ACC_ICM20689
#define ACC_ICM20689_ALIGN CW90_DEG
#define USE_GYRO_ICM20689
#define GYRO_ICM20689_ALIGN CW90_DEG
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C2
#define MAG_HMC5883_ALIGN CW90_DEG

1
src/main/target/F4BY/target.mk
View file

@ -3,6 +3,7 @@ FEATURES += SDCARD VCP MSC
TARGET_SRC = \
drivers/accgyro/accgyro_mpu6000.c \
drivers/accgyro/accgyro_icm20689.c \
drivers/barometer/barometer_ms56xx.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \

1
src/main/target/FF_F35_LIGHTNING/config.c
View file

@ -48,7 +48,6 @@ void targetConfiguration(void)
rxConfigMutable()->serialrx_provider = SERIALRX_CRSF;
serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
telemetryConfigMutable()->uartUnidirectional = 1;
mixerConfigMutable()->platformType = PLATFORM_AIRPLANE;
}

1
src/main/target/FF_PIKOF4/config.c
View file

@ -28,6 +28,5 @@
void targetConfiguration(void)
{
rxConfigMutable()->halfDuplex = false;
telemetryConfigMutable()->uartUnidirectional = true;
batteryMetersConfigMutable()->current.scale = CURRENTSCALE;
}

8
src/main/target/FURYF4OSD/target.c
View file

@ -24,10 +24,10 @@
const timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6_CH3
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - DMA1_ST7_CH5
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA2_ST6_CH0
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1_CH3
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT - D(1, 6, 3)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - D(1, 7, 5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - D(1, 2, 5)
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4_OUT - D(1, 1, 3)
DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0),
DEF_TIM(TIM2, CH4, PB11, TIM_USE_ANY, 0, 0),
DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA1_ST2_CH6

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

@ -38,7 +38,7 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M5 , DMA2_ST7
DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M6 , DMA1_ST1
DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP, DMA1_ST0
DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

3
src/main/target/KAKUTEF7/target.h
View file

@ -172,6 +172,9 @@
#define SERIALRX_UART SERIAL_PORT_USART6
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define USE_LED_STRIP
#define WS2811_PIN PD12 //TIM4_CH1
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff

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

@ -27,7 +27,7 @@
const timerHardware_t timerHardware[] = {
DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 0 ), // S4_OUT – D(2, 6, 0)
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7)

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

@ -30,7 +30,7 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6)
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 0 ), // S4_OUT – D(2, 6, 0)
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6)
DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3)
};

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

@ -46,7 +46,7 @@
#define USE_ACC
#define USE_ACC_MPU6000
#define ACC_MPU6500_ALIGN CW180_DEG
#define ACC_MPU6000_ALIGN CW180_DEG
// *************** Baro **************************
#define USE_I2C

6
src/main/target/OMNIBUSF4/target.c
View file

@ -24,7 +24,7 @@
#include "drivers/bus.h"
const timerHardware_t timerHardware[] = {
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN
#else
@ -42,7 +42,7 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1
// { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR_3
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !(defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5_OUT
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT
#elif defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
@ -59,7 +59,7 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6_OUT
#endif
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4)
#endif
};

31
src/main/target/OMNIBUSF4/target.h
View file

@ -17,12 +17,18 @@
#pragma once
//Same target as OMNIBUSF4PRO with LED strip in M5
#ifdef OMNIBUSF4PRO_LEDSTRIPM5
#define OMNIBUSF4PRO
#endif
//Same target as OMNIBUSF4V3 with softserial in M5 and M6
#if defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#define OMNIBUSF4V3
#endif
#ifdef OMNIBUSF4PRO
#define TARGET_BOARD_IDENTIFIER "OBSD"
#elif defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#elif defined(OMNIBUSF4V3)
#define TARGET_BOARD_IDENTIFIER "OB43"
#elif defined(DYSF4PRO)
#define TARGET_BOARD_IDENTIFIER "DYS4"
@ -32,7 +38,7 @@
#define TARGET_BOARD_IDENTIFIER "OBF4"
#endif
#if defined(DYSF4PRO)
#if defined(DYSF4PRO) || defined(DYSF4PROV2)
#define USBD_PRODUCT_STRING "DysF4Pro"
#else
#define USBD_PRODUCT_STRING "Omnibus F4"
@ -69,8 +75,7 @@
#define MPU6000_CS_PIN PA4
#define MPU6000_SPI_BUS BUS_SPI1
// Long sentence, OMNIBUSF4 always defined
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#define USE_GYRO_MPU6000
#define GYRO_MPU6000_ALIGN CW270_DEG
@ -85,7 +90,7 @@
#endif
// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#define MPU6500_CS_PIN MPU6000_CS_PIN
#define MPU6500_SPI_BUS MPU6000_SPI_BUS
@ -111,7 +116,7 @@
#define USE_BARO
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#define USE_BARO_BMP280
#define BMP280_SPI_BUS BUS_SPI3
#define BMP280_CS_PIN PB3 // v1
@ -141,7 +146,7 @@
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5)
#if defined(OMNIBUSF4PRO)
#define INVERTER_PIN_UART1_RX PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it.
#endif
@ -152,12 +157,12 @@
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#if defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4V3)
#define INVERTER_PIN_UART6_RX PC8
#define INVERTER_PIN_UART6_TX PC9
#endif
#if defined(OMNIBUSF4V3)
#if defined(OMNIBUSF4V3) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX
#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX
@ -201,7 +206,7 @@
#define USE_SPI_DEVICE_1
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
@ -210,7 +215,7 @@
#endif
#define USE_SPI_DEVICE_3
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#define SPI3_NSS_PIN PA15
#else
#define SPI3_NSS_PIN PB3
@ -224,7 +229,7 @@
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PA15
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)
#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define USE_SDCARD
#define USE_SDCARD_SPI
@ -259,7 +264,7 @@
#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
#define USE_LED_STRIP
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS))
#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5)
#define WS2811_PIN PB6
#else
#define WS2811_PIN PA1

12
src/main/target/SPRACINGF3NEO/target.h
View file

@ -107,9 +107,6 @@
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
#define USE_VTX_RTC6705
#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
#undef USE_VTX_FFPV
#undef USE_VTX_SMARTAUDIO // Disabled due to flash size
#undef USE_VTX_TRAMP // Disabled due to flash size
@ -117,19 +114,10 @@
#undef USE_PITOT // Disabled due to RAM size
#undef USE_PITOT_ADC // Disabled due to RAM size
#define RTC6705_CS_PIN PF4
#define RTC6705_SPI_INSTANCE SPI3
#define RTC6705_POWER_PIN PC3
#define USE_RTC6705_CLK_HACK
#define RTC6705_CLK_PIN SPI3_SCK_PIN
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PA15
#define SPI_SHARED_MAX7456_AND_RTC6705
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_DETECT_INVERTED

3
src/main/target/SPRACINGF3NEO/target.mk_
View file

@ -15,5 +15,4 @@ TARGET_SRC = \
drivers/compass/compass_mag3110.c \
drivers/compass/compass_lis3mdl.c \
drivers/light_ws2811strip.c \
drivers/max7456.c \
drivers/vtx_rtc6705.c
drivers/max7456.c

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

@ -126,12 +126,6 @@
#define SPI3_MISO_PIN PB4 // NC
#define SPI3_MOSI_PIN PB5 // NC
#define USE_VTX_RTC6705
#define VTX_RTC6705_OPTIONAL // SPI3 on an F4 EVO may be used for RTC6705 VTX control.
#define RTC6705_CS_PIN SPI3_NSS_PIN
#define RTC6705_SPI_INSTANCE SPI3
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_DETECT_INVERTED

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