As they are always moving horizontally, the check doesn't make sense
for fixed-wings.
Also don't run the check while on ground to prevent getting a failed
check during pre-takeoff manipulation.
Calculating K(HP) takes less operations than (KH)P because K and H are
vectors.
Without considering the sparsity optimization:
- KH (24*24 operations) is then a 24x24 matrix an it takes
24^3 operations to multiply it with P. Total: 14400 op
- HP (24*(24+24-1) operations) is a row vector
and it takes 24 operations to left-multiply it by K. Total:1152 op
when the vertical pos or vel innov ratio is above the threshold, the
other one needs to be significant too and not just positive to trigger
the failure
- set local home using global pos and global home
- set local home using GNSS pos and global home
- set global home using global ref of local frame and local home
This is to make clear that the relevant part of the home position
message for navigator is the global one. Local home position isn't
required as everything is done in global coordinates.
The specific home_alt_valid is used when lat/lon are not used
- always try to set local or global home position when possible
- set global home with GNSS position if global pos from EKF isn't
available
- reset home when significantly moved from home before takeoff (checking
lpos or gpos or GNSS)
- reset home on takeoff transition
- reset home on mavlink arm command
- remove "home required accuracy" parameters, rely on validity flags
To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1
To pass from valid to invalid:
- low-passed signed test ratio >= 1
At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.
- Also use the delayed current data instead of newest that might not be
available (gps buffer is sometimes empty if the dt between samples is
larger than the delayed horizon).
- Separate "baro fault" from "baro intermittent": intermittent is a
temporary failure and should prevent from switching to baro right now,
but "fault" means that it should never be used anymore
- In case of height timeout, check for metrics but not for consistency
as the filter is likely to have diverged already.
This fixes the issue when changing the altitude during a goto for
example, where the vehicle was going backwards and upwards to reach the
closest point to the line. Now the vehicle simply goes towards the
target waypoint.
The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.
Since the same parameter is used to generate the trajectory and to
saturate the controller, there is no additional space for the output of
the position controller once it is filled with the feedforward, letting
the altitude grow uncontrolled.
In MPC_YAW_MODE 3 (along trajectory), the flight task is asked to
produce a yaw setpoint. If the previous yaw value was NAN (e.g.: during
early takeoff), the setpoint should then be reset to the current
estimate.
CA: fix saturation computation
Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix
MCRateControl: use control_allocator_status to get saturation info
an airpseed of > 2m/s is required to start the fusion (set by param)
fw mode is also required
Given the larger estimated windspeed after those changes, the change of
static pressure is larger and the height estimate takes more time to
reach the final value
When the change in velocity is exactly the same on several axes, the T1
of the recumputed trajectories after time sync was set to 0 because it
was skipping both if and else if.