Adds in flight monitoring of the rx frame rate and adapts the filters if the frame rate changes. Primarily to add support for Crossfire with its ability to switch from 150hz to 50hz (and back) under some circumstances. Will work with any protocol - not CRSF specific. So if future receivers add the ability to switch frame rates dynamically the logic should support them.
If the current rx frame rate is more than +-20% from the previously detected rate, then the process will retrain for the next 50 samples as long as the rate continues to be outside the 20% tolerance. Once 50 samples are collected the new frame rate is updated and the filter cutoffs are adjusted. Only filters set with their cutoffs = 0 (auto) will be adjusted. There is a 2 second guard time after a successful update before retraining can start again to prevent rapid switching back and forth.
The logic is optimized to not perform any training if the filters are set to manual cutoffs. So there is an opportunity for advanced users to choose specific cutoffs and reduce the PID loop load slightly. However this is not recommended for Crossfire or other protocols that might change their rx frame rate.
Updated the output of the `rc_smoothing_info` cli command to match the revised logic.
Based on feedback from KISS and BLHeli developers the maximum delay for a beacon command is 1020ms. Since there is some built-in delay in sendint the DSHOT beacon command 10 times leaving a little margin for error at 1.2 seconds.
GPS_RESCUE and subsequent changes increased gpsSol.llh.alt from 0.1m per lsb UNIT16 to 0.01m per lsb INT32.
The transfer of altitude data via MSP had to be corrected by factor 10 rescalings to be backwards compatible.
Before GPS_RESCUE it was decimeter (0.1m per lsb) from ublox but meter (1m per lsb) from NMEA, stored as unit16.
Now both protocols should deliver centimeters (0.01m per lsb), stored as int32 in order to cover negative and high altitudes too.
Since introduction of GSP_RESCUE the altitude (from ublox receivers) is logged in cm (0.01m per lsb as int32).
Before that is was scaled in decimeters (0.1m per lsb as uint16) from ublox receivers. But with NMEA protocol it was in meters per lsb.
This will be harmonized in another commit to cm (0.01m per lsb).
Uses the OSD_WARN element rather than a separate separate screen before arming. The "BEACON ON" message will appear for the first 0.5 seconds and then "AM IN x.y" will countdown for the remaining delay.
Also fixed OSD_WARN element blinking related to battery warnings to only blink the warning element if the battery warnings are actually configured.
Provides a clear indication that arming is delayed for cases where DSHOT beacon is active.
Clears the OSD and displays "DISABLING BEACON" and "ARMING IN X.Y" with an active countdown in tenths of a second while arming is delayed due to DSHOT beacon. Once delay period is over the normal "ARMING" message appears.
If the DSHOT beacon is not active then this delay screen is not displayed.
Fixes underflows at negative altitude (below MSL) and overflows at altitude higher than 655.35m
Corrected parenthesis in maths.h avoid incorrect equations if arguments contain expressions.
Tries to prevent DSHOT beacon commands from interfering with commands to set the motor direction.
Adds a 2 second delay after disarming before DSHOT beacon commands will be sent. This attempts to prevent the beacon commands from interfering with the motor direction reset that happens after using crash flip mode.
During arming if a DSHOT beacon command has been sent within 2 seconds the arming will be delayed until the 2 seconds have passed. This attempts to prevent interference with the motor direction commands sent at arming.
Handles cases where the flight controller and receiver are powered but the TX is not. When the TX is powered on and CRSF establishes the link, it will initially run at 50hz for about 0.6 seconds before shifting to 150hz. This change avoids that initialization and link establishment period.
* will require coordination with BFC of course.
* rationale: previously `DEBUG_GYRO_NOTCH` debugging was used to grab the
scaled, unfiltered gyro readings, prior to the FFT running. This has been
updated to `DEBUG_GYRO_SCALED`.
similarly, `DEBUG_GYRO` debugging was used to record the filtered gyro. This
is updated to `GYRO_FILTERED`.
interestingly, `DEBUG_GYRO` was also used for movement threshold calibration.
This has been updated to be `DEBUG_GYRO_CALIBRATION` and also now stores
per-axis standard deviation.
Application of filter position moved slightly for logical grouping, has no
effect due to LTI.
Renamed variables to be more representative of their content.
Simplified the auto-cutoff calculation to be 90% of the nyquist frequency of the measured rx frame rate.
Simplified the PT1 from BIQUAD calculation.
Added active cutoffs to the blackbox log header. Reduce the number of headers by combining like entries into a single line.
Improved the rx frame rate detection/training by delaying calculation to avoid loop time jitter during flight controller initialization.
For auto cutoffs calculate a value appropriate for BIQUAD or PT1 depending on the configuration.
Added a new rc_smoothing_info cli command to display internal details about its operation.
Adds options to select the filter type for both input and derivative.
rc_smoothing_input_type = PT1 | BIQUAD (default is BIQUAD)
rc_smoothing_derivative_type = OFF | PT1 | BIQUAD (default is OFF)
Adds an additional rc channel smoothing algorithm that can be used in place of the default rc interpolation. Utilizing a filter-based approach the smoothing has lower latency and is immune to loop time jitter that can introduce artifacts. Additionally a smoothing filter is added to the setpoint derivative used to produce D-term setpoint weight resulting in a smoother effect on D.
The default setting is to use the previous interpolation logic and there are no changes unless the optional method is selected.
Configuration:
rc_smoothing_type: (INTERPOLATION | FILTER) - defaults to INTERPOLATION
rc_smoothing_input_hz: (0-255) - sets the rc channel input filter cutoff in Hz. Default value of 0 will enable auto calculation based on received RX frame rate.
rc_smoothing_derivative_hz: (0-255) - sets the setpoint weight derivative filter cutoff in Hz. Default value of 0 will enable auto calculation based on received RX frame rate.
rc_smoothing_debug_axis: (ROLL | PITCH | YAW | THROTTLE) - determines which axis is logged in the debug fields
Debug logging:
set debug_mode = RC_SMOOTHING
debug(0) = raw un-smoothed rc channel data
debug(1) = raw un-smoothed setpoint derivative
debug(2) = filtered setpoint derivative before applied to setpoint weight
debug(3) = auto-calculated filter cutoff frequency base after sampling the rx frame rate
Notes:
Currently only enabled for F4/F7 due to flash size limitations
Uses the rc_inter_ch parameter to determine which channels are smoothed (same as default interpolation logic)
The auto filter cutoff calculation will set a cutoff frequency of 30Hz for typical SBUS frames (9ms). 11ms Spektrum will calculate to approximately 25Hz. The user can manually enter the filter cutoffs to be used instead of the auto calculation. The current default calculation was chosen as a good starting point but may be adjusted in the future.
Setting a lower cutoff frequency will result in more smoothing, but also more delay.
There currently isn't any support for receivers that change their rx frame rate dynamically. So for CRSF users wishing to use this alternate smoothing method should change their settings to lock the rx frame rate for now. Support for auto-adjusting to new frame rates in flight will likely be added.
Updates for finalize the flight performance.
Disable if GPS Rescue is active
Constrain the max setpoint calculated by the angle correction logic. To catch border cases where the user activates the feature while inverted and has a high gain which could result in excessively high setpoint rates.
Make sure internal states are reset when feature is activated via mode switch.
Adds a new angle limiting mode for pilots who are learning to fly in acro mode. Primarily targeted at new LOS acro pilots, but can be used with FPV as well.
The feature is activated with a new mode named "ACRO TRAINER". When the feature is active, the craft will fly in normal acro mode but will limit roll/pitch axes so that they don't exceed the configured angle limit. New pilots can start with a small angle limit and progressively increase as their skills improve.
The accelerometer must be enabled for the feature to be configured and function.
Also the feature will only be active while in acro flight and will disable if ANGLE or HORIZON modes are selected.
For most users all they need to do is simply configure the new mode to be active as desired on the "Modes" tab in the configurator and configure the desired angle limit in the cli.
Configuration parameters:
acro_trainer_angle_limit: (range 10-80) Angle limit in degrees.
acro_trainer_lookahead_ms: (range 10-200) Time in milliseconds that the logic will "look ahead" to help minimize overshoot and bounce-back if the limit is approached at high gyro rates. The default value of 50 should be good for most users. For low powered or unresponsive craft (micros or brushed) it may be helpful to increase this setting if you're seeing substantial overshoot.
acro_trainer_debug_axis: (ROLL, PITCH) The axis that will log information if debugging is active.
To enable debugging:
set debug_mode = ACRO_TRAINER
debug(0) - Current angle
debug(1) - The internal logic state
debug(2) - Modified setpoint
debug(3) - Projected angle based gyro rate and lookahead period
The `failsafe_kill_switch` parameter has been renamed to
`failsafe_switch_mode` and it determines what happens
when the Failsafe mode is activated with an AUX switch.
It takes one of three values:
0 - simulates RC signal loss - thus activates Stage1 failsafe
(former behavior when kill switch option was OFF)
1 - disarms immediately
(former behavior when kill switch option was ON)
2 - activates the failsafe procedure (Stage2) immediately (new)
* through extensive testing prior to the beginning of the RC cycle, we have
discovered that the simplest combination of filters appears to be up to four
PT1 filters: two for gyro, and two for d-term.
* non-cascaded biquad filter plumbing is retained for noisy setups and the
dynamic notch bandpass, although gyro and d-term variants of the filtering may
eventually be removed in favor of pt1
* update all related unit tests
s0up
jflyper
Bruce Luckcuck
Diego Basch
Faduf
Michael Keller
AirBreak69
Andrey Mironov
stronnag
Steffen Windoffer
Miguel Angel Mulero Martinez
MJ666
Curtis Bangert
wysiwyng
ctzsnooze
Bryce Johnson
AJ Christensen
Thorsten Laux
jirif
supiiik
Kevin
Cleric-K
borisbstyle