First part of this fix is freezing the position controller's xy-axis
feed foward as we transition to the new segment.
Second part is work-around for straight line segments in that we allow
the target point to actually overshoot the end of the segment by up to
2m if the segment is a "fast waypoint". Ideally we would instead notice
the waypoint has been completed and take any left over time or distance
and move our target along the track towards the next waypoint but that
would require a much larger change to allow the wpnav lib to hold the
next two waypoints.
We only use the current target position as origin if the waypoint
controller is active (i..e has been used in the past 1 second). This is
consistent with how we initialise straight line waypoints
The problem is that the init_targets is intended to ensure the
controller doesn't build up the I term when the throttle is low. Because
it is poorly named (or used) it have been written to do something else.
Here I change it to do what the code is trying to use it to do.
this fixes a bug caused by GPIO_INPUT and GPIO_OUTPUT already being
defined in NuttX, which caused pinMode() not to setup pins for output
when requested
Analysis of copter logs has shown cases with a healthy EKF where spikes in EKF4.DS of up to 25% of the threshold have occurred.
A value of closer to 10% for normal operation is preferred.
Loiter is only a horizontal position controller so it should not set the
z-axis position.
Moved pos_control.set_speed and accel functions so order matches
init_loiter_targets function order
AC_PosControl::init_xy_controller() has been added to PosControl and is
called by init_loiter_target.
Hybrid is currently using set_loiter_target function to init the loiter
controller. So we have to call init_xy_controller() by set_loiter_target
function.
What happens otherwise?
In AC_PosControl::update_xy_controller, we update "now" with
now = hal.scheduler->millis();
and, as _last_update_xy_ms has not been updated previously by
init_xy_controller(), we just call init_xy_controller().
So, _dt_xy will be negative and used anyways in all the functions and
PID called by update_xy_controller.
That will avoid at least _accel_target.x/y to be set to 0 but I'm not
sure for the high values, probably an I_term that is not reset and
reached very high value.
Or maybe a cast error somewhere... no clue at all
Don't do bias estimation if tilted by more than 60 degrees to prevent scale
factor errors affecting result unnecessarily.
Prevent Kalman gain from having the wrong sign due to numerical errors
associated with small process noise values.
Allow smaller Z accel bias process noise values to be set
Bring Plane glitch protection thresholds into alignment with copter and
rover
Slight increase in accelerometer bias process noise to prevent bias
estimate divergence into limits (Rover and Plane only as Copter does not
seem respond as well to this change)
effective increase in threshold on divergence test to allow increased
margin for bad GPS velocities
Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.
Don't reset filterDiverged status immediately during reset
Set filterDiverged true if covariance blows up
Add fault status reporting
This will enable in-flight magnetometer calibration to be inhibited unconditionally,
This is required for long balloon carriage flights where ground speed can be high
enough to put it into in-air state, but with very poor observability of magnetic field
states causing bad state estimates and heading offsets to develop over time.
The covariance matrix no longer has rows and columns artificially zeroed when in static
mode. Instead booleans indicating whether wind or magentic field state estimation is
active are used to:
a) Set the process noise on these states to zero to stop their variances from increasing
unchecked when not being updated, and
b) Turn off updates for these states when measurement fusion is being performed.
This reduces the likelihood of a badly conditioned covariance matrix forming
during static mode operation.
A filter divergence check has also been added that will declare the filter unhealthy if
position, velocity and magnetic field observations are all failing their innovation
consistency checks. This unhealthy status will persist for 10 seconds after the
condition clears.
AP_NavEKF: Remove unnecessary zeroing of wind covariances
