dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
957 Commits 514 Branches 136 Tags 234 MiB
Commit Graph

155 Commits

Author SHA1 Message Date
ChristophTobler 2f382c8e0e if hgt mode is range, switch to baro if range is faulty 2017-06-29 09:47:02 +02:00
ChristophTobler e4f36215cb if in range aid mode, check faultiness that otherwise would never change back 2017-06-29 09:47:02 +02:00
ChristophTobler e800de88b6 remove duplicate 2017-06-29 09:47:02 +02:00
Daniel Agar c44488fdb8
EKF matrix typedef cleanup 2017-06-19 11:10:01 -04:00
ChristophTobler 1fed209804 fix formatting 2017-06-15 09:10:43 +02:00
Roman 5036967343 barometer does not special height sensor offset because a barometer 
offset from the local origin is calculated if the primary source for
height is not baro

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Roman c5d464b821 make range sensor height offset computation more robust 
- when switching to range finder use the current terrain estimate as
height sensor offset, otherwise spikes in the range measurements could lead
to a wrong offset

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Roman 182ea43445 fixed baro offset calculation 
- do not subtract the height sensor offset variable when computing the
baro offset from the local origin. The baro height offset is calculated
when baro is not fused and so the height sensor offset used in that case
is associated to another sensor and has nothing to do with the baro.

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Roman 39983a7d55 range aid: added hysteresis for switching in and out of range aid 
- prevents rapid switching
- added innovation consistency check for using range aid

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Roman 3778f0921a ekf: enable range finder to be used for estimating height even if it's not 
the primary height source

- moved height control into single function in order to decide which sensor
should be used for estimating height
- under certain conditions allow to use the range finder to estimate height
even if it's not the primary height source
- fixed a bug where the delta time for the baro offset calculation was always
zero
- use methods to set height control flags to reduce code duplication and
to prevent bugs

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
Roman 36bffd2571 ekf: calculate the delta time between consecutive baro measurments 
(used for calculating filtered baro offset when primary height source
is not baro)

Signed-off-by: Roman <bapstroman@gmail.com>
 2017-06-15 09:10:43 +02:00
CarlOlsson a1308482fd EKF: Reset the mag states to the field state variance when switching from the inhibit mag mode to standard mode 2017-05-31 10:18:40 +02:00
CarlOlsson be16406b35 EKF: Add feature to use the mag to only update the mag states 2017-05-26 11:29:21 +02:00
Paul Riseborough 05c3c46f83 ekf2: changes arising from code review 2017-05-17 08:59:01 +02:00
Paul Riseborough 394dd95cba EKF: Don't do magnetic field estimation without earth frame measurements 
Use horizontal acceleration to check if yaw is observable independent of the magnetometer.
Use rotation about the vertical to check if mag raises are observable.
If neither yaw of mag biases are observable, save the magnetic field variances and switch to magnetic yaw fusion.
Use the last learned declination when using magnetic yaw fusion so that the yaw reference remains consistent.
When yaw or biases become observable, reinstate the saved variances and switch back to 3D mag fusion.
 2017-05-17 08:59:01 +02:00
Paul Riseborough 5ee0ed5a0d EKF: Increase sensitivity and add tuning of bad accel checks 2017-04-29 11:47:38 +02:00
Paul Riseborough 3758c5a09d EKF: down-sample drag specific force data 2017-04-16 13:07:12 +02:00
Paul Riseborough 465b145929 EKF: Add multi-rotor drag model for wind estimation 2017-04-16 13:07:12 +02:00
Paul Riseborough 2f2ac5be43 EKF: Improve protection against severe IMU accel errors 
Use vertical velocity and position innovation failure to detect bad accelerometer data caused by clipping or aliasing which can cause large vertical acceleration errors and loss of height estimation. When bad accel data is detected:

1) Inhibit accelerometer bias learning
2) Force fusion of vertical velocity and height data
3) Increase accelerometer process noise
 2017-04-07 08:47:13 +10:00
Paul Riseborough 040639837e EKF: Use common timeout test for reporting deadreckoning 
Use a parameter to set the time-out threshold
Use a class variable for the time-out flag
Clean up comments
 2017-03-15 07:57:38 +01:00
Paul Riseborough 6561edb9a5 EKF: Clean up terrain estimation functionality. 
This is a functionally equivalent. It moves all of the code for the terrain estimator into a single function call from the main filter update, making it clear that it is independent of the main filter.
 2017-02-22 14:12:05 +01:00
Paul Riseborough cf9c8de167 EKF: Add arbitrary pitch offset for range sensor 2017-02-22 14:12:05 +01:00
Paul Riseborough 21270bb06a EKF: Use last known position when resuming OF aiding 2017-01-17 08:58:01 +11:00
Paul Riseborough 3c4c09593f EKF: Rationalise console messages 
Combine the observation action buffer and alignment messages
Ensure all data timeout messages are warnings.
Use consistent terminology.
 2016-12-28 10:39:47 +01:00
Paul Riseborough a776b2c549 EKF: Fix bug preventing reset to GPS position when using optical flow 2016-11-26 10:28:58 +01:00
Paul Riseborough 460c9e5250 EKF: Don't try to fuse air data if not in normal aiding mode 
Taking off before passing GPS checks would cause airspeed or sideslip to be fused when the filter was still using a constant position assumption. This would cause large airspeed innovations, invalid wind estimates and degrade filter performance after GPS was gained and position and velocity was reset.
 2016-11-26 10:28:58 +01:00
Paul Riseborough 951c8b77af EKF: Improve wind state reset logic 
Enables wind estimation without an airspeed sensor and enables synthetic sideslip to be used with an airspeed sensor for improved wind state estimation.
Wind states and covariances are reset differently depending on whether airspeed is available.
 2016-11-05 12:19:01 +01:00
Paul Riseborough 352b4ca177 EKF: Allow sideslip fusion with airspeed fusion 2016-11-05 12:19:01 +01:00
CarlOlsson a9182259a5 fixed bug in beta fusion 2016-11-05 12:19:01 +01:00
CarlOlsson e1d9d503e3 updated logic of when to fuse beta. Now synthetic sideslip measurements are fused after 5 seconds of cruise flight with airspeed sensor activated 2016-11-05 12:19:01 +01:00
CarlOlsson dec686afd7 updated parameters 2016-11-05 12:19:01 +01:00
CarlOlsson 5205a8198b added function to control when to fuse beta 2016-11-05 12:19:01 +01:00
Paul Riseborough 8aee45dd96 EKF: Fix bug causing repeated resets if GPS sensor disconnected 2016-11-05 09:36:06 +11:00
Paul Riseborough 968cfae632 EKF: Fix bug preventing fallback from GPS mode if GPS is lost 2016-10-25 08:32:01 +11:00
Roman 63cf3d50be ekf gps reset: do not bother checking for need of reset if gps is not 
available

Signed-off-by: Roman <bapstroman@gmail.com>
 2016-10-24 11:45:36 +02:00
Roman c5f750dcca ekf reset: more granular reset / timeout strategy for gps fusion 
- if both gps position and velocity measurements are rejected for 7 seconds
do a reset
- if only gps position measurements are rejected then wait for 14 seconds
as we still have velocity measurements to constrain the drift in position
- introduced ecl internal parameter for the timeout

Signed-off-by: Roman <bapstroman@gmail.com>
 2016-10-24 11:40:12 +02:00
Roman f80b3481a4 reset the newest output sample for all possible resets 
Signed-off-by: Roman <bapstroman@gmail.com>
 2016-10-12 08:54:38 +02:00
CarlOlsson 51c8715638 fixed issue with airspeed never fused 2016-08-11 15:26:21 +02:00
Paul Riseborough d1d56d9e5e EKF: Add logic to to control wind state estimation 2016-08-11 15:26:21 +02:00
Roman Bapst df0f8fed45 fixed wrong condition on which gps height would be fused (#180) 
- this solves the issue where height innovations would jump
between two regimes

Signed-off-by: Roman <bapstroman@gmail.com>
 2016-08-05 14:45:48 +02:00
Paul Riseborough eb2afc522d EKF: fix bug preventing use of baro as a backup height source 2016-06-30 17:06:32 +10:00
Paul Riseborough 573f252b73 EKF: fix bug preventing GPS use as a backup height source 2016-06-30 17:06:32 +10:00
Paul Riseborough 5f9752cac8 EKF: Improve function names 2016-06-30 17:06:32 +10:00
Paul Riseborough 7bc9217f00 EKF: Clean up control of observation fusion 
All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon.
Information and warning messages are improved.
 2016-06-30 17:06:32 +10:00
Paul Riseborough 3c84d37175 Merge pull request #156 from PX4/fix_printfs 
EKF: use ECL printfs everywhere
 2016-06-07 09:45:00 +10:00
Paul Riseborough 081e17729c EKF: delay commencement of 3D mag fusion until clear of ground 
Wait until enough height has been gained to be clear of ground based magnetic anomalies. Failure to do so can result in incorrect earth field initialisation.
 2016-06-06 21:59:46 +10:00
Paul Riseborough f08aee0a7d EKF: Fix covariance index bug 2016-06-04 00:36:07 +10:00
Julian Oes b8e2f79005 EKF: correct include paths 2016-06-02 16:29:22 +01:00
Julian Oes ecfd8c867a EKF: use ECL printfs everywhere 
- Changes all printfs to ECL printfs
- Add ECL_ERR.
- Include ecl.h where needed.
- Add forgotten pragma once.
 2016-06-02 16:07:26 +01:00
Paul Riseborough 6a55d908c5 EKF: replace reset event times with event counters 
Using a 64bit integer was unnecessary given it was only being used to detect a new reset event.
 2016-06-01 17:13:00 +10:00
Paul Riseborough 3ec9221c18 EKF: update output observer and capture reset event for EV yaw resets 2016-06-01 17:13:00 +10:00
Paul Riseborough 9f81b8f09e EKF: provide reset protection for external vision height 2016-05-27 13:15:53 +10:00
Paul Riseborough 1b6c5bbafd EKF: Enable height source to be selected independent of EV aiding 2016-05-27 13:14:52 +10:00
Carl Olsson 0fafc49a49 fixed typos (#147) 2016-05-22 17:40:43 +02:00
Paul Riseborough c1b02eaa91 EKF: Don't use 3-axis magnetometer fusion until the tilt is aligned 2016-05-18 21:06:45 +10:00
Paul Riseborough aaac867da8 EKF: Adjust tilt alignment threshold 2016-05-18 20:34:12 +10:00
Paul Riseborough 1540e937b1 EKF: Improve tilt alignment monitoring 
Convert quaternion covariances into an angular alignment variance vector and discard the z component so that yaw uncertainty does not affect the result.
 2016-05-17 11:17:24 +10:00
Paul Riseborough 920d83d68c EKF: Fix bugs preventing use of external vision yaw data 2016-05-14 21:56:41 +10:00
Paul Riseborough 13c3a95bc1 EKF: Add missing line returns 2016-05-14 21:17:30 +10:00
Paul Riseborough b985e58333 EKF: clean up control function 
With the addition of new observation types, the control function has become too large and needed be broken up into separate functions
 2016-05-14 21:17:29 +10:00
Paul Riseborough f4a0f69f6e EKF: print to console when starting EV fusion 2016-05-14 21:17:29 +10:00
Paul Riseborough ac9b7a3df6 EKF: Ensure use of EV aiding inhibits use of other height sources 2016-05-14 21:17:29 +10:00
Paul Riseborough e917d6c7f2 EKF: Add fusion of external yaw data 2016-05-14 21:17:29 +10:00
Paul Riseborough 37a09c61bc EKF: Don't use delayed data to start EV aiding 2016-05-14 21:17:29 +10:00
Paul Riseborough 81469d6621 EKF: Add position, height and velocity reset for EV aiding 2016-05-14 21:17:29 +10:00
Paul Riseborough 481c624975 EKF: Remove use of vehicle arm status 
Use single externally set in-air status for all decisions
 2016-05-10 10:23:08 +10:00
Paul Riseborough 161ac2e051 EKF: ensure position co-variances are reset 
When transitioning into optical flow nav  from a non-aiding condition during ground operation, ensure position covariances are reset as they could be invalid.
 2016-05-08 18:03:09 +10:00
Paul Riseborough 724280fd1f EKF: move calculation of optical flow observation variance into a function 
Allows it to be used when calculating initial state variance
 2016-05-08 15:40:06 +10:00
Paul Riseborough 25682dce91 EKF: Prevent badly conditioned covariance calculation when starting or resetting to optical flow 2016-05-08 10:30:40 +10:00
Paul Riseborough da9d894441 EKF: Improvements to covariance reset 2016-05-07 12:57:41 +10:00
Paul Riseborough a7d08d08fc EKF: Relax tilt alignment check 2016-05-05 21:23:03 +10:00
Paul Riseborough 727a43764f EKF: update initial angle alignment check 2016-05-05 21:23:03 +10:00
Paul Riseborough fe9f88a8b4 EKF: test new derivation 
Use direct attitude parameterisation
Discard scale factors
Add accel bias
 2016-05-05 21:23:03 +10:00
Paul Riseborough 3e81b86280 EKF: improve height reset logic 
Don't use failed sensors unless necessary and handle case where not height sensor is available for reset
 2016-04-22 10:50:53 +10:00
Paul Riseborough 874558d194 EKF: improve detection of bad vert accel data 
Improve ability to detect if bad vertical accel data has caused loss of height accuracy by using historical data.
 2016-04-22 09:24:04 +10:00
Paul Riseborough e3365525c2 EKF: rework height reset logic 2016-04-22 08:39:24 +10:00
Paul Riseborough a7417657c3 EKF: ensure fusion timeout counters are reset when required 2016-04-22 08:33:11 +10:00
Roman Bapst 99fc61c27c ekf2 airspeed fusion: 
- finished logic for fusion
- fixed bug where previous control status was set in the wrong location
 2016-04-19 09:53:31 +02:00
Paul Riseborough 006b6b58e4 EKF: fix bug in status print statement 2016-04-12 10:38:50 +10:00
Lorenz Meier f86ef34782 EKF: Fix error message which lied 2016-04-05 19:06:35 -07:00
Paul Riseborough 6b2e2dba90 EKF: Add GPS height option and improve height recovery 2016-03-16 10:40:51 +11:00
Paul Riseborough d2407c3463 EKF: code style updates 2016-03-13 21:17:51 +11:00
Paul Riseborough c58ab3e256 EKF: Enable fallback to baro alt when using range finder for height 2016-03-13 21:17:51 +11:00
Paul Riseborough 370f643f42 EKF: Enable use of range finder for primary height source 2016-03-13 18:44:34 +11:00
Paul Riseborough 109e0e6dfc EKF: When commencing GPS aiding, don't reset local position and velocity if using optical flow 2016-03-11 11:03:43 +11:00
Paul Riseborough d97d308ca7 EKF: Add control of optical flow and range finder fusion 2016-03-11 11:03:43 +11:00
Roman Bapst b2744f965d use in_air flag from land detector to determine if we are on the ground 2016-03-03 10:53:39 +01:00
Paul Riseborough d8627c6aaf EKF: Reset the vertical position states if height fusion times out 2016-03-02 08:39:07 +01:00
Paul Riseborough d5e47d21db EKF: miscellaneous comment and format changes 2016-03-01 15:35:45 +11:00
Paul Riseborough 7c83ac4b6d EKF: Use heading fusion as default for startup and ground operation 2016-03-01 15:35:17 +11:00
Paul Riseborough 5b5bddebea EKF: Always reset yaw and mag field states on entry into 3-axis mag fusion mode 2016-02-25 08:16:42 +11:00
Paul Riseborough 2d09a5f3ac EKF: Don't reset yaw and mag field states when not necessary 2016-02-25 08:16:42 +11:00
Paul Riseborough a711632017 EKF: Add method to fuse horizontal magnetometer data 
This method is more suitable than a raw heading measurement because it works across a full range of pitch angles.
It has been made the default for ground operation.
 2016-02-20 19:45:32 +11:00
Paul Riseborough d9bf4e9870 EKF: Enable control mode transitions to be detected 
Save the previous value of the filter control modes
 2016-02-16 11:08:30 +11:00
Paul Riseborough 7da40a45a4 EKF: Add control mode for wind state estimation 2016-02-16 11:08:30 +11:00
Paul Riseborough dba58aa4c6 EKF: Add control logic for fusion modes 2016-02-14 22:01:53 +01:00
Paul Riseborough 22c177c951 EKF: Re-align yaw and magnetic field states when changing into GPS aiding mode 2016-02-14 22:01:53 +01:00
Paul Riseborough c089079321 EKF: Split tilt and yaw align 2016-02-14 22:01:53 +01:00
Paul Riseborough 020cc5978e EKF: Move calculation of declination into a separate function 2016-02-14 22:01:53 +01:00
Paul Riseborough 5ea0ef39cd EKF: Update declination fusion logic 
We do not need to run declination fusion if 3-axis mag fusion is not being used.
 2016-02-09 07:54:24 +11:00