diff --git a/Makefile b/Makefile index 1b015d486a..f19b1b0938 100644 --- a/Makefile +++ b/Makefile @@ -185,9 +185,11 @@ endif ifneq ($(SEMIHOSTING),) SEMIHOSTING_CFLAGS = -DSEMIHOSTING SEMIHOSTING_LDFLAGS = --specs=rdimon.specs -lc -lrdimon +SYSLIB := else SEMIHOSTING_CFLAGS = SEMIHOSTING_LDFLAGS = +SYSLIB := -lnosys endif DEBUG_FLAGS = -ggdb3 -DDEBUG @@ -224,7 +226,7 @@ LDFLAGS = -lm \ -nostartfiles \ --specs=nano.specs \ -lc \ - -lnosys \ + $(SYSLIB) \ $(ARCH_FLAGS) \ $(LTO_FLAGS) \ $(DEBUG_FLAGS) \ diff --git a/docs/Cli.md b/docs/Cli.md index 64799baab9..cfb8e3d065 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -182,7 +182,7 @@ A shorter form is also supported to enable and disable functions using `serial < | nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | -| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm] | +| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | | nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | | nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | @@ -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. | diff --git a/docs/Logic Conditions.md b/docs/Logic Conditions.md index bc25e3cccd..ac8ae4d8fd 100644 --- a/docs/Logic Conditions.md +++ b/docs/Logic Conditions.md @@ -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 diff --git a/docs/Rx.md b/docs/Rx.md index 51cd57b2a2..951c1b4c91 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -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. diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md new file mode 100644 index 0000000000..0d6bc015ab --- /dev/null +++ b/docs/development/serial_printf_debugging.md @@ -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. diff --git a/make/release.mk b/make/release.mk index cff7d39718..bc7c18e83e 100644 --- a/make/release.mk +++ b/make/release.mk @@ -25,10 +25,10 @@ RELEASE_TARGETS += NOX WINGFC RELEASE_TARGETS += OMNIBUSF4V6 -RELEASE_TARGETS += MAMBAF405 +RELEASE_TARGETS += MAMBAF405 MAMBAF405US MAMBAF722 RELEASE_TARGETS += IFLIGHTF7_TWING IFLIGHTF4_TWING RELEASE_TARGETS += AIKONF4 -RELEASE_TARGETS += MATEKF765 \ No newline at end of file +RELEASE_TARGETS += MATEKF765 diff --git a/make/source.mk b/make/source.mk index 20cfc6e8f7..e3a58d96db 100644 --- a/make/source.mk +++ b/make/source.mk @@ -101,9 +101,11 @@ COMMON_SRC = \ flight/gyroanalyse.c \ flight/secondary_imu.c \ flight/rpm_filter.c \ + flight/dynamic_gyro_notch.c \ io/beeper.c \ io/esc_serialshot.c \ io/frsky_osd.c \ + io/osd_dji_hd.c \ io/lights.c \ io/piniobox.c \ io/pwmdriver_i2c.c \ @@ -163,9 +165,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 \ diff --git a/make/tools.mk b/make/tools.mk index a5820aee63..28e1953b61 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -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 diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fa800a93f9..4095224c87 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 7456dc893d..52c51332aa 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -68,5 +68,7 @@ typedef enum { DEBUG_RPM_FILTER, DEBUG_RPM_FREQ, DEBUG_IMU2, + DEBUG_DYNAMIC_FILTER, + DEBUG_DYNAMIC_FILTER_FREQUENCY, DEBUG_COUNT } debugType_e; diff --git a/src/main/build/version.h b/src/main/build/version.h index ec17abe33a..bc3988d841 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -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 diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 6d70284c9d..fcfb56ebea 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -63,6 +63,7 @@ #include "fc/settings.h" #include "flight/mixer.h" +#include "flight/servos.h" // For VISIBLE* #include "io/osd.h" @@ -765,6 +766,7 @@ void cmsMenuOpen(void) { if (!cmsInMenu) { // New open + setServoOutputEnabled(false); pCurrentDisplay = cmsDisplayPortSelectCurrent(); if (!pCurrentDisplay) return; @@ -860,6 +862,8 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) displayRelease(pDisplay); currentCtx.menu = NULL; + setServoOutputEnabled(true); + if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT)) { displayClearScreen(pDisplay); displayWrite(pDisplay, 5, 3, "REBOOTING..."); diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index f5378268af..bd4d4fc082 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -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), diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 5ba507addd..d902acb26b 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -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, }; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 0ecb5bb0dd..bf35aa8997 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -15,132 +15,247 @@ * along with Cleanflight. If not, see . */ +#include #include #include -#include +#include #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 diff --git a/src/main/cms/cms_menu_vtx.h b/src/main/cms/cms_menu_vtx.h index 2e5bee6278..cadad8be45 100644 --- a/src/main/cms/cms_menu_vtx.h +++ b/src/main/cms/cms_menu_vtx.h @@ -17,4 +17,4 @@ #pragma once -extern const CMS_Menu cmsx_menuVtx; +extern const CMS_Menu cmsx_menuVtxControl; diff --git a/src/main/cms/cms_menu_vtx_ffpv.c b/src/main/cms/cms_menu_vtx_ffpv.c deleted file mode 100644 index d2b5719ee7..0000000000 --- a/src/main/cms/cms_menu_vtx_ffpv.c +++ /dev/null @@ -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 . - */ - -#include -#include -#include -#include - -#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 diff --git a/src/main/cms/cms_menu_vtx_ffpv.h b/src/main/cms/cms_menu_vtx_ffpv.h deleted file mode 100644 index 6faf7cb59a..0000000000 --- a/src/main/cms/cms_menu_vtx_ffpv.h +++ /dev/null @@ -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 . - */ - -#pragma once - -#include "cms/cms.h" -#include "cms/cms_types.h" - -extern const CMS_Menu cmsx_menuVtxFFPV; diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c deleted file mode 100644 index 054e89c189..0000000000 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ /dev/null @@ -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 . - */ - -#include -#include -#include -#include - -#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 diff --git a/src/main/cms/cms_menu_vtx_smartaudio.h b/src/main/cms/cms_menu_vtx_smartaudio.h deleted file mode 100644 index e29f05d891..0000000000 --- a/src/main/cms/cms_menu_vtx_smartaudio.h +++ /dev/null @@ -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 . - */ - -#pragma once - -#include "cms/cms.h" -#include "cms/cms_types.h" - -extern CMS_Menu cmsx_menuVtxSmartAudio; - -void saCmsUpdate(void); -void saCmsResetOpmodel(void); diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c deleted file mode 100644 index 540a78421e..0000000000 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ /dev/null @@ -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 . - */ - -#include -#include -#include -#include - -#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 diff --git a/src/main/cms/cms_menu_vtx_tramp.h b/src/main/cms/cms_menu_vtx_tramp.h deleted file mode 100644 index c08f938679..0000000000 --- a/src/main/cms/cms_menu_vtx_tramp.h +++ /dev/null @@ -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 . - */ - -#pragma once - -#include "cms/cms.h" -#include "cms/cms_types.h" - -extern const CMS_Menu cmsx_menuVtxTramp; diff --git a/src/main/common/global_functions.c b/src/main/common/global_functions.c index 7bf4466913..7421dab33c 100644 --- a/src/main/common/global_functions.c +++ b/src/main/common/global_functions.c @@ -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]; @@ -164,4 +164,4 @@ int16_t FAST_CODE getRcCommandOverride(int16_t command[], uint8_t axis) { return outputValue; } -#endif \ No newline at end of file +#endif diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c index 9f1be2ce0c..38f71b134c 100644 --- a/src/main/common/logic_condition.c +++ b/src/main/common/logic_condition.c @@ -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; @@ -363,4 +383,4 @@ void logicConditionReset(void) { logicConditionStates[i].value = 0; logicConditionStates[i].flags = 0; } -} \ No newline at end of file +} diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h index df6518a4c5..10129db72c 100644 --- a/src/main/common/logic_condition.h +++ b/src/main/common/logic_condition.h @@ -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 { diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 521a3a3526..3ed062a198 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -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))) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 1342c363cb..0abbfe97a9 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -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 diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index ca8bb993d9..532fd03606 100755 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -127,6 +127,7 @@ static bool mpu6500DeviceDetect(busDevice_t * dev) switch (tmp) { case MPU6500_WHO_AM_I_CONST: case ICM20608G_WHO_AM_I_CONST: + case ICM20601_WHO_AM_I_CONST: case ICM20602_WHO_AM_I_CONST: case ICM20689_WHO_AM_I_CONST: // Compatible chip detected diff --git a/src/main/drivers/compass/compass_mag3110.c b/src/main/drivers/compass/compass_mag3110.c index 4abc44ca04..5cd8454ebc 100755 --- a/src/main/drivers/compass/compass_mag3110.c +++ b/src/main/drivers/compass/compass_mag3110.c @@ -97,7 +97,7 @@ static bool mag3110Read(magDev_t * mag) mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]); mag->magADCRaw[Y] = (int16_t)(buf[2] << 8 | buf[3]); - mag->magADCRaw[Z] = (int16_t)(buf[4] << 8 | buf[5]); + mag->magADCRaw[Z] = -(int16_t)(buf[4] << 8 | buf[5]); // Z is down, not up in this sensor return true; } diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index aa4bfcb2c9..69bde9c41e 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -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) { diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 34b2cf27e8..6f4cd52373 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -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; +} diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 7e7ee7c9c2..9971b8f497 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -30,19 +30,12 @@ #define VTX_SETTINGS_DEFAULT_BAND 4 #define VTX_SETTINGS_DEFAULT_CHANNEL 1 -#define VTX_SETTINGS_DEFAULT_FREQ 5740 -#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0 +#define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL 1 #define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0 #define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting -#if defined(USE_VTX_RTC6705) - -#include "drivers/vtx_rtc6705.h" - -#endif - #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) #define VTX_SETTINGS_POWER_COUNT 5 @@ -51,14 +44,7 @@ #define VTX_SETTINGS_MIN_USER_FREQ 5000 #define VTX_SETTINGS_MAX_USER_FREQ 5999 #define VTX_SETTINGS_FREQCMD -#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) - -#elif defined(USE_VTX_RTC6705) - -#define VTX_SETTINGS_POWER_COUNT VTX_RTC6705_POWER_COUNT -#define VTX_SETTINGS_DEFAULT_POWER VTX_RTC6705_DEFAULT_POWER -#define VTX_SETTINGS_MIN_POWER VTX_RTC6705_MIN_POWER -#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1) +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) #endif @@ -68,7 +54,7 @@ typedef enum { VTXDEV_UNSUPPORTED = 0, // reserved for MSP - VTXDEV_RTC6705 = 1, + VTXDEV_RTC6705 = 1, // deprecated // 2 reserved VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, @@ -82,23 +68,32 @@ typedef struct vtxDeviceCapability_s { uint8_t bandCount; uint8_t channelCount; uint8_t powerCount; + char **bandNames; + char **channelNames; + char **powerNames; } vtxDeviceCapability_t; +typedef struct vtxDeviceOsdInfo_s { + int band; + int channel; + int frequency; + int powerIndex; + int powerMilliwatt; + char bandLetter; + const char * bandName; + const char * channelName; + char powerIndexLetter; +} vtxDeviceOsdInfo_t; + typedef struct vtxDevice_s { const struct vtxVTable_s * const vTable; vtxDeviceCapability_t capability; - uint16_t *frequencyTable; // Array of [bandCount][channelCount] - char **bandNames; // char *bandNames[bandCount] - char **channelNames; // char *channelNames[channelCount] - char **powerNames; // char *powerNames[powerCount] - uint8_t band; // Band = 1, 1-based uint8_t channel; // CH1 = 1, 1-based uint8_t powerIndex; // Lowest/Off = 0 uint8_t pitMode; // 0 = non-PIT, 1 = PIT - } vtxDevice_t; // {set,get}BandAndChannel: band and channel are 1 origin @@ -113,12 +108,14 @@ typedef struct vtxVTable_s { void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level); void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff); - void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq); bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex); bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff); bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq); + + bool (*getPower)(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw); + bool (*getOsdInfo)(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo); } vtxVTable_t; // 3.1.0 @@ -137,9 +134,10 @@ bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice); void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index); void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); -void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency); bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex); bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff); bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq); bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability); +bool vtxCommonGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw); +bool vtxCommonGetOsdInfo(vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo); diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c deleted file mode 100644 index 591f22efc3..0000000000 --- a/src/main/drivers/vtx_rtc6705.c +++ /dev/null @@ -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 . - */ - -/* - * 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 -#include - -#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 diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h deleted file mode 100644 index 7a41938607..0000000000 --- a/src/main/drivers/vtx_rtc6705.h +++ /dev/null @@ -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 . - */ - -/* - * 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 - -#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); diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c deleted file mode 100644 index d7795e6c47..0000000000 --- a/src/main/drivers/vtx_rtc6705_soft_spi.c +++ /dev/null @@ -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 . - */ - -#include -#include - -#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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d8986ef808..cdb9cd939c 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -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" @@ -146,8 +147,8 @@ static bool commandBatchError = false; // sync this with features_e static const char * const featureNames[] = { "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", - "DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "RPM_FILTERS", - "", "TELEMETRY", "CURRENT_METER", "3D", "", + "", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", @@ -1322,7 +1323,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; @@ -3025,6 +3026,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()); @@ -3196,13 +3220,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 diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 0fac3af42c..4ac930f1fa 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -163,7 +163,7 @@ __attribute__((weak)) void targetConfiguration(void) uint32_t getLooptime(void) { return gyro.targetLooptime; -} +} void validateAndFixConfig(void) { @@ -181,13 +181,7 @@ void validateAndFixConfig(void) } // Disable unused features - featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); - -#if defined(DISABLE_RX_PWM_FEATURE) || !defined(USE_RX_PWM) - if (rxConfig()->receiverType == RX_TYPE_PWM) { - rxConfigMutable()->receiverType = RX_TYPE_NONE; - } -#endif + featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); #if !defined(USE_RX_PPM) if (rxConfig()->receiverType == RX_TYPE_PPM) { @@ -196,16 +190,6 @@ void validateAndFixConfig(void) #endif - if (rxConfig()->receiverType == RX_TYPE_PWM) { -#if defined(CHEBUZZ) || defined(STM32F3DISCOVERY) - // led strip needs the same ports - featureClear(FEATURE_LED_STRIP); -#endif - - // software serial needs free PWM ports - featureClear(FEATURE_SOFTSERIAL); - } - #if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) { const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 06b52bde51..a12fc63a07 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -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, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f1f20a83a6..06f7fb77d9 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -517,7 +517,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); } @@ -534,7 +534,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) { @@ -635,7 +635,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); @@ -651,13 +651,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 { @@ -755,7 +755,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(); } @@ -784,7 +784,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()) { diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c55128b8b6..e78612b997 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -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" @@ -108,6 +107,7 @@ #include "io/ledstrip.h" #include "io/pwmdriver_i2c.h" #include "io/osd.h" +#include "io/osd_dji_hd.h" #include "io/rcdevice_cam.h" #include "io/serial.h" #include "io/displayport_msp.h" @@ -252,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 @@ -265,6 +265,11 @@ void init(void) // to run after the sensors have been detected. mspSerialInit(); +#if defined(USE_DJI_HD_OSD) + // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task + djiOsdSerialInit(); +#endif + #if defined(USE_LOG) // LOG might use serial output, so we only can init it after serial port is ready // From this point on we can use LOG_*() to produce real-time debugging information @@ -279,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 @@ -290,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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 014f0a5ddc..212b7f01f3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 03e7bd29eb..05187869d8 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -164,13 +164,13 @@ void initActiveBoxIds(void) activeBoxIdCount = 0; activeBoxIds[activeBoxIdCount++] = BOXARM; - if (sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXHORIZON; activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST; } - if (!feature(FEATURE_AIRMODE)) { + if (!feature(FEATURE_AIRMODE) && STATE(ALTITUDE_CONTROL)) { activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; } @@ -181,35 +181,39 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; } - activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; + if (STATE(ALTITUDE_CONTROL)) { + activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; + } //Camstab mode is enabled always activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; #ifdef USE_GPS - if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) { + if (STATE(ALTITUDE_CONTROL) && (sensors(SENSOR_BARO) || (STATE(AIRPLANE) && feature(FEATURE_GPS)))) { activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; activeBoxIds[activeBoxIdCount++] = BOXSURFACE; } - const bool navReadyQuads = !STATE(FIXED_WING) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; - if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) { - activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; - if (STATE(FIXED_WING)) { + if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) { + if (!STATE(ROVER) && !STATE(BOAT)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + } + if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; } } - if (navReadyQuads || navReadyPlanes) { + if (navReadyMultirotor || navReadyOther) { activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; if (feature(FEATURE_GPS)) { activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - if (STATE(FIXED_WING)) { + if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; } } @@ -223,8 +227,11 @@ void initActiveBoxIds(void) #endif - if (STATE(FIXED_WING)) { + if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { activeBoxIds[activeBoxIdCount++] = BOXMANUAL; + } + + if (STATE(AIRPLANE)) { if (!feature(FEATURE_FW_LAUNCH)) { activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH; } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 6ac5200600..a032a5cf68 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -62,6 +62,7 @@ #include "io/serial.h" #include "io/rcdevice_cam.h" #include "io/vtx.h" +#include "io/osd_dji_hd.h" #include "msp/msp_serial.h" @@ -98,6 +99,11 @@ void taskHandleSerial(timeUs_t currentTimeUs) // Allow MSP processing even if in CLI mode mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); + +#if defined(USE_DJI_HD_OSD) + // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task + djiOsdSerialProcess(); +#endif } void taskUpdateBattery(timeUs_t currentTimeUs) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 86c41fac6b..b741e75b87 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -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(); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 3a372a0ffb..23c8887cfa 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -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; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 01f84af7f4..505c04ed59 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -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(); } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 8540e7b317..0d1a60a25c 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -102,18 +102,21 @@ uint32_t sensorsMask(void) flightModeForTelemetry_e getFlightModeForTelemetry(void) { - if (FLIGHT_MODE(MANUAL_MODE)) - return FLM_MANUAL; - if (FLIGHT_MODE(FAILSAFE_MODE)) return FLM_FAILSAFE; + if (FLIGHT_MODE(MANUAL_MODE)) + return FLM_MANUAL; + if (FLIGHT_MODE(NAV_RTH_MODE)) return FLM_RTH; if (FLIGHT_MODE(NAV_POSHOLD_MODE)) return FLM_POSITION_HOLD; + if (FLIGHT_MODE(NAV_CRUISE_MODE)) + return FLM_CRUISE; + if (FLIGHT_MODE(NAV_WP_MODE)) return FLM_MISSION; @@ -129,5 +132,5 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(NAV_LAUNCH_MODE)) return FLM_LAUNCH; - return FLM_ACRO; + return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO; } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index f97b499ea0..67a2a40cca 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -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,12 @@ 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), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) @@ -134,12 +140,14 @@ extern uint32_t stateFlags; typedef enum { FLM_MANUAL, FLM_ACRO, + FLM_ACRO_AIR, FLM_ANGLE, FLM_HORIZON, FLM_ALTITUDE_HOLD, FLM_POSITION_HOLD, FLM_RTH, FLM_MISSION, + FLM_CRUISE, FLM_LAUNCH, FLM_FAILSAFE, FLM_COUNT diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a68d89a118..37f81db43d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -22,10 +22,10 @@ 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"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"] - name: rx_spi_protocol values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"] enum: rx_spi_protocol_e @@ -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", "DEBUG_IMU2"] + "ERPM", "RPM_FILTER", "RPM_FREQ", "IMU2", "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 @@ -747,9 +746,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 @@ -766,19 +765,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 @@ -940,9 +939,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 @@ -958,21 +954,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 @@ -1020,7 +1001,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 @@ -1753,8 +1734,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 @@ -2142,16 +2123,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 diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c new file mode 100644 index 0000000000..f45b3b6c09 --- /dev/null +++ b/src/main/flight/dynamic_gyro_notch.c @@ -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 +#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 \ No newline at end of file diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h new file mode 100644 index 0000000000..f6ae3b7779 --- /dev/null +++ b/src/main/flight/dynamic_gyro_notch.h @@ -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 +#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); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index b03da0541e..c0f951dcc3 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -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; diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 9d61ff7214..64da4305f1 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -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(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH); - } else { - biquadFilterUpdate(¬chFilterDyn[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 diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index b4329309e8..0b4e060a4d 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -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 \ No newline at end of file diff --git a/src/main/flight/hil.c b/src/main/flight/hil.c index 59fe5b47ef..bb3b4a2d5d 100644 --- a/src/main/flight/hil.c +++ b/src/main/flight/hil.c @@ -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]; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3dfdf29b81..ff9f0f9063 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 50f579b815..4d8477cfe5 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -61,20 +61,22 @@ static float mixerScale = 1.0f; static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static EXTENDED_FASTRAM uint8_t motorCount = 0; EXTENDED_FASTRAM int mixerThrottleCommand; -static EXTENDED_FASTRAM int throttleIdleValue = 0; +static EXTENDED_FASTRAM int throttleIdleValue = 0; +static EXTENDED_FASTRAM int motorValueWhenStopped = 0; +static 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,14 +149,36 @@ bool mixerIsOutputSaturated(void) void mixerUpdateStateFlags(void) { - // set flag that we're on something with wings + DISABLE_STATE(FIXED_WING_LEGACY); + DISABLE_STATE(MULTIROTOR); + DISABLE_STATE(ROVER); + DISABLE_STATE(BOAT); + DISABLE_STATE(AIRPLANE); + DISABLE_STATE(MOVE_FORWARD_ONLY); + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { - ENABLE_STATE(FIXED_WING); + ENABLE_STATE(FIXED_WING_LEGACY); + ENABLE_STATE(AIRPLANE); + ENABLE_STATE(ALTITUDE_CONTROL); + ENABLE_STATE(MOVE_FORWARD_ONLY); + } if (mixerConfig()->platformType == PLATFORM_ROVER) { + ENABLE_STATE(ROVER); + ENABLE_STATE(FIXED_WING_LEGACY); + ENABLE_STATE(MOVE_FORWARD_ONLY); + } if (mixerConfig()->platformType == PLATFORM_BOAT) { + ENABLE_STATE(BOAT); + ENABLE_STATE(FIXED_WING_LEGACY); + ENABLE_STATE(MOVE_FORWARD_ONLY); + } else if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + ENABLE_STATE(MULTIROTOR); + ENABLE_STATE(ALTITUDE_CONTROL); + } else if (mixerConfig()->platformType == PLATFORM_TRICOPTER) { + ENABLE_STATE(MULTIROTOR); + ENABLE_STATE(ALTITUDE_CONTROL); } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) { - DISABLE_STATE(FIXED_WING); - } else { - DISABLE_STATE(FIXED_WING); - } + ENABLE_STATE(MULTIROTOR); + ENABLE_STATE(ALTITUDE_CONTROL); + } if (mixerConfig()->hasFlaps) { ENABLE_STATE(FLAPERON_AVAILABLE); @@ -172,7 +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; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index adf78341a8..351a371e4d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e5a7eea21a..e222d4b81c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -119,7 +119,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; @@ -531,7 +530,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); #ifdef USE_SECONDARY_IMU @@ -659,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; @@ -723,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); @@ -877,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) @@ -921,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 { @@ -1034,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; } @@ -1058,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); @@ -1088,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; diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index e2d9604727..7d861de498 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -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 \ No newline at end of file diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index 29f825152c..eb41d23b46 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -49,5 +49,4 @@ PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); void disableRpmFilters(void); void rpmFiltersInit(void); void rpmFilterUpdateTask(timeUs_t currentTimeUs); -float rpmFilterGyroApply(uint8_t axis, float input); -float rpmFilterDtermApply(uint8_t axis, float input); \ No newline at end of file +float rpmFilterGyroApply(uint8_t axis, float input); \ No newline at end of file diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 5715000714..d3863981a1 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -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 diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 8ecc5f3200..dac370616b 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -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; } @@ -449,6 +449,11 @@ bool FAST_CODE NOINLINE isServoOutputEnabled(void) return servoOutputEnabled; } +void NOINLINE setServoOutputEnabled(bool flag) +{ + servoOutputEnabled = flag; +} + bool FAST_CODE NOINLINE isMixerUsingServos(void) { return mixerUsesServos; diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index f247f3ade3..3025bc4edc 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -142,6 +142,7 @@ typedef struct servoMetadata_s { extern int16_t servo[MAX_SUPPORTED_SERVOS]; bool isServoOutputEnabled(void); +void setServoOutputEnabled(bool flag); bool isMixerUsingServos(void); void writeServos(void); void loadCustomServoMixer(void); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index dcb49f0080..0de1f18ad4 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -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) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 877597201a..3079518cc6 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -531,14 +531,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 @@ -660,6 +652,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; @@ -812,7 +806,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"); @@ -1698,34 +1692,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; @@ -2143,7 +2129,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()) { diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 5a4c310a35..c4d562be45 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -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); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c new file mode 100644 index 0000000000..38e7bf52ce --- /dev/null +++ b/src/main/io/osd_dji_hd.c @@ -0,0 +1,653 @@ +/* + * This file is part of INAV. + * + * 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/. + * + * @author Konstantin Sharlaimov (ksharlaimov@inavflight.com) + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" +#include "build/version.h" + +#include "common/streambuf.h" +#include "common/utils.h" +#include "common/maths.h" +#include "common/time.h" +#include "common/crc.h" + +#include "fc/fc_core.h" +#include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/fc_msp.h" +#include "fc/fc_msp_box.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/pid.h" +#include "flight/mixer.h" + +#include "io/serial.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/osd_dji_hd.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/battery.h" +#include "sensors/rangefinder.h" +#include "sensors/acceleration.h" +#include "sensors/esc_sensor.h" + +#include "msp/msp.h" +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +#include "navigation/navigation.h" + +#include "scheduler/scheduler.h" + +#if defined(USE_DJI_HD_OSD) + +#define DJI_MSP_BAUDRATE 115200 + +#define DJI_ARMING_DISABLE_FLAGS_COUNT 25 +#define DJI_OSD_WARNING_COUNT 16 +#define DJI_OSD_TIMER_COUNT 2 +#define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0) + +/* + * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0 + * DJI uses a subset of messages and assume fixed bit positions for flight modes + * + * To avoid compatibility issues we maintain a separate MSP command processor + * but reuse the packet decoder to minimize code duplication + */ + +// External dependency on looptime +extern timeDelta_t cycleTime; + +// MSP packet decoder state structure +static mspPort_t djiMspPort; + +// Mapping table between DJI PID and INAV PID (order is different) +const uint8_t djiPidIndexMap[] = { + PID_ROLL, // DJI: PID_ROLL + PID_PITCH, // DJI: PID_PITCH + PID_YAW, // DJI: PID_YAW + PID_LEVEL, // DJI: PID_LEVEL + PID_HEADING // DJI: PID_MAG +}; + +const int djiOSDItemIndexMap[] = { + OSD_RSSI_VALUE, // DJI: OSD_RSSI_VALUE + OSD_MAIN_BATT_VOLTAGE, // DJI: OSD_MAIN_BATT_VOLTAGE + OSD_CROSSHAIRS, // DJI: OSD_CROSSHAIRS + OSD_ARTIFICIAL_HORIZON, // DJI: OSD_ARTIFICIAL_HORIZON + OSD_HORIZON_SIDEBARS, // DJI: OSD_HORIZON_SIDEBARS + OSD_ONTIME, // DJI: OSD_ITEM_TIMER_1 + OSD_FLYTIME, // DJI: OSD_ITEM_TIMER_2 + OSD_FLYMODE, // DJI: OSD_FLYMODE + OSD_CRAFT_NAME, // DJI: OSD_CRAFT_NAME + OSD_THROTTLE_POS, // DJI: OSD_THROTTLE_POS + OSD_VTX_CHANNEL, // DJI: OSD_VTX_CHANNEL + OSD_CURRENT_DRAW, // DJI: OSD_CURRENT_DRAW + OSD_MAH_DRAWN, // DJI: OSD_MAH_DRAWN + OSD_GPS_SPEED, // DJI: OSD_GPS_SPEED + OSD_GPS_SATS, // DJI: OSD_GPS_SATS + OSD_ALTITUDE, // DJI: OSD_ALTITUDE + OSD_ROLL_PIDS, // DJI: OSD_ROLL_PIDS + OSD_PITCH_PIDS, // DJI: OSD_PITCH_PIDS + OSD_YAW_PIDS, // DJI: OSD_YAW_PIDS + OSD_POWER, // DJI: OSD_POWER + -1, // DJI: OSD_PIDRATE_PROFILE + -1, // DJI: OSD_WARNINGS + OSD_MAIN_BATT_CELL_VOLTAGE, // DJI: OSD_AVG_CELL_VOLTAGE + OSD_GPS_LON, // DJI: OSD_GPS_LON + OSD_GPS_LAT, // DJI: OSD_GPS_LAT + OSD_DEBUG, // DJI: OSD_DEBUG + OSD_ATTITUDE_PITCH, // DJI: OSD_PITCH_ANGLE + OSD_ATTITUDE_ROLL, // DJI: OSD_ROLL_ANGLE + -1, // DJI: OSD_MAIN_BATT_USAGE + -1, // DJI: OSD_DISARMED + OSD_HOME_DIR, // DJI: OSD_HOME_DIR + OSD_HOME_DIST, // DJI: OSD_HOME_DIST + OSD_HEADING, // DJI: OSD_NUMERICAL_HEADING + OSD_VARIO_NUM, // DJI: OSD_NUMERICAL_VARIO + -1, // DJI: OSD_COMPASS_BAR + -1, // DJI: OSD_ESC_TMP + OSD_ESC_RPM, // DJI: OSD_ESC_RPM + OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, // DJI: OSD_REMAINING_TIME_ESTIMATE + OSD_RTC_TIME, // DJI: OSD_RTC_DATETIME + -1, // DJI: OSD_ADJUSTMENT_RANGE + -1, // DJI: OSD_CORE_TEMPERATURE + -1, // DJI: OSD_ANTI_GRAVITY + -1, // DJI: OSD_G_FORCE + -1, // DJI: OSD_MOTOR_DIAG + -1, // DJI: OSD_LOG_STATUS + -1, // DJI: OSD_FLIP_ARROW + -1, // DJI: OSD_LINK_QUALITY + OSD_TRIP_DIST, // DJI: OSD_FLIGHT_DIST + -1, // DJI: OSD_STICK_OVERLAY_LEFT + -1, // DJI: OSD_STICK_OVERLAY_RIGHT + -1, // DJI: OSD_DISPLAY_NAME + -1, // DJI: OSD_ESC_RPM_FREQ + -1, // DJI: OSD_RATE_PROFILE_NAME + -1, // DJI: OSD_PID_PROFILE_NAME + -1, // DJI: OSD_PROFILE_NAME + -1, // DJI: OSD_RSSI_DBM_VALUE + -1, // DJI: OSD_RC_CHANNELS +}; + +const int djiOSDStatisticsMap[] = { + -1, // DJI: OSD_STAT_RTC_DATE_TIME + -1, // DJI: OSD_STAT_TIMER_1 + -1, // DJI: OSD_STAT_TIMER_2 + -1, // DJI: OSD_STAT_MAX_SPEED + -1, // DJI: OSD_STAT_MAX_DISTANCE + -1, // DJI: OSD_STAT_MIN_BATTERY + -1, // DJI: OSD_STAT_END_BATTERY + -1, // DJI: OSD_STAT_BATTERY + -1, // DJI: OSD_STAT_MIN_RSSI + -1, // DJI: OSD_STAT_MAX_CURRENT + -1, // DJI: OSD_STAT_USED_MAH + -1, // DJI: OSD_STAT_MAX_ALTITUDE + -1, // DJI: OSD_STAT_BLACKBOX + -1, // DJI: OSD_STAT_BLACKBOX_NUMBER + -1, // DJI: OSD_STAT_MAX_G_FORCE + -1, // DJI: OSD_STAT_MAX_ESC_TEMP + -1, // DJI: OSD_STAT_MAX_ESC_RPM + -1, // DJI: OSD_STAT_MIN_LINK_QUALITY + -1, // DJI: OSD_STAT_FLIGHT_DISTANCE + -1, // DJI: OSD_STAT_MAX_FFT + -1, // DJI: OSD_STAT_TOTAL_FLIGHTS + -1, // DJI: OSD_STAT_TOTAL_TIME + -1, // DJI: OSD_STAT_TOTAL_DIST + -1, // DJI: OSD_STAT_MIN_RSSI_DBM +}; + +void djiOsdSerialInit(void) +{ + memset(&djiMspPort, 0, sizeof(mspPort_t)); + + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DJI_HD_OSD); + + if (!portConfig) { + return; + } + + serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_DJI_HD_OSD, NULL, NULL, DJI_MSP_BAUDRATE, MODE_RXTX, SERIAL_NOT_INVERTED); + + if (serialPort) { + resetMspPort(&djiMspPort, serialPort); + } +} + +static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask) +{ + memset(flightModeBitmask, 0, sizeof(boxBitmask_t)); + + // Map flight modes to DJI-supported bits + switch(getFlightModeForTelemetry()) { + case FLM_MANUAL: + case FLM_ACRO: + case FLM_ACRO_AIR: + // DJI: No bits set = ACRO + break; + case FLM_ANGLE: + bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE + break; + case FLM_HORIZON: + bitArraySet(flightModeBitmask->bits, 2); // DJI: 1 << 2 + break; + case FLM_RTH: + bitArraySet(flightModeBitmask->bits, 5); // DJI: 1 << 5 : GPS_RESQUE + break; + case FLM_CRUISE: + bitArraySet(flightModeBitmask->bits, 3); // DJI: 1 << 3 : technically HEADFREE + break; + case FLM_FAILSAFE: + bitArraySet(flightModeBitmask->bits, 4); // DJI: 1 << 4 + break; + case FLM_LAUNCH: + case FLM_ALTITUDE_HOLD: + case FLM_POSITION_HOLD: + case FLM_MISSION: + default: + // Unsupported ATM, keep at ANGLE + bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE + } + + // Set ARMED mode + if (ARMING_FLAG(ARMED)) { + bitArraySet(flightModeBitmask->bits, 0); // DJI: 1 << 0 : ARMED + } +} + +static uint32_t djiPackArmingDisabledFlags(void) +{ + // TODO: Map INAV arming disabled flags to DJI/BF ones + // https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42 + // For now hide everything in ARMING_DISABLED_ARM_SWITCH (bit 24) + + return isArmingDisabled() ? (1 << 24) : 0; +} + +static uint32_t djiEncodeOSDEnabledWarnings(void) +{ + // TODO: + return 0; +} + +static void djiSerializeOSDConfigReply(sbuf_t *dst) +{ + // Only send supported flag - always + sbufWriteU8(dst, DJI_OSD_FLAGS_OSD_FEATURE); + + // 7456 video system (AUTO/PAL/NTSC) + sbufWriteU8(dst, osdConfig()->video_system); + + // Configuration + sbufWriteU8(dst, osdConfig()->units); + + // Alarms + sbufWriteU8(dst, osdConfig()->rssi_alarm); + sbufWriteU16(dst, currentBatteryProfile->capacity.warning); + + // OSD_ITEM_COUNT (previously was timer alarm) + sbufWriteU8(dst, 0); + sbufWriteU8(dst, ARRAYLEN(djiOSDItemIndexMap)); + + // Altitude alarm + sbufWriteU16(dst, osdConfig()->alt_alarm); + + // OSD element position and visibility + for (unsigned i = 0; i < ARRAYLEN(djiOSDItemIndexMap); i++) { + int inavOSDIdx = djiOSDItemIndexMap[i]; + if (inavOSDIdx >= 0) { + // Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV + // However visibility is different. INAV has 3 layouts, while BF only has visibility profiles + // Here we use only one OSD layout mapped to first OSD BF profile + uint16_t itemPos = osdConfig()->item_pos[0][inavOSDIdx]; + + // Workarounds for certain OSD element positions + // INAV calculates these dynamically, while DJI expects the config to have defined coordinates + switch(inavOSDIdx) { + case OSD_CROSSHAIRS: + itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(13, 6); + break; + + case OSD_ARTIFICIAL_HORIZON: + itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 2); + break; + + case OSD_HORIZON_SIDEBARS: + itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 5); + break; + } + + // Enforce visibility in 3 BF OSD profiles + if (OSD_VISIBLE(itemPos)) { + itemPos |= 0x3000; + } + + sbufWriteU16(dst, itemPos); + } + else { + // Hide OSD elements unsupported by INAV + sbufWriteU16(dst, 0); + } + } + + // Post flight statistics + sbufWriteU8(dst, ARRAYLEN(djiOSDStatisticsMap)); + for (unsigned i = 0; i < ARRAYLEN(djiOSDStatisticsMap); i++ ) { + if (djiOSDStatisticsMap[i] >= 0) { + // FIXME: Map post-flight statistics from INAV to BF/DJI + sbufWriteU8(dst, 0); + } + else { + sbufWriteU8(dst, 0); + } + } + + // Timers + sbufWriteU8(dst, DJI_OSD_TIMER_COUNT); + for (int i = 0; i < DJI_OSD_TIMER_COUNT; i++) { + // STUB: We don't support BF's OSD timers + sbufWriteU16(dst, 0); + } + + // Enabled warnings + // API < 1.41 stub, kept for compatibility + sbufWriteU16(dst, djiEncodeOSDEnabledWarnings() & 0xFFFF); + + // API >= 1.41 + // Send the warnings count and 32bit enabled warnings flags. + sbufWriteU8(dst, DJI_OSD_WARNING_COUNT); + sbufWriteU32(dst, djiEncodeOSDEnabledWarnings()); + + // DJI OSD expects 1 OSD profile + sbufWriteU8(dst, 1); + sbufWriteU8(dst, 1); + + // No OSD stick overlay + sbufWriteU8(dst, 0); + + // API >= 1.43 + // Camera frame element width/height - magic numbers taken from Betaflight source + //sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width + //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height +} + +static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + UNUSED(mspPostProcessFn); + + sbuf_t *dst = &reply->buf; + sbuf_t *src = &cmd->buf; + + // Start initializing the reply message + reply->cmd = cmd->cmd; + reply->result = MSP_RESULT_ACK; + + switch (cmd->cmd) { + case DJI_MSP_API_VERSION: + sbufWriteU8(dst, MSP_PROTOCOL_VERSION); + sbufWriteU8(dst, DJI_API_VERSION_MAJOR); + sbufWriteU8(dst, DJI_API_VERSION_MINOR); + break; + + case DJI_MSP_FC_VARIANT: + { + const char * const flightControllerIdentifier = INAV_IDENTIFIER; + sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); + } + break; + + case DJI_MSP_FC_VERSION: + sbufWriteU8(dst, 4); + sbufWriteU8(dst, 1); + sbufWriteU8(dst, 0); + break; + + case DJI_MSP_NAME: + { + const char * name = systemConfig()->name; + int len = strlen(name); + if (len > 12) len = 12; + sbufWriteData(dst, name, len); + } + break; + + case DJI_MSP_STATUS: + case DJI_MSP_STATUS_EX: + { + // DJI OSD relies on a statically defined bit order and doesn't use MSP_BOXIDS + // to get actual BOX order. We need a special packBoxModeFlags() + boxBitmask_t flightModeBitmask; + djiPackBoxModeBitmask(&flightModeBitmask); + + sbufWriteU16(dst, (uint16_t)cycleTime); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, packSensorStatus()); + sbufWriteData(dst, &flightModeBitmask, 4); // unconditional part of flags, first 32 bits + sbufWriteU8(dst, getConfigProfile()); + + sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); + if (cmd->cmd == MSP_STATUS_EX) { + sbufWriteU8(dst, 3); // PID_PROFILE_COUNT + sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex() + } else { + sbufWriteU16(dst, cycleTime); // gyro cycle time + } + + // Cap BoxModeFlags to 32 bits + // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow + sbufWriteU8(dst, 0); + // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount); + + // Write arming disable flags + sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT); + sbufWriteU32(dst, djiPackArmingDisabledFlags()); + + // Extra flags + sbufWriteU8(dst, 0); + } + break; + + case DJI_MSP_RC: + // Only send sticks (first 4 channels) + for (int i = 0; i < STICK_CHANNEL_COUNT; i++) { + sbufWriteU16(dst, rxGetRawChannelValue(i)); + } + break; + + case DJI_MSP_RAW_GPS: + sbufWriteU8(dst, gpsSol.fixType); + sbufWriteU8(dst, gpsSol.numSat); + sbufWriteU32(dst, gpsSol.llh.lat); + sbufWriteU32(dst, gpsSol.llh.lon); + sbufWriteU16(dst, gpsSol.llh.alt / 100); + sbufWriteU16(dst, gpsSol.groundSpeed); + sbufWriteU16(dst, gpsSol.groundCourse); + break; + + case DJI_MSP_COMP_GPS: + sbufWriteU16(dst, GPS_distanceToHome); + sbufWriteU16(dst, GPS_directionToHome); + sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0); + break; + + case DJI_MSP_ATTITUDE: + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); + break; + + case DJI_MSP_ALTITUDE: + sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z))); + sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z))); + break; + + case DJI_MSP_ANALOG: + sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); + sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery + sbufWriteU16(dst, getRSSI()); + sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A + sbufWriteU16(dst, getBatteryVoltage()); + break; + + case DJI_MSP_PID: + for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) { + sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].P); + sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].I); + sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].D); + } + break; + + case DJI_MSP_BATTERY_STATE: + // Battery characteristics + sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255)); + sbufWriteU16(dst, currentBatteryProfile->capacity.value); + + // Battery state + sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps + sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); + sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); + + // Battery alerts - used values match Betaflight's/DJI's + sbufWriteU8(dst, getBatteryState()); + + // Additional battery voltage field (in 0.01V steps) + sbufWriteU16(dst, getBatteryVoltage()); + break; + + case DJI_MSP_RTC: + { + dateTime_t datetime; + + // We don't care about validity here - dt will be always set to a sane value + // rtcGetDateTime() will call rtcGetDefaultDateTime() internally + rtcGetDateTime(&datetime); + + sbufWriteU16(dst, datetime.year); + sbufWriteU8(dst, datetime.month); + sbufWriteU8(dst, datetime.day); + sbufWriteU8(dst, datetime.hours); + sbufWriteU8(dst, datetime.minutes); + sbufWriteU8(dst, datetime.seconds); + sbufWriteU16(dst, datetime.millis); + } + break; + +#if defined(USE_ESC_SENSOR) + case DJI_MSP_ESC_SENSOR_DATA: + if (STATE(ESC_SENSOR_ENABLED)) { + sbufWriteU8(dst, getMotorCount()); + for (int i = 0; i < getMotorCount(); i++) { + const escSensorData_t * escSensor = getEscTelemetry(i); + sbufWriteU8(dst, escSensor->temperature); + sbufWriteU16(dst, escSensor->rpm); + } + } + else { + reply->result = MSP_RESULT_ERROR; + } + break; +#endif + + case DJI_MSP_OSD_CONFIG: +#if defined(USE_OSD) + // This involved some serious magic, better contain in a separate function for readability + djiSerializeOSDConfigReply(dst); +#else + sbufWriteU8(dst, 0); +#endif + break; + + case DJI_MSP_FILTER_CONFIG: + sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz + sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz + sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); // BF: gyroConfig()->gyro_soft_notch_hz_1 + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1 + sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); // BF: currentPidProfile->dterm_notch_hz + sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); // BF: currentPidProfile->dterm_notch_cutoff + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); // BF: gyroConfig()->gyro_soft_notch_hz_2 + sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); // BF: gyroConfig()->gyro_soft_notch_cutoff_2 + sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type + sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf); + sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf + sbufWriteU16(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz); + sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz); // BF: gyroConfig()->gyro_lowpass2_hz); + sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type); + sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type); + sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz); + sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter2_type); + break; + + case DJI_MSP_RC_TUNING: + sbufWriteU8(dst, 100); // INAV doesn't use rcRate + sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); + for (int i = 0 ; i < 3; i++) { + // R,P,Y rates see flight_dynamics_index_t + sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); + } + sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); + sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8); + sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8); + sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint); + sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); + sbufWriteU8(dst, 100); // INAV doesn't use rcRate + sbufWriteU8(dst, 100); // INAV doesn't use rcRate + sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); + + // added in 1.41 + sbufWriteU8(dst, 0); + sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); + break; + + case DJI_MSP_SET_PID: + // Check if we have enough data for all PID coefficients + if ((unsigned)sbufBytesRemaining(src) >= ARRAYLEN(djiPidIndexMap) * 3) { + for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) { + pidBankMutable()->pid[djiPidIndexMap[i]].P = sbufReadU8(src); + pidBankMutable()->pid[djiPidIndexMap[i]].I = sbufReadU8(src); + pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src); + } + schedulePidGainsUpdate(); +#if defined(USE_NAV) + // This is currently unnecessary, DJI HD doesn't set any NAV PIDs + //navigationUsePIDs(); +#endif + } + else { + reply->result = MSP_RESULT_ERROR; + } + break; + + case DJI_MSP_PID_ADVANCED: + // TODO + reply->result = MSP_RESULT_ERROR; + break; + + case DJI_MSP_SET_FILTER_CONFIG: + case DJI_MSP_SET_PID_ADVANCED: + case DJI_MSP_SET_RC_TUNING: + // TODO + reply->result = MSP_RESULT_ERROR; + break; + + default: + // debug[1]++; + // debug[2] = cmd->cmd; + reply->result = MSP_RESULT_ERROR; + break; + } + + // Process DONT_REPLY flag + if (cmd->flags & MSP_FLAG_DONT_REPLY) { + reply->result = MSP_RESULT_NO_REPLY; + } + + return reply->result; +} + +void djiOsdSerialProcess(void) +{ + // Check if DJI OSD is configured + if (!djiMspPort.port) { + return; + } + + // Piggyback on existing MSP protocol stack, but pass our special command processing function + mspSerialProcessOnePort(&djiMspPort, MSP_SKIP_NON_MSP_DATA, djiProcessMspCommand); +} + +#endif diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h new file mode 100644 index 0000000000..d1105783d3 --- /dev/null +++ b/src/main/io/osd_dji_hd.h @@ -0,0 +1,66 @@ +/* + * This file is part of INAV. + * + * 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/. + * + * @author Konstantin Sharlaimov (ksharlaimov@inavflight.com) + */ + +#pragma once + +#include "msp/msp.h" +#include "msp/msp_serial.h" + +#if defined(USE_DJI_HD_OSD) + +#define DJI_API_VERSION_MAJOR 1 +#define DJI_API_VERSION_MINOR 42 + +#define DJI_MSP_API_VERSION 1 // INAV: Implemented | DSI: ??? | +#define DJI_MSP_FC_VARIANT 2 // INAV: Implemented | DSI: ??? | +#define DJI_MSP_FC_VERSION 3 // INAV: Implemented | DSI: ??? | +#define DJI_MSP_NAME 10 // INAV: Implemented | DSI: Implemented | For OSD 'Craft Name' +#define DJI_MSP_OSD_CONFIG 84 // INAV: Implemented | DSI: Implemented | OSD item count + positions +#define DJI_MSP_FILTER_CONFIG 92 // INAV: Not implemented | DSI: Implemented | +#define DJI_MSP_PID_ADVANCED 94 // INAV: Not implemented | DSI: Implemented | +#define DJI_MSP_STATUS 101 // INAV: Implemented | DSI: Implemented | For OSD ‘armingTime’, Flight controller arming status +#define DJI_MSP_RC 105 // INAV: Implemented | DSI: Implemented | +#define DJI_MSP_RAW_GPS 106 // INAV: Implemented | DSI: Implemented | For OSD ‘GPS Sats’ + coordinates +#define DJI_MSP_COMP_GPS 107 // INAV: Implemented | DSI: Not implemented | GPS direction to home & distance to home +#define DJI_MSP_ATTITUDE 108 // INAV: Implemented | DSI: Implemented | For OSD ‘Angle: roll & pitch’ +#define DJI_MSP_ALTITUDE 109 // INAV: Implemented | DSI: Implemented | For OSD ‘Numerical Vario’ +#define DJI_MSP_ANALOG 110 // INAV: Implemented | DSI: Implemented | For OSD ‘RSSI Value’, For OSD ‘Battery voltage’ etc +#define DJI_MSP_RC_TUNING 111 // INAV: Not implemented | DSI: Implemented | +#define DJI_MSP_PID 112 // INAV: Implemented | DSI: Implemented | For OSD ‘PID roll, yaw, pitch' +#define DJI_MSP_BATTERY_STATE 130 // INAV: Implemented | DSI: Implemented | For OSD ‘Battery current mAh drawn’ etc +#define DJI_MSP_ESC_SENSOR_DATA 134 // INAV: Implemented | DSI: Implemented | For OSD ‘ESC temperature’ +#define DJI_MSP_STATUS_EX 150 // INAV: Implemented | DSI: Implemented | For OSD ‘Fly mode', For OSD ‘Disarmed’ +#define DJI_MSP_RTC 247 // INAV: Implemented | DSI: Implemented | For OSD ‘RTC date time’ + +#define DJI_MSP_SET_FILTER_CONFIG 93 +#define DJI_MSP_SET_PID_ADVANCED 95 +#define DJI_MSP_SET_PID 202 +#define DJI_MSP_SET_RC_TUNING 204 + +void djiOsdSerialInit(void); +void djiOsdSerialProcess(void); + +#endif diff --git a/src/main/io/serial.h b/src/main/io/serial.h index afd2d6c584..bea6abb001 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -52,6 +52,7 @@ typedef enum { FUNCTION_ESCSERIAL = (1 << 18), // 262144: this is used for both SERIALSHOT and ESC_SENSOR telemetry FUNCTION_TELEMETRY_SIM = (1 << 19), // 524288 FUNCTION_FRSKY_OSD = (1 << 20), // 1048576 + FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 16274209a5..485a880353 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -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; } - } - 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) -{ - uint8_t vtxPower; - if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) { - const vtxSettingsConfig_t settings = vtxGetSettings(); - if (vtxPower != settings.power) { - vtxCommonSetPowerByIndex(vtxDevice, settings.power); + if (vtxBand != runtimeSettings->band || vtxChan != runtimeSettings->channel) { + vtxCommonSetBandAndChannel(vtxDevice, runtimeSettings->band, runtimeSettings->channel); return true; } } return false; } -static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) +static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings) +{ + uint8_t vtxPower; + if (!vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) { + return false; + } + + if (vtxPower != runtimeSettings->power) { + vtxCommonSetPowerByIndex(vtxDevice, runtimeSettings->power); + return true; + } + return false; +} + +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); - break; - case VTX_PARAM_BANDCHAN: - if (vtxGetSettings().band) { - vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice); -#if defined(VTX_SETTINGS_FREQCMD) - } else { - vtxUpdatePending = vtxProcessFrequency(vtxDevice); -#endif - } - break; - case VTX_PARAM_PITMODE: - vtxUpdatePending = vtxProcessPitMode(vtxDevice); - break; - case VTX_PARAM_CONFIRM: - vtxUpdatePending = vtxProcessStateUpdate(vtxDevice); - break; - default: - break; - } - currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT; - } while (!vtxUpdatePending && currentSchedule != startingSchedule); + + switch (currentSchedule) { + case VTX_PARAM_POWER: + vtxUpdatePending = vtxProcessPower(vtxDevice, runtimeSettings); + break; + case VTX_PARAM_BANDCHAN: + vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice, runtimeSettings); + break; + case VTX_PARAM_PITMODE: + vtxUpdatePending = vtxProcessPitMode(vtxDevice, runtimeSettings); + break; + case VTX_PARAM_CONFIRM: + vtxUpdatePending = vtxProcessCheckParameters(vtxDevice, runtimeSettings); + break; + default: + break; + } if (!ARMING_FLAG(ARMED) || vtxUpdatePending) { vtxCommonProcess(vtxDevice, currentTimeUs); } + + currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT; } } diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 35500d799f..e3bb6018a6 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -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; diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 4853a6fd06..6e213d7557 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -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, ); diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 3b07168891..e39e447a9f 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -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); diff --git a/src/main/io/vtx_ffpv24g.c b/src/main/io/vtx_ffpv24g.c index 32521f89bf..3cac3acf04 100644 --- a/src/main/io/vtx_ffpv24g.c +++ b/src/main/io/vtx_ffpv24g.c @@ -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; +} + +static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo) +{ + if (!impl_IsReady(vtxDevice)) { + return false; } - else { - state.pitMode = 0; - state.band = 1; - state.channel = 1; - state.frequency = ffpvFrequencyTable[0][0]; - state.powerIndex = 1; - state.powerMilliwatt = 25; - } - 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) diff --git a/src/main/io/vtx_ffpv24g.h b/src/main/io/vtx_ffpv24g.h index d4110b68cf..fbe983b841 100644 --- a/src/main/io/vtx_ffpv24g.h +++ b/src/main/io/vtx_ffpv24g.h @@ -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); \ No newline at end of file diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index d607d553b3..a23d590201 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -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 }; @@ -620,7 +614,7 @@ void saSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; - buf[4] = (mode & 0x3f)|saLockMode; + buf[4] = (mode & 0x3f) | saLockMode; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); @@ -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, }; diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index 754935ead6..ed75c981bd 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -25,8 +25,9 @@ #include "platform.h" #include "build/debug.h" -#define VTX_STRING_BAND_COUNT 5 -#define VTX_STRING_CHAN_COUNT 8 +#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; diff --git a/src/main/io/vtx_string.h b/src/main/io/vtx_string.h index f7a5618411..cb63cbe7ae 100644 --- a/src/main/io/vtx_string.h +++ b/src/main/io/vtx_string.h @@ -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); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index c9c6b7694f..14f0811b31 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -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, }; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index b1308d9192..dfa560b0a0 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -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)) diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 1434ed1a82..b57c3a4c75 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -40,7 +40,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; -static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) +void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) { memset(mspPortToReset, 0, sizeof(mspPort_t)); @@ -452,6 +452,40 @@ static void mspProcessPendingRequest(mspPort_t * mspPort) } } +void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn) +{ + mspPostProcessFnPtr mspPostProcessFn = NULL; + + if (serialRxBytesWaiting(mspPort->port)) { + // There are bytes incoming - abort pending request + mspPort->lastActivityMs = millis(); + mspPort->pendingRequest = MSP_PENDING_NONE; + + // Process incoming bytes + while (serialRxBytesWaiting(mspPort->port)) { + const uint8_t c = serialRead(mspPort->port); + const bool consumed = mspSerialProcessReceivedData(mspPort, c); + + if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) { + mspEvaluateNonMspData(mspPort, c); + } + + if (mspPort->c_state == MSP_COMMAND_RECEIVED) { + mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn); + break; // process one command at a time so as not to block. + } + } + + if (mspPostProcessFn) { + waitForSerialPortToFinishTransmitting(mspPort->port); + mspPostProcessFn(mspPort->port); + } + } + else { + mspProcessPendingRequest(mspPort); + } +} + /* * Process MSP commands from serial ports configured as MSP ports. * @@ -461,39 +495,8 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm { for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t * const mspPort = &mspPorts[portIndex]; - if (!mspPort->port) { - continue; - } - - mspPostProcessFnPtr mspPostProcessFn = NULL; - - if (serialRxBytesWaiting(mspPort->port)) { - // There are bytes incoming - abort pending request - mspPort->lastActivityMs = millis(); - mspPort->pendingRequest = MSP_PENDING_NONE; - - // Process incoming bytes - while (serialRxBytesWaiting(mspPort->port)) { - const uint8_t c = serialRead(mspPort->port); - const bool consumed = mspSerialProcessReceivedData(mspPort, c); - - if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) { - mspEvaluateNonMspData(mspPort, c); - } - - if (mspPort->c_state == MSP_COMMAND_RECEIVED) { - mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn); - break; // process one command at a time so as not to block. - } - } - - if (mspPostProcessFn) { - waitForSerialPortToFinishTransmitting(mspPort->port); - mspPostProcessFn(mspPort->port); - } - } - else { - mspProcessPendingRequest(mspPort); + if (mspPort->port) { + mspSerialProcessOnePort(mspPort, evaluateNonMspData, mspProcessCommandFn); } } } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 536f737fce..0c49509680 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -18,6 +18,7 @@ #pragma once #include "drivers/time.h" +#include "io/serial.h" #include "msp/msp.h" // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 3 MSP ports. @@ -99,7 +100,9 @@ typedef struct mspPort_s { void mspSerialInit(void); +void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); +void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0334bc1147..011e95f488 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -617,12 +617,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 +842,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 +940,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 +1019,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 +1050,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 +1079,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 +1116,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 +1131,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 +1145,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 +1223,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 +1368,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 +1416,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 } @@ -1399,14 +1425,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance), - posControl.wpInitialDistance, posControl.wpInitialDistance / 10, + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), + posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } 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 +1455,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 +1477,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 +1709,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)); @@ -2052,7 +2088,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 @@ -2311,7 +2347,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 { @@ -2368,7 +2404,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 { @@ -2380,7 +2416,7 @@ bool isLandingDetected(void) { bool landingDetected; - if (STATE(FIXED_WING)) { // FIXED_WING + if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY landingDetected = isFixedWingLandingDetected(); } else { @@ -2406,7 +2442,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); @@ -2429,7 +2465,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 { @@ -2439,7 +2475,7 @@ static void resetAltitudeController(bool useTerrainFollowing) static void setupAltitudeController(void) { - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { setupFixedWingAltitudeController(); } else { @@ -2449,7 +2485,7 @@ static void setupAltitudeController(void) static bool adjustAltitudeFromRCInput(void) { - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { return adjustFixedWingAltitudeFromRCInput(); } else { @@ -2462,7 +2498,7 @@ static bool adjustAltitudeFromRCInput(void) *-----------------------------------------------------------*/ static void resetHeadingController(void) { - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { resetFixedWingHeadingController(); } else { @@ -2472,7 +2508,7 @@ static void resetHeadingController(void) static bool adjustHeadingFromRCInput(void) { - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { return adjustFixedWingHeadingFromRCInput(); } else { @@ -2485,7 +2521,7 @@ static bool adjustHeadingFromRCInput(void) *-----------------------------------------------------------*/ static void resetPositionController(void) { - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { resetFixedWingPositionController(); } else { @@ -2498,7 +2534,7 @@ static bool adjustPositionFromRCInput(void) { bool retValue; - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { retValue = adjustFixedWingPositionFromRCInput(); } else { @@ -2596,9 +2632,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; @@ -2766,7 +2802,7 @@ static void processNavigationRCAdjustments(void) posControl.flags.isAdjustingPosition = adjustPositionFromRCInput(); } else { - if (!STATE(FIXED_WING)) { + if (!STATE(FIXED_WING_LEGACY)) { resetMulticopterBrakingMode(); } } @@ -2818,7 +2854,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 { @@ -2891,7 +2927,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; @@ -2974,7 +3010,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); } /*----------------------------------------------------------- @@ -2983,7 +3019,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)); } /*----------------------------------------------------------- @@ -2992,7 +3028,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)); } @@ -3007,7 +3043,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; } @@ -3032,7 +3068,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) { @@ -3072,6 +3108,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; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 758b47a94e..64b8a1d823 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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 { @@ -231,6 +232,7 @@ typedef struct gpsOrigin_s { typedef enum { NAV_WP_ACTION_WAYPOINT = 0x01, + NAV_WP_ACTION_JUMP = 0x06, NAV_WP_ACTION_RTH = 0x04 } navWaypointActions_e; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4f3db5ccd2..005224750f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -586,7 +586,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)) { diff --git a/src/main/rx/msp_override.c b/src/main/rx/msp_override.c index cf526a7d46..f94faa1712 100755 --- a/src/main/rx/msp_override.c +++ b/src/main/rx/msp_override.c @@ -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 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 7ba4884c9c..5ed140f5dd 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -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 @@ -195,6 +195,9 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_SBUS: enabled = sbusInit(rxConfig, rxRuntimeConfig); break; + case SERIALRX_SBUS_FAST: + enabled = sbusInitFast(rxConfig, rxRuntimeConfig); + break; #endif #ifdef USE_SERIALRX_SUMD case SERIALRX_SUMD: @@ -259,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 @@ -324,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; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 3d44a152e0..4444cb847a 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -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 { @@ -79,6 +78,7 @@ typedef enum { SERIALRX_JETIEXBUS = 8, SERIALRX_CRSF = 9, SERIALRX_FPORT = 10, + SERIALRX_SBUS_FAST = 11, } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 @@ -152,7 +152,7 @@ typedef struct rxRuntimeConfig_s { rcReadRawDataFnPtr rcReadRawFn; rcFrameStatusFnPtr rcFrameStatusFn; rcProcessFrameFnPtr rcProcessFrameFn; - rxLinkQualityTracker_e * lqTracker; // Pointer to a + rxLinkQualityTracker_e * lqTracker; // Pointer to a uint16_t *channelData; void *frameData; } rxRuntimeConfig_t; diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index d891b3539b..04bc855697 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -56,7 +56,8 @@ #define SBUS_FRAME_BEGIN_BYTE 0x0F -#define SBUS_BAUDRATE 100000 +#define SBUS_BAUDRATE 100000 +#define SBUS_BAUDRATE_FAST 200000 #if !defined(SBUS_PORT_OPTIONS) #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN) @@ -191,7 +192,7 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return retValue; } -bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint32_t sbusBaudRate) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; static sbusFrameData_t sbusFrameData; @@ -221,7 +222,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) FUNCTION_RX_SERIAL, sbusDataReceive, &sbusFrameData, - SBUS_BAUDRATE, + sbusBaudRate, portShared ? MODE_RXTX : MODE_RX, SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) ); @@ -234,4 +235,14 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) return sBusPort != NULL; } + +bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE); +} + +bool sbusInitFast(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE_FAST); +} #endif diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index b12a030f28..1fcd3680d0 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -20,3 +20,4 @@ #define SBUS_DEFAULT_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 9743731581..618db67221 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -112,7 +112,7 @@ static bool escSensorDecodeFrame(void) escSensorData[escSensorMotor].temperature = telemetryBuffer[0]; escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2]; escSensorData[escSensorMotor].current = ((uint16_t)telemetryBuffer[3]) << 8 | telemetryBuffer[4]; - escSensorData[escSensorMotor].rpm = ((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8]; + escSensorData[escSensorMotor].rpm = computeRpm(((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8]); escSensorDataNeedsUpdate = true; if (escSensorMotor < 4) { @@ -169,7 +169,7 @@ escSensorData_t * escSensorGetData(void) if (usedEscSensorCount) { escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset; escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount; - escSensorDataCombined.rpm = computeRpm((float)escSensorDataCombined.rpm / usedEscSensorCount); + escSensorDataCombined.rpm = (float)escSensorDataCombined.rpm / usedEscSensorCount; } else { escSensorDataCombined.dataAge = ESC_DATA_INVALID; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 1e10d834b4..97d5671d42 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -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 { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f1e7cbd933..27934c549c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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(¬chFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH); - biquadFilterInit(¬chFilterDyn2[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 *)¬chFilterDyn[axis], gyroADCf); - gyroADCf = notchFilterDynApplyFn2((filter_t *)¬chFilterDyn2[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 diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 3d9a3acd3b..51e7fda3af 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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); diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index fd5026f669..3f7f69da27 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -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 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 4ddd3aaf2c..0fa8b8beec 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -180,3 +180,6 @@ #define MAX_PWM_OUTPUT_PORTS 4 #define PCA9685_I2C_BUS BUS_I2C2 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index bcdf88a1e8..9231b8a808 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -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 diff --git a/src/main/target/F4BY/target.mk b/src/main/target/F4BY/target.mk index d5a0a84c3c..0b69d55170 100644 --- a/src/main/target/F4BY/target.mk +++ b/src/main/target/F4BY/target.mk @@ -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 \ diff --git a/src/main/target/FF_F35_LIGHTNING/config.c b/src/main/target/FF_F35_LIGHTNING/config.c index 14c033fb23..4de3455f24 100644 --- a/src/main/target/FF_F35_LIGHTNING/config.c +++ b/src/main/target/FF_F35_LIGHTNING/config.c @@ -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; } diff --git a/src/main/target/FF_PIKOF4/config.c b/src/main/target/FF_PIKOF4/config.c index 6f00b0fccb..5d7d0df06e 100644 --- a/src/main/target/FF_PIKOF4/config.c +++ b/src/main/target/FF_PIKOF4/config.c @@ -28,6 +28,5 @@ void targetConfiguration(void) { rxConfigMutable()->halfDuplex = false; - telemetryConfigMutable()->uartUnidirectional = true; batteryMetersConfigMutable()->current.scale = CURRENTSCALE; } diff --git a/src/main/target/FURYF4OSD/target.c b/src/main/target/FURYF4OSD/target.c index 56881d7c81..e9999e71eb 100644 --- a/src/main/target/FURYF4OSD/target.c +++ b/src/main/target/FURYF4OSD/target.c @@ -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 diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c index 17cbd1df6d..1ba0c8c035 100755 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -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]); diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 6a8dbb95c2..deaed6d8b7 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -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 diff --git a/src/main/target/MAMBAF405US/target.c b/src/main/target/MAMBAF405US/target.c index c79517e87f..d8e1c39129 100644 --- a/src/main/target/MAMBAF405US/target.c +++ b/src/main/target/MAMBAF405US/target.c @@ -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) diff --git a/src/main/target/MAMBAF722/target.c b/src/main/target/MAMBAF722/target.c index 14a8bf172f..6ed573e6b6 100644 --- a/src/main/target/MAMBAF722/target.c +++ b/src/main/target/MAMBAF722/target.c @@ -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) }; diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 63989ba97c..09c3074756 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -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 diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 97e591b617..233ab8c526 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -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 }; diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 101d2d98ea..991fb6c4ab 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -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 diff --git a/src/main/target/SPEEDYBEEF4/target.c b/src/main/target/SPEEDYBEEF4/target.c index 3c5b3162c8..45a98da28b 100644 --- a/src/main/target/SPEEDYBEEF4/target.c +++ b/src/main/target/SPEEDYBEEF4/target.c @@ -24,17 +24,18 @@ #include "drivers/bus.h" const timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 UP(2,1) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 UP(1,2) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 UP(1,7) DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED D(1,0) UP(2,6) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED D(1,0) UP(2,6) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 96ce695a5c..0e7f8e2c53 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -150,8 +150,8 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 6 -#define TARGET_MOTOR_COUNT 6 +#define MAX_PWM_OUTPUT_PORTS 7 +#define TARGET_MOTOR_COUNT 7 #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index b8a29fc50d..e8ff5509e1 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -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 diff --git a/src/main/target/SPRACINGF3NEO/target.mk_ b/src/main/target/SPRACINGF3NEO/target.mk_ index 4e1c1016b8..fa6bb65435 100644 --- a/src/main/target/SPRACINGF3NEO/target.mk_ +++ b/src/main/target/SPRACINGF3NEO/target.mk_ @@ -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 diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index b3a9b6c2a5..04f92232f4 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -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 diff --git a/src/main/target/SPRACINGF4EVO/target.mk b/src/main/target/SPRACINGF4EVO/target.mk index c586a9a061..3e17d6cc12 100755 --- a/src/main/target/SPRACINGF4EVO/target.mk +++ b/src/main/target/SPRACINGF4EVO/target.mk @@ -14,5 +14,4 @@ TARGET_SRC = \ drivers/compass/compass_lis3mdl.c \ drivers/barometer/barometer_ms56xx.c \ drivers/light_ws2811strip.c \ - drivers/max7456.c -# drivers/vtx_rtc6705.c + drivers/max7456.c \ No newline at end of file diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index f114662c40..3f65888364 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -155,16 +155,6 @@ #define SDCARD_SPI_BUS BUS_SPI3 #define SDCARD_CS_PIN PC3 -// disabled for motor outputs 5-8: -//#define USE_VTX_RTC6705 -//#define USE_VTX_RTC6705_SOFTSPI -//#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL - -//#define RTC6705_SPI_MOSI_PIN PB0 // Shared with PWM8 -//#define RTC6705_CS_PIN PB6 // Shared with PWM5 -//#define RTC6705_SPICLK_PIN PB1 // Shared with PWM7 -//#define RTC6705_POWER_PIN PB7 // Shared with PWM6 - #define GYRO_1_CS_PIN SPI1_NSS_PIN #define GYRO_1_SPI_INSTANCE BUS_SPI1 #define GYRO_2_CS_PIN PB2 diff --git a/src/main/target/SPRACINGF7DUAL/target.mk b/src/main/target/SPRACINGF7DUAL/target.mk index 01a33f4d6f..45fb3dd727 100644 --- a/src/main/target/SPRACINGF7DUAL/target.mk +++ b/src/main/target/SPRACINGF7DUAL/target.mk @@ -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_soft_spi.c + drivers/max7456.c \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index c130c09091..5c8803245f 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -107,6 +107,7 @@ #define USE_TELEMETRY_SIM #define USE_FRSKYOSD +#define USE_DJI_HD_OSD #define NAV_NON_VOLATILE_WAYPOINT_CLI diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 37ce3fc62d..c0cf04f148 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -185,7 +185,7 @@ #if !defined(MAG3110_I2C_BUS) #define MAG3110_I2C_BUS MAG_I2C_BUS #endif - BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE); #endif #if defined(USE_MAG_LIS3MDL) diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index d90d85a5b8..92a0faf835 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -19,11 +19,6 @@ #pragma once -// Targets with built-in vtx do not need external vtx -#if defined(VTX) || defined(USE_RTC6705) -# undef USE_VTX_CONTROL -#endif - // Backward compatibility for I2C OLED display #if !defined(USE_I2C) # undef USE_DASHBOARD diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 449e92f625..0e8c9dc2a5 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -490,7 +490,7 @@ void initCrsfTelemetry(void) } #endif #if defined(USE_BARO) || defined(USE_GPS) - if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) { + if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); } #endif diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index d9e5d99bab..9e1f9f3622 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -368,7 +368,7 @@ void configureHoTTTelemetryPort(void) return; } - portOptions_t portOptions = (telemetryConfig()->uartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (SERIAL_NOT_INVERTED); + portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (SERIAL_NOT_INVERTED); hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, portOptions); diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 0f5345cda3..ffb333c72a 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -128,9 +128,9 @@ static ibusAddress_t getAddress(uint8_t ibusPacket[static IBUS_MIN_LEN]) { return (ibusPacket[1] & 0x0F); } -// MANUAL, ACRO, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, LAUNCH, FAILSAFE -static uint8_t flightModeToIBusTelemetryMode1[FLM_COUNT] = { 0, 1, 3, 2, 5, 6, 7, 4, 8, 9 }; -static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 0, 7, 2, 8, 6, 3, 4, 9 }; +// MANUAL, ACRO, AIR, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, CRUISE, LAUNCH, FAILSAFE +static uint8_t flightModeToIBusTelemetryMode1[FLM_COUNT] = { 0, 1, 1, 3, 2, 5, 6, 7, 4, 4, 8, 9 }; +static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8, 6, 3, 3, 4, 9 }; static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { #if defined(USE_GPS) uint8_t fix = 0; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 10901f62d9..e6194797d2 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -467,7 +467,7 @@ void mavlinkSendHUDAndHeartbeat(void) flightModeForTelemetry_e flm = getFlightModeForTelemetry(); uint8_t mavCustomMode; - if (STATE(FIXED_WING)) { + if (STATE(FIXED_WING_LEGACY)) { mavCustomMode = inavToArduPlaneMap[flm]; } else { diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index c3b935af89..8fe6b7c53c 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -132,7 +132,7 @@ static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED; static int simRssi; static uint8_t accEvent = ACC_EVENT_NONE; static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " }; -static char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" }; +static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS" }; static const char gpsFixIndicators[] = { '!', '*', ' ' }; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 2cece39a34..9ac0dce545 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -306,7 +306,7 @@ static void freeSmartPortTelemetryPort(void) static void configureSmartPortTelemetryPort(void) { if (portConfig) { - portOptions_t portOptions = (telemetryConfig()->uartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); + portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 92ce74576f..9949906092 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -64,7 +64,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .frsky_pitch_roll = 0, .report_cell_voltage = 0, .hottAlarmSoundInterval = 5, - .uartUnidirectional = 0, + .halfDuplex = 1, .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH, .ibusTelemetryType = 0, .ltmUpdateRate = LTM_RATE_NORMAL, @@ -190,7 +190,7 @@ void telemetryProcess(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); // since not used by all the telemetry protocols - #if defined(USE_TELEMETRY_FRSKY) +#if defined(USE_TELEMETRY_FRSKY) handleFrSkyTelemetry(); #endif @@ -214,7 +214,7 @@ void telemetryProcess(timeUs_t currentTimeUs) handleJetiExBusTelemetry(); #endif -#if defined(USE_TELEMETRY_IBUS) +#if defined(USE_SERIALRX_IBUS) && defined(USE_TELEMETRY_IBUS) handleIbusTelemetry(); #endif diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 81dc0b1bc7..9b0dc2cb2a 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -64,7 +64,7 @@ typedef struct telemetryConfig_s { uint8_t frsky_pitch_roll; uint8_t report_cell_voltage; uint8_t hottAlarmSoundInterval; - uint8_t uartUnidirectional; + uint8_t halfDuplex; smartportFuelUnit_e smartportFuelUnit; uint8_t ibusTelemetryType; uint8_t ltmUpdateRate; diff --git a/src/test/unit/battery_unittest.cc.txt b/src/test/unit/battery_unittest.cc.txt index 18103c68f1..ef6ea64178 100644 --- a/src/test/unit/battery_unittest.cc.txt +++ b/src/test/unit/battery_unittest.cc.txt @@ -281,10 +281,10 @@ bool feature(uint32_t mask) return false; } -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband) { UNUSED(*rxConfig); - UNUSED(deadband3d_throttle); + UNUSED(mid_throttle_deadband); return THROTTLE_HIGH; } diff --git a/src/test/unit/flight_failsafe_unittest.cc.txt b/src/test/unit/flight_failsafe_unittest.cc.txt index f72b61a7bb..eb4aacef72 100644 --- a/src/test/unit/flight_failsafe_unittest.cc.txt +++ b/src/test/unit/flight_failsafe_unittest.cc.txt @@ -420,10 +420,10 @@ uint32_t millis(void) return sysTickUptime; } -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband) { UNUSED(rxConfig); - UNUSED(deadband3d_throttle); + UNUSED(mid_throttle_deadband); return throttleStatus; }