Without range finder, the validity flag goes to false quite quickly and
if can be that a vehicle never starts HTE is the takeoff is too slow.
In this specific context of takeoff detection, since
the exact value is not important, we can safely ignore the validity flag.
the _landed flag only is not enough to tell if the drone is still touching
the ground, so an additional check based on the altitude AGL is required.
to have this in_air detection correctly updated, the module needs to be
scheduled on the vehicle_local_position message as no setpoint is
produced in non-assisted modes.
In the predition step, the process variance was erroneously
multiplied by dt instead of dt^2. The default values are adjusted
accordingly to keep the same tuning for the default loop rate of 50Hz
120 seconds of learning time in 3D fusion mode was too long for most normal flights. The learned bias is usually really good after a shorter period and was not used to update the parameters. 30s seems to be a good compromise.
Because the velocity starts with a positive value (down) to ramp-up
slowly the motors, the position setpoint (integral) was going down into
the ground, canceling the input of the velocity controller during the
early takeoff phase.
Instead of a smooth ramp, the behavior was a delay followed by a quick
takeoff or a land detection.
Setting the current altitude setpoint with the current altitude during
this phase avoids the canceling effect and creates a takeoff purely
velocity-based.
During takeoff, Navigator is sending a constant yaw value.
However, there is always a reset at 1.5m of the yaw estimate in EKF2
that is not handled by Navigator that produces a glitch in the rate
controller. Given that in FlightTask, the yaw
is already computed and properly corrected in case of an estimator reset
event, we just ignore the yaw value sent by navigator during takeoff.
This fixes the corner case where a NAV_DELAY command changes the
validity of the next WP but not the rest of the triplet. The logic in
FlightTask was ignoring this change because the check was only based on
WP position change.
HTE runs based on the position controller so, even if we whish to use
the estimate, it is only available in altitude and position modes.
Therefore, we need to always initialize the hoverThrottle using the hover
thrust parameter in case we fly in stabilized
* FlightTask: Fix ekf2 reset race condition during task switch
During a loss of GPS data when using GPS as primary height source,
the height is reset to baro and the local position gets invalid at the
same time. This triggers a switch to altitude flight task and a setpoint
reset.
This combination of events had the effect to ignore the height reset,
the large sudden height error could create an abrupt change of altitude
or even a crash.
The ekf2 reset is now done at the beginning of each update call.
The hover thrust estimator (HTE) starts to run after the first thrust
setpoint is published. Until then, the feedforward of the vertical
velocity controller was unitialized (= 0). This is now set to hover
thrust parameter until the estimate is available.
During the first few seconds after takeoff, the failure detector is allowed to
trigger motor lockdown.
This is done for safety reasons to detect tipping-over or unstable
tuning gains
This function updates the vertical velocity integrator with the change
in hover thrust to avoid propagating discontinuities through the
controller when changing the hover thrust.
This is also important when using the hover thrust estimator as its
estimate has unconstrained dynamics and can cause drops or kicks when
the estimate updates faster than the velocity integrator.
- Use a low-passed value of the signed innovation test ratio to trigger
the state variance boost. The threshold of 0.2 has been chosen using log
replay and simulation scenarii.
- Do not reset the learned accel noise during a state variance boost.
After a few tests, this does not seem to help at all.
- Continue to learn the accel noise even if the measurement got rejected
to avoid ignoring sudden changes of noise
- Lower the acceleration noise time constant and increase min/max
values to avoid learning quickly a small variance that could temporarly
destabilize the filter
- Update filter time constants. Increasing the speed of the residual lpf
improves the quality of the learned accel noise
This is done to test the recovery function of the estimator in case
of divergence or sudden extreme hover thrust change.
Also specify seed of random generator
with measurement noise auto-tuning
The purpose of this estimator is to improve land detection and vertical
velocity feedforward
Recovery strategy:
This is required when the setpoint suddenly changes in air or that the
EKF is diverging. A lowpassed test ratio is used as a trigger for the recovery logic
Also, a lowpassed residual is used to estimate the steady-state value
and remove it when estimating the accel noise to avoid increasing the
accel noise when the redisual is caused by an offset.
* NotchFilter: add NotchFilter template and test for float type
* NotchFilterTest: add test for Vector3f notch filter
* PX4Gyroscope: add notch filter with IMU_GYRO_NF_(FREQ|BW) parameters to
set the notch center frequency and bandwidth
used back in time when no soothing was done to avoid large steps when
the estimator stops sending a maximum velocity (e.g.:GPS gained during
optical flow flight). Since smoothing is applied later, this is not
required anymore.
The constrainAbs function was not prioritizing the minimum value
that produces the autocontinue behaviour. This caused zig-zag
paths when the waypoints were almost -but not exactly- aligned.
dt could drive the alpha filter crazy because of the small dt
approximation during the computation of the coefficient.
- Remove unused bitmask packing of the innovation checks
* ekf2: Add FirstOrderLpf and InnovationLpf classes for innovation lowpass filtering
* ekf2: use InnovLpf filter class in preflight checks
* ekf2: move selection of yaw test limit for pre-flight check in function
* ekf2: Move pre-flight checks into separate function
* ekf2: use static constexpr insetead of inline for sq (square) function
* ekf2: Split pre-flight checks in separate functions
Also use the same check for all the innovations:
innov_lpf < test and innov < 2xtest
* ekf2: Add optical flow pre-flight check
* ekf2: Combine FirstOrderLpf and InnovationLpf in single class
* ekf2: check vel_pos_innov when ev_pos is active as well
* ekf2: transform InnovationLpf into a header only library and pass the
spike limit during the update call to avoid storing it here
* ekf2: Static and const cleanup
- set spike_lim constants as static constexpr, set innovation
- set checker helper functions as static
- rename the mix of heading and yaw as heading to avoid confusion
* ekf2: use ternary operator in selectHeadingTestLimit instead of if-else
* ekf2: store intermediate redults in const bool flags. Those will be used for logging
* ekf2: set variable const whenever possible
* ekf2: create PreFlightChecker class that handle all the innovation
pre-flight checks.
Add simple unit testing
Use bitmask instead of general flag to have more granularity
* PreFlightChecker: use setter for the innovations to check instead of sending booleans in the update function
This makes it more scalable as more checks will be added
* ekf: Use booleans instead of bitmask for ekf preflt checks
Rename "down" to "vert"
part to avoid having to scale its saturation separately. This is
required to avoid premature saturation of the integrator when using
the K term.
Also remove double saturation of the integrator
A NAN is interpreted in the FlightTaskAuto as a non-valid global coordinate and sets a local position.
If a zero is sent instead, the global coordinate is recognized as valid and will be executed.
This is a problem when the global position is gained for the first time after takeoff and that a valid global reference did not exist before
Also split a few functions into smaller ones for readability, fix
formatting, use geters to get the current state of the trajectory
instead of return arguments.
This helps when the current velocity is smaller than the target but that
the acceleration is too large such that the velocity will overshoot.
Without this check, the algorithm increases the acceleration which leads
to an even larger overshoot.
This solves many numerical issues when the trajectory is close to the
primary NE axes (small velocities). It is also more robust when dt is
large and has some jitter.
bresch