Large magnetometer innovations on the ground could be caused by factors that will disappear when flying, eg:
a) Bad initial gyro bias
b) External magnetic field disturbances (adjacent metal structures, placement of hatches with magnets, etc)
To avoid unnecessary switches, we inhibit switching until off-ground and when sufficient time has lapsed from power on to learn gyro bias offsets.
If the magnetometer fails innovation consistency checks for too long (currently 10 sec), then the next available sensor approved for yaw measurement will be used.
The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis.
This was not achieved with the previous implementation.
These changes move the innovation consistency checks for all three axes to the top before any axes are fused.
Unnecessary performance timers have been removed.
This was problematic to implement with magnetometer switching. It is likely that slow magnetometer learning can still be performed externally (eg plane) but this will need to be monitored to see if it causes issues.
- moved terrain alt rangefinder power-off trigger from trerrain thread to rangefinder thread
- allow rangefinder to power-off using baro if terrain data not available
instead of computing the terrain status on-demand, assign it in update() and cache the result. Then external tasks that check the status won't be doing terrain intensive calculations in their thread. All the calculations needed for the status were being performed in update already so this is an optimization.
There is no crosstrack concept in the loiter navigation so when going from waypoint to loiter you will not converge onto the line between those two points. This commit adds crosstracking by performing normal waypoint navigation until you get near it.
Ensures that the latest GPS data is used to reset the states.
Separates the logic used to set the origin from the logic used to determine when to reset states and commence GPS aiding
The setting of the EKF origin and the entry into GPS aiding mode have been separated to make the logic clear.
The order of operations has been changed to ensure that when a reset to GPS is performed, a valid GPS measurement is available in the buffer
Declaration of GPS availability is not made unless the GPS data has been entered into the buffer
Only applied to interfaces required for data logging.
If an invalid instance is requested, the data for the primary instance is returned. This allows the primary data to be returned by calling with a -1 instance value.
Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive
Prevents frame over-runs due to simultaneous fusion of measurements on each instance.
The offset is only applied if less than 5msec available between frames
