2015-09-21 02:18:49 -03:00
# include <AP_HAL/AP_HAL.h>
# if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
# include "AP_NavEKF2_core.h"
# include <AP_Vehicle/AP_Vehicle.h>
2015-09-22 21:27:56 -03:00
# include <GCS_MAVLink/GCS.h>
2016-05-03 19:23:51 -03:00
# include <DataFlash/DataFlash.h>
2015-09-21 02:18:49 -03:00
/*
parameter defaults for different types of vehicle . The
APM_BUILD_DIRECTORY is taken from the main vehicle directory name
2015-11-03 09:46:29 -04:00
where the code is built .
2015-09-21 02:18:49 -03:00
*/
2016-05-03 19:23:51 -03:00
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_Replay)
2015-09-21 02:18:49 -03:00
// copter defaults
2016-05-24 23:06:04 -03:00
# define VELNE_M_NSE_DEFAULT 0.5f
# define VELD_M_NSE_DEFAULT 0.7f
# define POSNE_M_NSE_DEFAULT 1.0f
# define ALT_M_NSE_DEFAULT 3.0f
# define MAG_M_NSE_DEFAULT 0.05f
# define GYRO_P_NSE_DEFAULT 3.0E-02f
# define ACC_P_NSE_DEFAULT 6.0E-01f
# define GBIAS_P_NSE_DEFAULT 1.0E-04f
2016-05-21 09:10:32 -03:00
# define GSCALE_P_NSE_DEFAULT 5.0E-04f
2016-06-30 08:12:13 -03:00
# define ABIAS_P_NSE_DEFAULT 5.0E-03f
2016-07-04 11:21:09 -03:00
# define MAGB_P_NSE_DEFAULT 1.0E-04f
# define MAGE_P_NSE_DEFAULT 1.0E-03f
2016-05-24 23:06:04 -03:00
# define VEL_I_GATE_DEFAULT 500
# define POS_I_GATE_DEFAULT 500
# define HGT_I_GATE_DEFAULT 500
# define MAG_I_GATE_DEFAULT 300
2015-09-21 02:18:49 -03:00
# define MAG_CAL_DEFAULT 3
# define GLITCH_RADIUS_DEFAULT 25
# define FLOW_MEAS_DELAY 10
2016-05-24 23:06:04 -03:00
# define FLOW_M_NSE_DEFAULT 0.25f
# define FLOW_I_GATE_DEFAULT 300
2015-11-12 05:39:15 -04:00
# define CHECK_SCALER_DEFAULT 100
2015-09-21 02:18:49 -03:00
# elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
// rover defaults
2016-05-24 23:06:04 -03:00
# define VELNE_M_NSE_DEFAULT 0.5f
# define VELD_M_NSE_DEFAULT 0.7f
# define POSNE_M_NSE_DEFAULT 1.0f
# define ALT_M_NSE_DEFAULT 3.0f
# define MAG_M_NSE_DEFAULT 0.05f
# define GYRO_P_NSE_DEFAULT 3.0E-02f
# define ACC_P_NSE_DEFAULT 6.0E-01f
# define GBIAS_P_NSE_DEFAULT 1.0E-04f
2016-05-21 09:10:32 -03:00
# define GSCALE_P_NSE_DEFAULT 5.0E-04f
2016-06-30 08:12:13 -03:00
# define ABIAS_P_NSE_DEFAULT 5.0E-03f
2016-07-04 11:21:09 -03:00
# define MAGB_P_NSE_DEFAULT 1.0E-04f
# define MAGE_P_NSE_DEFAULT 1.0E-03f
2016-05-24 23:06:04 -03:00
# define VEL_I_GATE_DEFAULT 500
# define POS_I_GATE_DEFAULT 500
# define HGT_I_GATE_DEFAULT 500
# define MAG_I_GATE_DEFAULT 300
2015-10-16 09:18:23 -03:00
# define MAG_CAL_DEFAULT 2
2015-09-21 02:18:49 -03:00
# define GLITCH_RADIUS_DEFAULT 25
2015-09-24 04:40:18 -03:00
# define FLOW_MEAS_DELAY 10
2016-05-24 23:06:04 -03:00
# define FLOW_M_NSE_DEFAULT 0.25f
# define FLOW_I_GATE_DEFAULT 300
2015-11-12 05:39:15 -04:00
# define CHECK_SCALER_DEFAULT 100
2015-09-21 02:18:49 -03:00
2015-10-30 07:57:30 -03:00
# elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// plane defaults
2016-05-24 23:06:04 -03:00
# define VELNE_M_NSE_DEFAULT 0.5f
# define VELD_M_NSE_DEFAULT 0.7f
# define POSNE_M_NSE_DEFAULT 1.0f
# define ALT_M_NSE_DEFAULT 3.0f
# define MAG_M_NSE_DEFAULT 0.05f
# define GYRO_P_NSE_DEFAULT 3.0E-02f
# define ACC_P_NSE_DEFAULT 6.0E-01f
# define GBIAS_P_NSE_DEFAULT 1.0E-04f
2016-05-21 09:10:32 -03:00
# define GSCALE_P_NSE_DEFAULT 5.0E-04f
2016-06-30 08:12:13 -03:00
# define ABIAS_P_NSE_DEFAULT 5.0E-03f
2016-07-04 11:21:09 -03:00
# define MAGB_P_NSE_DEFAULT 1.0E-04f
# define MAGE_P_NSE_DEFAULT 1.0E-03f
2016-05-24 23:06:04 -03:00
# define VEL_I_GATE_DEFAULT 500
# define POS_I_GATE_DEFAULT 500
# define HGT_I_GATE_DEFAULT 500
# define MAG_I_GATE_DEFAULT 300
2015-10-16 09:18:23 -03:00
# define MAG_CAL_DEFAULT 0
2015-09-21 02:18:49 -03:00
# define GLITCH_RADIUS_DEFAULT 25
2015-09-24 04:40:18 -03:00
# define FLOW_MEAS_DELAY 10
2016-05-24 23:06:04 -03:00
# define FLOW_M_NSE_DEFAULT 0.25f
# define FLOW_I_GATE_DEFAULT 300
2015-11-12 05:39:15 -04:00
# define CHECK_SCALER_DEFAULT 150
2015-09-21 02:18:49 -03:00
2015-10-30 07:57:30 -03:00
# else
// build type not specified, use copter defaults
2016-05-24 23:06:04 -03:00
# define VELNE_M_NSE_DEFAULT 0.5f
# define VELD_M_NSE_DEFAULT 0.7f
# define POSNE_M_NSE_DEFAULT 1.0f
# define ALT_M_NSE_DEFAULT 3.0f
# define MAG_M_NSE_DEFAULT 0.05f
# define GYRO_P_NSE_DEFAULT 3.0E-02f
# define ACC_P_NSE_DEFAULT 6.0E-01f
# define GBIAS_P_NSE_DEFAULT 1.0E-04f
2016-05-21 09:10:32 -03:00
# define GSCALE_P_NSE_DEFAULT 5.0E-04f
2016-06-30 08:12:13 -03:00
# define ABIAS_P_NSE_DEFAULT 5.0E-03f
2016-07-04 11:21:09 -03:00
# define MAGB_P_NSE_DEFAULT 1.0E-04f
# define MAGE_P_NSE_DEFAULT 1.0E-03f
2016-05-24 23:06:04 -03:00
# define VEL_I_GATE_DEFAULT 500
# define POS_I_GATE_DEFAULT 500
# define HGT_I_GATE_DEFAULT 500
# define MAG_I_GATE_DEFAULT 300
2015-10-30 07:57:30 -03:00
# define MAG_CAL_DEFAULT 3
# define GLITCH_RADIUS_DEFAULT 25
# define FLOW_MEAS_DELAY 10
2016-05-24 23:06:04 -03:00
# define FLOW_M_NSE_DEFAULT 0.25f
# define FLOW_I_GATE_DEFAULT 300
2015-11-12 05:39:15 -04:00
# define CHECK_SCALER_DEFAULT 100
2015-10-30 07:57:30 -03:00
2015-09-21 02:18:49 -03:00
# endif // APM_BUILD_DIRECTORY
2015-11-04 21:22:49 -04:00
extern const AP_HAL : : HAL & hal ;
2015-09-21 02:18:49 -03:00
// Define tuning parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo NavEKF2 : : var_info [ ] = {
2015-09-21 02:18:49 -03:00
2015-09-22 20:57:02 -03:00
// @Param: ENABLE
// @DisplayName: Enable EKF2
2015-11-11 00:16:36 -04:00
// @Description: This enables EKF2. Enabling EKF2 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_TYPE=2. A reboot or restart will need to be performed after changing the value of EK2_ENABLE for it to take effect.
2015-09-22 20:57:02 -03:00
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
2016-01-05 01:40:43 -04:00
AP_GROUPINFO_FLAGS ( " ENABLE " , 0 , NavEKF2 , _enable , 1 , AP_PARAM_FLAG_ENABLE ) ,
2015-09-23 20:38:54 -03:00
// GPS measurement parameters
// @Param: GPS_TYPE
// @DisplayName: GPS mode control
2016-10-05 02:34:01 -03:00
// @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = Inhibit GPS use - this can be useful when flying with an optical flow sensor in an environment where GPS quality is poor and subject to large multipath errors.
// @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS
2015-09-23 20:38:54 -03:00
// @User: Advanced
AP_GROUPINFO ( " GPS_TYPE " , 1 , NavEKF2 , _fusionModeGPS , 0 ) ,
2015-09-23 04:29:28 -03:00
2016-05-24 23:06:04 -03:00
// @Param: VELNE_M_NSE
2015-11-11 00:16:36 -04:00
// @DisplayName: GPS horizontal velocity measurement noise (m/s)
// @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set horizontal velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS horizontal velocity measurements.
2015-09-21 02:18:49 -03:00
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " VELNE_M_NSE " , 2 , NavEKF2 , _gpsHorizVelNoise , VELNE_M_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: VELD_M_NSE
2015-11-11 00:16:36 -04:00
// @DisplayName: GPS vertical velocity measurement noise (m/s)
// @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set vertical velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS vertical velocity measurements.
2015-09-21 02:18:49 -03:00
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " VELD_M_NSE " , 3 , NavEKF2 , _gpsVertVelNoise , VELD_M_NSE_DEFAULT ) ,
2015-09-23 20:38:54 -03:00
2016-05-24 23:06:04 -03:00
// @Param: VEL_I_GATE
2015-11-11 00:16:36 -04:00
// @DisplayName: GPS velocity innovation gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
2015-09-23 20:38:54 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " VEL_I_GATE " , 4 , NavEKF2 , _gpsVelInnovGate , VEL_I_GATE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: POSNE_M_NSE
2015-09-21 02:18:49 -03:00
// @DisplayName: GPS horizontal position measurement noise (m)
2015-11-11 00:16:36 -04:00
// @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements.
2015-09-21 02:18:49 -03:00
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " POSNE_M_NSE " , 5 , NavEKF2 , _gpsHorizPosNoise , POSNE_M_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: POS_I_GATE
2015-09-23 20:38:54 -03:00
// @DisplayName: GPS position measurement gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " POS_I_GATE " , 6 , NavEKF2 , _gpsPosInnovGate , POS_I_GATE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2015-09-23 20:38:54 -03:00
// @Param: GLITCH_RAD
// @DisplayName: GPS glitch radius gate size (m)
2015-11-11 00:16:36 -04:00
// @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position.
// @Range: 10 100
2015-09-23 20:38:54 -03:00
// @Increment: 5
2015-09-21 02:18:49 -03:00
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m
2015-09-23 20:38:54 -03:00
AP_GROUPINFO ( " GLITCH_RAD " , 7 , NavEKF2 , _gpsGlitchRadiusMax , GLITCH_RADIUS_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
// @Param: GPS_DELAY
// @DisplayName: GPS measurement delay (msec)
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
2015-10-18 19:04:51 -03:00
// @Range: 0 250
2015-09-21 02:18:49 -03:00
// @Increment: 10
// @User: Advanced
2016-12-30 01:16:44 -04:00
// @Units: milliseconds
2015-10-24 07:46:23 -03:00
AP_GROUPINFO ( " GPS_DELAY " , 8 , NavEKF2 , _gpsDelay_ms , 220 ) ,
2015-09-21 02:18:49 -03:00
2015-09-23 20:38:54 -03:00
// Height measurement parameters
2015-09-21 02:18:49 -03:00
2015-09-23 20:38:54 -03:00
// @Param: ALT_SOURCE
2017-01-10 11:33:29 -04:00
// @DisplayName: Primary altitude sensor source
2016-10-25 17:54:29 -03:00
// @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK2_GPS_TYPE = 3). Setting 2 uses GPS. Setting 3 uses the range beacon data. NOTE - the EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
2015-09-21 02:18:49 -03:00
// @User: Advanced
2015-11-12 03:31:50 -04:00
AP_GROUPINFO ( " ALT_SOURCE " , 9 , NavEKF2 , _altSource , 0 ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: ALT_M_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Altitude measurement noise (m)
2015-11-11 00:16:36 -04:00
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors.
2015-09-23 20:38:54 -03:00
// @Range: 0.1 10.0
// @Increment: 0.1
2015-09-21 02:18:49 -03:00
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " ALT_M_NSE " , 10 , NavEKF2 , _baroAltNoise , ALT_M_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: HGT_I_GATE
2015-09-21 02:18:49 -03:00
// @DisplayName: Height measurement gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " HGT_I_GATE " , 11 , NavEKF2 , _hgtInnovGate , HGT_I_GATE_DEFAULT ) ,
2015-09-23 20:38:54 -03:00
// @Param: HGT_DELAY
// @DisplayName: Height measurement delay (msec)
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
2015-10-18 19:04:51 -03:00
// @Range: 0 250
2015-09-23 20:38:54 -03:00
// @Increment: 10
// @User: Advanced
2016-12-30 01:16:44 -04:00
// @Units: milliseconds
2015-09-23 20:38:54 -03:00
AP_GROUPINFO ( " HGT_DELAY " , 12 , NavEKF2 , _hgtDelay_ms , 60 ) ,
// Magnetometer measurement parameters
2016-05-24 23:06:04 -03:00
// @Param: MAG_M_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Magnetometer measurement noise (Gauss)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.01 0.5
// @Increment: 0.01
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: gauss
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " MAG_M_NSE " , 13 , NavEKF2 , _magNoise , MAG_M_NSE_DEFAULT ) ,
2015-09-23 20:38:54 -03:00
// @Param: MAG_CAL
2017-02-14 23:15:01 -04:00
// @DisplayName: Magnetometer default fusion mode
// @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK2_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK2_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK2_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK2_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK2_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter.
2015-10-16 09:18:23 -03:00
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
2015-09-23 20:38:54 -03:00
// @User: Advanced
AP_GROUPINFO ( " MAG_CAL " , 14 , NavEKF2 , _magCal , MAG_CAL_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: MAG_I_GATE
2015-09-21 02:18:49 -03:00
// @DisplayName: Magnetometer measurement gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " MAG_I_GATE " , 15 , NavEKF2 , _magInnovGate , MAG_I_GATE_DEFAULT ) ,
2015-09-23 20:38:54 -03:00
// Airspeed measurement parameters
2016-05-24 23:06:04 -03:00
// @Param: EAS_M_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Equivalent airspeed measurement noise (m/s)
2015-11-16 19:09:19 -04:00
// @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
2015-09-23 20:38:54 -03:00
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " EAS_M_NSE " , 16 , NavEKF2 , _easNoise , 1.4f ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: EAS_I_GATE
2015-09-21 02:18:49 -03:00
// @DisplayName: Airspeed measurement gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " EAS_I_GATE " , 17 , NavEKF2 , _tasInnovGate , 400 ) ,
2015-09-21 02:18:49 -03:00
2015-09-23 20:38:54 -03:00
// Rangefinder measurement parameters
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: RNG_M_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Range finder measurement noise (m)
// @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
2015-09-21 02:18:49 -03:00
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " RNG_M_NSE " , 18 , NavEKF2 , _rngNoise , 0.5f ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: RNG_I_GATE
2015-09-23 20:38:54 -03:00
// @DisplayName: Range finder measurement gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
2015-12-27 01:08:32 -04:00
// @Range: 100 1000
2015-11-16 19:08:46 -04:00
// @Increment: 25
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " RNG_I_GATE " , 19 , NavEKF2 , _rngInnovGate , 500 ) ,
2015-09-23 20:38:54 -03:00
2015-11-11 00:16:36 -04:00
// Optical flow measurement parameters
2015-09-23 20:38:54 -03:00
// @Param: MAX_FLOW
// @DisplayName: Maximum valid optical flow rate
2015-11-11 00:16:36 -04:00
// @Description: This sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
2015-12-27 01:08:32 -04:00
// @Range: 1.0 4.0
2015-09-23 20:38:54 -03:00
// @Increment: 0.1
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: rad/s
2015-09-23 20:38:54 -03:00
AP_GROUPINFO ( " MAX_FLOW " , 20 , NavEKF2 , _maxFlowRate , 2.5f ) ,
2016-05-24 23:06:04 -03:00
// @Param: FLOW_M_NSE
2015-09-21 02:18:49 -03:00
// @DisplayName: Optical flow measurement noise (rad/s)
// @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
2015-12-27 01:08:32 -04:00
// @Range: 0.05 1.0
2015-09-21 02:18:49 -03:00
// @Increment: 0.05
// @User: Advanced
// @Units: rad/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " FLOW_M_NSE " , 21 , NavEKF2 , _flowNoise , FLOW_M_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: FLOW_I_GATE
2015-09-21 02:18:49 -03:00
// @DisplayName: Optical Flow measurement gate size
2015-11-16 19:08:46 -04:00
// @Description: This sets the percentage number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
2015-12-27 01:08:32 -04:00
// @Range: 100 1000
2015-11-16 19:08:46 -04:00
// @Increment: 25
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " FLOW_I_GATE " , 22 , NavEKF2 , _flowInnovGate , FLOW_I_GATE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
// @Param: FLOW_DELAY
// @DisplayName: Optical Flow measurement delay (msec)
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
2015-12-27 01:08:32 -04:00
// @Range: 0 250
2015-09-21 02:18:49 -03:00
// @Increment: 10
// @User: Advanced
2016-12-30 01:16:44 -04:00
// @Units: milliseconds
2015-09-23 20:38:54 -03:00
AP_GROUPINFO ( " FLOW_DELAY " , 23 , NavEKF2 , _flowDelay_ms , FLOW_MEAS_DELAY ) ,
2015-09-21 02:18:49 -03:00
2015-09-23 20:38:54 -03:00
// State and Covariance Predition Parameters
2016-05-24 23:06:04 -03:00
// @Param: GYRO_P_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Rate gyro noise (rad/s)
2015-11-11 00:16:36 -04:00
// @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
2016-05-08 10:46:28 -03:00
// @Range: 0.0001 0.1
2015-11-14 21:40:29 -04:00
// @Increment: 0.0001
2015-09-21 02:18:49 -03:00
// @User: Advanced
2015-09-23 20:38:54 -03:00
// @Units: rad/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " GYRO_P_NSE " , 24 , NavEKF2 , _gyrNoise , GYRO_P_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: ACC_P_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Accelerometer noise (m/s^2)
2015-11-11 00:16:36 -04:00
// @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
2015-11-14 21:40:29 -04:00
// @Range: 0.01 1.0
2015-09-23 20:38:54 -03:00
// @Increment: 0.01
2015-09-21 02:18:49 -03:00
// @User: Advanced
2015-09-23 20:38:54 -03:00
// @Units: m/s/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " ACC_P_NSE " , 25 , NavEKF2 , _accNoise , ACC_P_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: GBIAS_P_NSE
2016-05-08 10:46:06 -03:00
// @DisplayName: Rate gyro bias stability (rad/s/s)
2015-11-11 00:16:36 -04:00
// @Description: This state process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier.
2016-05-08 10:46:06 -03:00
// @Range: 0.00001 0.001
2015-09-21 02:18:49 -03:00
// @User: Advanced
2016-05-08 10:46:06 -03:00
// @Units: rad/s/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " GBIAS_P_NSE " , 26 , NavEKF2 , _gyroBiasProcessNoise , GBIAS_P_NSE_DEFAULT ) ,
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: GSCL_P_NSE
2016-05-08 10:46:28 -03:00
// @DisplayName: Rate gyro scale factor stability (1/s)
2015-09-23 20:38:54 -03:00
// @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
2016-05-08 10:46:28 -03:00
// @Range: 0.000001 0.001
2015-09-23 20:38:54 -03:00
// @User: Advanced
// @Units: 1/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " GSCL_P_NSE " , 27 , NavEKF2 , _gyroScaleProcessNoise , GSCALE_P_NSE_DEFAULT ) ,
2015-09-23 20:38:54 -03:00
2016-05-24 23:06:04 -03:00
// @Param: ABIAS_P_NSE
2016-05-08 10:46:06 -03:00
// @DisplayName: Accelerometer bias stability (m/s^3)
2015-11-11 00:16:36 -04:00
// @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
2015-09-23 20:38:54 -03:00
// @Range: 0.00001 0.001
// @User: Advanced
2016-05-08 10:46:06 -03:00
// @Units: m/s/s/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " ABIAS_P_NSE " , 28 , NavEKF2 , _accelBiasProcessNoise , ABIAS_P_NSE_DEFAULT ) ,
2015-09-23 20:38:54 -03:00
2016-06-13 05:58:28 -03:00
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE
2015-09-21 02:18:49 -03:00
2016-05-24 23:06:04 -03:00
// @Param: WIND_P_NSE
2015-09-23 20:38:54 -03:00
// @DisplayName: Wind velocity process noise (m/s^2)
2015-11-11 00:16:36 -04:00
// @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
2015-09-23 20:38:54 -03:00
// @Range: 0.01 1.0
// @Increment: 0.1
// @User: Advanced
2015-11-11 00:16:36 -04:00
// @Units: m/s/s
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " WIND_P_NSE " , 30 , NavEKF2 , _windVelProcessNoise , 0.1f ) ,
2015-09-23 20:38:54 -03:00
// @Param: WIND_PSCALE
2017-01-10 11:33:29 -04:00
// @DisplayName: Height rate to wind process noise scaler
2015-11-11 00:16:36 -04:00
// @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser.
2015-09-23 20:38:54 -03:00
// @Range: 0.0 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " WIND_PSCALE " , 31 , NavEKF2 , _wndVarHgtRateScale , 0.5f ) ,
2015-09-21 02:18:49 -03:00
2015-10-09 14:59:47 -03:00
// @Param: GPS_CHECK
// @DisplayName: GPS preflight check
2015-11-11 00:16:36 -04:00
// @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat.
2017-02-17 18:09:16 -04:00
// @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
2015-10-09 14:59:47 -03:00
// @User: Advanced
AP_GROUPINFO ( " GPS_CHECK " , 32 , NavEKF2 , _gpsCheck , 31 ) ,
2015-11-04 21:00:57 -04:00
// @Param: IMU_MASK
// @DisplayName: Bitmask of active IMUs
2015-11-05 21:06:25 -04:00
// @Description: 1 byte bitmap of IMUs to use in EKF2. A separate instance of EKF2 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF2 will fail to start.
2017-01-10 11:33:29 -04:00
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
2015-11-04 21:00:57 -04:00
// @User: Advanced
2016-01-05 01:40:43 -04:00
AP_GROUPINFO ( " IMU_MASK " , 33 , NavEKF2 , _imuMask , 3 ) ,
2015-11-04 21:00:57 -04:00
2015-11-12 05:39:15 -04:00
// @Param: CHECK_SCALE
// @DisplayName: GPS accuracy check scaler (%)
// @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error.
// @Range: 50 200
// @User: Advanced
// @Units: %
AP_GROUPINFO ( " CHECK_SCALE " , 34 , NavEKF2 , _gpsCheckScaler , CHECK_SCALER_DEFAULT ) ,
2016-05-24 23:06:04 -03:00
// @Param: NOAID_M_NSE
2016-01-19 23:07:10 -04:00
// @DisplayName: Non-GPS operation position uncertainty (m)
// @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
// @Range: 0.5 50.0
2016-01-09 17:20:49 -04:00
// @User: Advanced
2017-01-10 11:33:29 -04:00
// @Units: m
2016-05-24 23:06:04 -03:00
AP_GROUPINFO ( " NOAID_M_NSE " , 35 , NavEKF2 , _noaidHorizNoise , 10.0f ) ,
2016-01-09 17:20:49 -04:00
2016-05-08 23:26:57 -03:00
// @Param: LOG_MASK
// @DisplayName: EKF sensor logging IMU mask
// @Description: This sets the IMU mask of sensors to do full logging for
2017-01-10 11:33:29 -04:00
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
2016-05-03 03:33:42 -03:00
// @User: Advanced
2016-05-08 23:26:57 -03:00
AP_GROUPINFO ( " LOG_MASK " , 36 , NavEKF2 , _logging_mask , 1 ) ,
2016-06-13 07:51:45 -03:00
// control of magentic yaw angle fusion
// @Param: YAW_M_NSE
// @DisplayName: Yaw measurement noise (rad)
// @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 1.0
// @Increment: 0.05
// @User: Advanced
2017-01-09 00:21:58 -04:00
// @Units: rad
2016-06-13 07:51:45 -03:00
AP_GROUPINFO ( " YAW_M_NSE " , 37 , NavEKF2 , _yawNoise , 0.5f ) ,
// @Param: YAW_I_GATE
// @DisplayName: Yaw measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO ( " YAW_I_GATE " , 38 , NavEKF2 , _yawInnovGate , 300 ) ,
2016-06-16 00:32:43 -03:00
// @Param: TAU_OUTPUT
// @DisplayName: Output complementary filter time constant (centi-sec)
2016-07-01 00:39:38 -03:00
// @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
2016-07-04 03:20:11 -03:00
// @Range: 10 50
2016-07-01 00:39:38 -03:00
// @Increment: 5
2016-06-16 00:32:43 -03:00
// @User: Advanced
2017-01-09 00:21:58 -04:00
// @Units: cs
2016-07-04 03:20:11 -03:00
AP_GROUPINFO ( " TAU_OUTPUT " , 39 , NavEKF2 , _tauVelPosOutput , 25 ) ,
2016-06-16 00:32:43 -03:00
2016-06-13 05:58:28 -03:00
// @Param: MAGE_P_NSE
// @DisplayName: Earth magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO ( " MAGE_P_NSE " , 40 , NavEKF2 , _magEarthProcessNoise , MAGE_P_NSE_DEFAULT ) ,
// @Param: MAGB_P_NSE
// @DisplayName: Body magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO ( " MAGB_P_NSE " , 41 , NavEKF2 , _magBodyProcessNoise , MAGB_P_NSE_DEFAULT ) ,
2016-07-12 05:56:58 -03:00
// @Param: RNG_USE_HGT
// @DisplayName: Range finder switch height percentage
// @Description: The range finder will be used as the primary height source when below a specified percentage of the sensor maximum as set by the RNGFND_MAX_CM parameter. Set to -1 to prevent range finder use.
// @Range: -1 70
// @Increment: 1
// @User: Advanced
// @Units: %
AP_GROUPINFO ( " RNG_USE_HGT " , 42 , NavEKF2 , _useRngSwHgt , - 1 ) ,
2016-09-09 19:52:37 -03:00
// @Param: TERR_GRAD
// @DisplayName: Maximum terrain gradient
2017-01-06 12:15:35 -04:00
// @Description: Specifies the maximum gradient of the terrain below the vehicle when it is using range finder as a height reference
2016-09-09 19:52:37 -03:00
// @Range: 0 0.2
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO ( " TERR_GRAD " , 43 , NavEKF2 , _terrGradMax , 0.1f ) ,
2016-10-25 17:54:29 -03:00
// @Param: BCN_M_NSE
// @DisplayName: Range beacon measurement noise (m)
// @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO ( " BCN_M_NSE " , 44 , NavEKF2 , _rngBcnNoise , 1.0f ) ,
// @Param: BCN_I_GTE
// @DisplayName: Range beacon measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the range beacon measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO ( " BCN_I_GTE " , 45 , NavEKF2 , _rngBcnInnovGate , 500 ) ,
// @Param: BCN_DELAY
// @DisplayName: Range beacon measurement delay (msec)
// @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
2016-12-30 01:16:44 -04:00
// @Units: milliseconds
2016-10-25 17:54:29 -03:00
AP_GROUPINFO ( " BCN_DELAY " , 46 , NavEKF2 , _rngBcnDelay_ms , 50 ) ,
2016-10-05 08:09:36 -03:00
// @Param: RNG_USE_SPD
// @DisplayName: Range finder max ground speed
// @Description: The range finder will not be used as the primary height source when the horizontal ground speed is greater than this value.
// @Range: 2.0 6.0
// @Increment: 0.5
// @User: Advanced
2017-01-10 11:33:29 -04:00
// @Units: m/s
2016-10-05 08:09:36 -03:00
AP_GROUPINFO ( " RNG_USE_SPD " , 47 , NavEKF2 , _useRngSwSpd , 2.0f ) ,
2017-02-14 23:15:01 -04:00
// @Param: MAG_MASK
// @DisplayName: Bitmask of active EKF cores that will always use heading fusion
// @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced
AP_GROUPINFO ( " MAG_MASK " , 48 , NavEKF2 , _magMask , 0 ) ,
2015-09-21 02:18:49 -03:00
AP_GROUPEND
} ;
2015-09-22 21:27:56 -03:00
NavEKF2 : : NavEKF2 ( const AP_AHRS * ahrs , AP_Baro & baro , const RangeFinder & rng ) :
_ahrs ( ahrs ) ,
_baro ( baro ) ,
_rng ( rng ) ,
2015-09-21 02:18:49 -03:00
gpsNEVelVarAccScale ( 0.05f ) , // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
gpsDVelVarAccScale ( 0.07f ) , // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
gpsPosVarAccScale ( 0.05f ) , // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
2015-09-23 20:38:54 -03:00
magDelay_ms ( 60 ) , // Magnetometer measurement delay (msec)
tasDelay_ms ( 240 ) , // Airspeed measurement delay (msec)
2016-10-25 17:54:29 -03:00
tiltDriftTimeMax_ms ( 15000 ) , // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)
posRetryTimeUseVel_ms ( 10000 ) , // Position aiding retry time with velocity measurements (msec)
posRetryTimeNoVel_ms ( 7000 ) , // Position aiding retry time without velocity measurements (msec)
2015-09-23 20:38:54 -03:00
hgtRetryTimeMode0_ms ( 10000 ) , // Height retry time with vertical velocity measurement (msec)
hgtRetryTimeMode12_ms ( 5000 ) , // Height retry time without vertical velocity measurement (msec)
tasRetryTime_ms ( 5000 ) , // True airspeed timeout and retry interval (msec)
2015-09-21 02:18:49 -03:00
magFailTimeLimit_ms ( 10000 ) , // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
2015-10-24 06:10:20 -03:00
magVarRateScale ( 0.005f ) , // scale factor applied to magnetometer variance due to angular rate and measurement timing jitter. Assume timing jitter of 10msec
2015-09-21 02:18:49 -03:00
gyroBiasNoiseScaler ( 2.0f ) , // scale factor applied to imu gyro bias learning before the vehicle is armed
2015-09-23 20:38:54 -03:00
hgtAvg_ms ( 100 ) , // average number of msec between height measurements
betaAvg_ms ( 100 ) , // average number of msec between synthetic sideslip measurements
2015-10-15 09:01:04 -03:00
covTimeStepMax ( 0.1f ) , // maximum time (sec) between covariance prediction updates
2015-09-21 02:18:49 -03:00
covDelAngMax ( 0.05f ) , // maximum delta angle between covariance prediction updates
DCM33FlowMin ( 0.71f ) , // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
fScaleFactorPnoise ( 1e-10 f ) , // Process noise added to focal length scale factor state variance at each time step
flowTimeDeltaAvg_ms ( 100 ) , // average interval between optical flow measurements (msec)
flowIntervalMax_ms ( 100 ) , // maximum allowable time between flow fusion events
gndEffectTimeout_ms ( 1000 ) , // time in msec that baro ground effect compensation will timeout after initiation
2015-09-23 20:38:54 -03:00
gndEffectBaroScaler ( 4.0f ) , // scaler applied to the barometer observation variance when operating in ground effect
2015-11-12 04:11:56 -04:00
gndGradientSigma ( 50 ) , // RMS terrain gradient percentage assumed by the terrain height estimation
2016-12-12 17:58:53 -04:00
fusionTimeStep_ms ( 10 ) , // The minimum number of msec between covariance prediction and fusion operations
runCoreSelection ( false ) // true when the default primary core has stabilised after startup and core selection can run
2015-09-21 02:18:49 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
2016-05-03 19:23:51 -03:00
/*
see if we should log some sensor data
*/
void NavEKF2 : : check_log_write ( void )
{
2016-05-08 23:26:57 -03:00
if ( ! have_ekf_logging ( ) ) {
2016-05-03 19:23:51 -03:00
return ;
}
if ( logging . log_compass ) {
2016-05-05 00:37:37 -03:00
DataFlash_Class : : instance ( ) - > Log_Write_Compass ( * _ahrs - > get_compass ( ) , imuSampleTime_us ) ;
2016-05-03 19:23:51 -03:00
logging . log_compass = false ;
}
if ( logging . log_gps ) {
2016-05-05 00:37:37 -03:00
DataFlash_Class : : instance ( ) - > Log_Write_GPS ( _ahrs - > get_gps ( ) , 0 , imuSampleTime_us ) ;
2016-05-03 19:23:51 -03:00
logging . log_gps = false ;
}
if ( logging . log_baro ) {
2016-05-05 00:37:37 -03:00
DataFlash_Class : : instance ( ) - > Log_Write_Baro ( _baro , imuSampleTime_us ) ;
2016-05-03 19:23:51 -03:00
logging . log_baro = false ;
}
if ( logging . log_imu ) {
const AP_InertialSensor & ins = _ahrs - > get_ins ( ) ;
2016-05-08 23:26:57 -03:00
DataFlash_Class : : instance ( ) - > Log_Write_IMUDT ( ins , imuSampleTime_us , _logging_mask . get ( ) ) ;
2016-05-03 19:23:51 -03:00
logging . log_imu = false ;
}
2016-05-04 06:09:33 -03:00
// this is an example of an ad-hoc log in EKF
2016-05-18 00:30:51 -03:00
// DataFlash_Class::instance()->Log_Write("NKA", "TimeUS,X", "Qf", AP_HAL::micros64(), (double)2.4f);
2016-05-03 19:23:51 -03:00
}
2015-09-22 21:27:56 -03:00
// Initialise the filter
bool NavEKF2 : : InitialiseFilter ( void )
{
if ( _enable = = 0 ) {
return false ;
}
2016-05-05 00:37:37 -03:00
imuSampleTime_us = AP_HAL : : micros64 ( ) ;
2016-05-08 23:26:57 -03:00
// see if we will be doing logging
DataFlash_Class * dataflash = DataFlash_Class : : instance ( ) ;
if ( dataflash ! = nullptr ) {
logging . enabled = dataflash - > log_replay ( ) ;
}
2016-05-05 00:37:37 -03:00
2015-09-22 21:27:56 -03:00
if ( core = = nullptr ) {
2015-11-04 21:00:57 -04:00
2016-01-05 01:40:43 -04:00
// don't run multiple filters for 1 IMU
const AP_InertialSensor & ins = _ahrs - > get_ins ( ) ;
uint8_t mask = ( 1U < < ins . get_accel_count ( ) ) - 1 ;
_imuMask . set ( _imuMask . get ( ) & mask ) ;
2015-11-04 21:00:57 -04:00
// count IMUs from mask
num_cores = 0 ;
for ( uint8_t i = 0 ; i < 7 ; i + + ) {
if ( _imuMask & ( 1U < < i ) ) {
num_cores + + ;
}
}
2015-11-04 21:22:49 -04:00
if ( hal . util - > available_memory ( ) < sizeof ( NavEKF2_core ) * num_cores + 4096 ) {
GCS_MAVLINK : : send_statustext_all ( MAV_SEVERITY_CRITICAL , " NavEKF2: not enough memory " ) ;
_enable . set ( 0 ) ;
return false ;
}
2015-11-04 21:00:57 -04:00
core = new NavEKF2_core [ num_cores ] ;
2015-09-22 21:27:56 -03:00
if ( core = = nullptr ) {
_enable . set ( 0 ) ;
2015-10-24 18:45:41 -03:00
GCS_MAVLINK : : send_statustext_all ( MAV_SEVERITY_CRITICAL , " NavEKF2: allocation failed " ) ;
2015-09-22 21:27:56 -03:00
return false ;
}
2015-11-04 21:00:57 -04:00
// set the IMU index for the cores
num_cores = 0 ;
for ( uint8_t i = 0 ; i < 7 ; i + + ) {
if ( _imuMask & ( 1U < < i ) ) {
2015-11-13 18:28:45 -04:00
if ( ! core [ num_cores ] . setup_core ( this , i , num_cores ) ) {
return false ;
}
2015-11-05 21:04:20 -04:00
num_cores + + ;
2015-11-04 21:00:57 -04:00
}
}
2015-11-05 22:11:49 -04:00
// Set the primary initially to be the lowest index
primary = 0 ;
2015-11-04 21:00:57 -04:00
}
// initialse the cores. We return success only if all cores
// initialise successfully
bool ret = true ;
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
ret & = core [ i ] . InitialiseFilterBootstrap ( ) ;
2015-09-22 21:27:56 -03:00
}
2016-05-03 19:23:51 -03:00
2016-11-22 06:29:30 -04:00
// zero the structs used capture reset events
2016-11-22 06:43:32 -04:00
memset ( & yaw_reset_data , 0 , sizeof ( yaw_reset_data ) ) ;
memset ( & pos_reset_data , 0 , sizeof ( pos_reset_data ) ) ;
2016-11-22 06:29:30 -04:00
memset ( & pos_down_reset_data , 0 , sizeof ( pos_down_reset_data ) ) ;
2016-05-03 19:23:51 -03:00
check_log_write ( ) ;
2015-11-04 21:00:57 -04:00
return ret ;
2015-09-22 21:27:56 -03:00
}
// Update Filter States - this should be called whenever new IMU data is available
void NavEKF2 : : UpdateFilter ( void )
{
2015-11-04 21:13:11 -04:00
if ( ! core ) {
return ;
}
2015-11-09 20:25:44 -04:00
2016-05-05 00:37:37 -03:00
imuSampleTime_us = AP_HAL : : micros64 ( ) ;
2015-11-09 20:25:44 -04:00
const AP_InertialSensor & ins = _ahrs - > get_ins ( ) ;
2016-11-21 18:21:01 -04:00
bool statePredictEnabled [ num_cores ] ;
2015-11-04 21:13:11 -04:00
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
2015-11-09 20:25:44 -04:00
// if the previous core has only recently finished a new state prediction cycle, then
2016-05-12 14:04:56 -03:00
// don't start a new cycle to allow time for fusion operations to complete if the update
2015-11-09 20:25:44 -04:00
// rate is higher than 200Hz
if ( ( i > 0 ) & & ( core [ i - 1 ] . getFramesSincePredict ( ) < 2 ) & & ( ins . get_sample_rate ( ) > 200 ) ) {
2016-11-21 18:21:01 -04:00
statePredictEnabled [ i ] = false ;
2015-11-09 20:25:44 -04:00
} else {
2016-11-21 18:21:01 -04:00
statePredictEnabled [ i ] = true ;
2015-11-09 20:25:44 -04:00
}
2016-11-21 18:21:01 -04:00
core [ i ] . UpdateFilter ( statePredictEnabled [ i ] ) ;
2015-11-04 21:13:11 -04:00
}
2016-11-21 18:21:01 -04:00
// If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
2016-12-12 17:58:53 -04:00
// Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
// due to initial alignment fluctuations and race conditions
if ( ! runCoreSelection ) {
static uint64_t lastUnhealthyTime_us = 0 ;
if ( ! core [ primary ] . healthy ( ) | | lastUnhealthyTime_us = = 0 ) {
lastUnhealthyTime_us = imuSampleTime_us ;
}
runCoreSelection = ( imuSampleTime_us - lastUnhealthyTime_us ) > 1E7 ;
}
2016-11-21 18:21:01 -04:00
float primaryErrorScore = core [ primary ] . errorScore ( ) ;
2016-12-12 17:58:53 -04:00
if ( ( primaryErrorScore > 1.0f | | ! core [ primary ] . healthy ( ) ) & & runCoreSelection ) {
2016-11-23 18:30:30 -04:00
float lowestErrorScore = 0.67f * primaryErrorScore ;
2016-11-21 18:21:01 -04:00
uint8_t newPrimaryIndex = primary ; // index for new primary
for ( uint8_t coreIndex = 0 ; coreIndex < num_cores ; coreIndex + + ) {
if ( coreIndex ! = primary ) {
// an alternative core is available for selection only if healthy and if states have been updated on this time step
bool altCoreAvailable = core [ coreIndex ] . healthy ( ) & & statePredictEnabled [ coreIndex ] ;
// If the primary core is unhealthy and another core is available, then switch now
// If the primary core is still healthy,then switching is optional and will only be done if
// a core with a significantly lower error score can be found
float altErrorScore = core [ coreIndex ] . errorScore ( ) ;
2016-11-23 18:30:30 -04:00
if ( altCoreAvailable & & ( ! core [ primary ] . healthy ( ) | | altErrorScore < lowestErrorScore ) ) {
2016-11-21 18:21:01 -04:00
newPrimaryIndex = coreIndex ;
lowestErrorScore = altErrorScore ;
2015-11-05 22:11:49 -04:00
}
}
2015-11-04 21:00:57 -04:00
}
2016-11-21 18:21:01 -04:00
// update the yaw and position reset data to capture changes due to the lane switch
if ( newPrimaryIndex ! = primary ) {
2016-11-22 06:43:32 -04:00
updateLaneSwitchYawResetData ( newPrimaryIndex , primary ) ;
updateLaneSwitchPosResetData ( newPrimaryIndex , primary ) ;
2016-11-22 06:29:30 -04:00
updateLaneSwitchPosDownResetData ( newPrimaryIndex , primary ) ;
2016-11-21 18:21:01 -04:00
primary = newPrimaryIndex ;
}
2015-09-22 21:27:56 -03:00
}
2016-05-03 19:23:51 -03:00
check_log_write ( ) ;
2015-09-22 21:27:56 -03:00
}
// Check basic filter health metrics and return a consolidated health status
bool NavEKF2 : : healthy ( void ) const
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . healthy ( ) ;
2015-09-22 21:27:56 -03:00
}
2015-11-05 21:13:43 -04:00
// returns the index of the primary core
2015-11-05 23:04:22 -04:00
// return -1 if no primary core selected
int8_t NavEKF2 : : getPrimaryCoreIndex ( void ) const
2015-11-05 21:13:43 -04:00
{
if ( ! core ) {
2015-11-05 23:04:22 -04:00
return - 1 ;
2015-11-05 21:13:43 -04:00
}
2015-11-05 23:04:22 -04:00
return primary ;
2015-11-05 21:13:43 -04:00
}
2016-09-03 03:50:26 -03:00
// returns the index of the IMU of the primary core
// return -1 if no primary core selected
int8_t NavEKF2 : : getPrimaryCoreIMUIndex ( void ) const
{
if ( ! core ) {
return - 1 ;
}
return core [ primary ] . getIMUIndex ( ) ;
}
2016-07-11 20:54:50 -03:00
// Write the last calculated NE position relative to the reference point (m).
2016-07-10 05:43:28 -03:00
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool NavEKF2 : : getPosNE ( int8_t instance , Vector2f & posNE )
{
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
if ( ! core ) {
return false ;
}
return core [ instance ] . getPosNE ( posNE ) ;
}
2016-07-11 20:54:50 -03:00
// Write the last calculated D position relative to the reference point (m).
2016-07-10 05:43:28 -03:00
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool NavEKF2 : : getPosD ( int8_t instance , float & posD )
{
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
if ( ! core ) {
return false ;
}
return core [ instance ] . getPosD ( posD ) ;
}
2015-09-22 21:27:56 -03:00
// return NED velocity in m/s
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getVelNED ( int8_t instance , Vector3f & vel )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getVelNED ( vel ) ;
2015-09-22 21:27:56 -03:00
}
}
2017-01-05 14:07:14 -04:00
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
2015-11-07 08:00:29 -04:00
float NavEKF2 : : getPosDownDerivative ( int8_t instance )
2015-10-12 03:29:13 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
// return the value calculated from a complementary filer applied to the EKF height and vertical acceleration
2015-10-12 03:29:13 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
return core [ instance ] . getPosDownDerivative ( ) ;
2015-10-12 03:29:13 -03:00
}
2015-10-15 02:27:40 -03:00
return 0.0f ;
2015-10-12 03:29:13 -03:00
}
2015-09-22 21:27:56 -03:00
// This returns the specific forces in the NED frame
void NavEKF2 : : getAccelNED ( Vector3f & accelNED ) const
{
if ( core ) {
2015-11-04 21:00:57 -04:00
core [ primary ] . getAccelNED ( accelNED ) ;
2015-09-22 21:27:56 -03:00
}
}
// return body axis gyro bias estimates in rad/sec
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getGyroBias ( int8_t instance , Vector3f & gyroBias )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getGyroBias ( gyroBias ) ;
2015-09-22 21:27:56 -03:00
}
}
2015-09-23 23:05:42 -03:00
// return body axis gyro scale factor error as a percentage
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getGyroScaleErrorPercentage ( int8_t instance , Vector3f & gyroScale )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getGyroScaleErrorPercentage ( gyroScale ) ;
2015-09-22 21:27:56 -03:00
}
}
2015-11-07 08:00:29 -04:00
// return tilt error convergence metric for the specified instance
void NavEKF2 : : getTiltError ( int8_t instance , float & ang )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getTiltError ( ang ) ;
2015-09-22 21:27:56 -03:00
}
}
// reset body axis gyro bias estimates
void NavEKF2 : : resetGyroBias ( void )
{
if ( core ) {
2016-06-08 20:06:39 -03:00
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
core [ i ] . resetGyroBias ( ) ;
}
2015-09-22 21:27:56 -03:00
}
}
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool NavEKF2 : : resetHeightDatum ( void )
{
2016-06-08 20:06:39 -03:00
bool status = true ;
if ( core ) {
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
if ( ! core [ i ] . resetHeightDatum ( ) ) {
status = false ;
}
}
} else {
status = false ;
2015-09-22 21:27:56 -03:00
}
2016-06-08 20:06:39 -03:00
return status ;
2015-09-22 21:27:56 -03:00
}
// Commands the EKF to not use GPS.
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2 : : setInhibitGPS ( void )
{
if ( ! core ) {
return 0 ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . setInhibitGPS ( ) ;
2015-09-22 21:27:56 -03:00
}
// return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void NavEKF2 : : getEkfControlLimits ( float & ekfGndSpdLimit , float & ekfNavVelGainScaler ) const
{
if ( core ) {
2015-11-04 21:00:57 -04:00
core [ primary ] . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2015-09-22 21:27:56 -03:00
}
}
// return the individual Z-accel bias estimates in m/s^2
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getAccelZBias ( int8_t instance , float & zbias )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getAccelZBias ( zbias ) ;
2015-09-22 21:27:56 -03:00
}
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getWind ( int8_t instance , Vector3f & wind )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getWind ( wind ) ;
2015-09-22 21:27:56 -03:00
}
}
// return earth magnetic field estimates in measurement units / 1000
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getMagNED ( int8_t instance , Vector3f & magNED )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getMagNED ( magNED ) ;
2015-09-22 21:27:56 -03:00
}
}
// return body magnetic field estimates in measurement units / 1000
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getMagXYZ ( int8_t instance , Vector3f & magXYZ )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getMagXYZ ( magXYZ ) ;
2015-09-22 21:27:56 -03:00
}
}
2015-11-08 05:42:09 -04:00
// return the magnetometer in use for the specified instance
uint8_t NavEKF2 : : getActiveMag ( int8_t instance )
{
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
if ( core ) {
return core [ instance ] . getActiveMag ( ) ;
2015-11-09 07:29:47 -04:00
} else {
return 255 ;
2015-11-08 05:42:09 -04:00
}
}
2015-09-22 21:27:56 -03:00
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
2016-03-29 17:06:42 -03:00
bool NavEKF2 : : getMagOffsets ( uint8_t mag_idx , Vector3f & magOffsets ) const
2015-09-22 21:27:56 -03:00
{
if ( ! core ) {
return false ;
}
2016-03-29 17:06:42 -03:00
// try the primary first, else loop through all of the cores and return when one has offsets for this mag instance
if ( core [ primary ] . getMagOffsets ( mag_idx , magOffsets ) ) {
return true ;
}
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
if ( core [ i ] . getMagOffsets ( mag_idx , magOffsets ) ) {
return true ;
}
}
return false ;
2015-09-22 21:27:56 -03:00
}
// Return the last calculated latitude, longitude and height in WGS-84
// If a calculated location isn't available, return a raw GPS measurement
// The status will return true if a calculation or raw measurement is available
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
bool NavEKF2 : : getLLH ( struct Location & loc ) const
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . getLLH ( loc ) ;
2015-09-22 21:27:56 -03:00
}
// return the latitude and longitude and height used to set the NED origin
// All NED positions calculated by the filter are relative to this location
// Returns false if the origin has not been set
bool NavEKF2 : : getOriginLLH ( struct Location & loc ) const
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . getOriginLLH ( loc ) ;
2015-09-22 21:27:56 -03:00
}
// set the latitude and longitude and height used to set the NED origin
2017-01-05 14:07:14 -04:00
// All NED positions calculated by the filter will be relative to this location
2015-09-22 21:27:56 -03:00
// The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
// Returns false if the filter has rejected the attempt to set the origin
bool NavEKF2 : : setOriginLLH ( struct Location & loc )
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . setOriginLLH ( loc ) ;
2015-09-22 21:27:56 -03:00
}
// return estimated height above ground level
// return false if ground height is not being estimated.
bool NavEKF2 : : getHAGL ( float & HAGL ) const
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . getHAGL ( HAGL ) ;
2015-09-22 21:27:56 -03:00
}
2015-11-07 08:00:29 -04:00
// return the Euler roll, pitch and yaw angle in radians for the specified instance
void NavEKF2 : : getEulerAngles ( int8_t instance , Vector3f & eulers )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getEulerAngles ( eulers ) ;
2015-09-22 21:27:56 -03:00
}
}
// return the transformation matrix from XYZ (body) to NED axes
void NavEKF2 : : getRotationBodyToNED ( Matrix3f & mat ) const
{
if ( core ) {
2015-11-04 21:00:57 -04:00
core [ primary ] . getRotationBodyToNED ( mat ) ;
2015-09-22 21:27:56 -03:00
}
}
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF2 : : getQuaternion ( Quaternion & quat ) const
{
if ( core ) {
2015-11-04 21:00:57 -04:00
core [ primary ] . getQuaternion ( quat ) ;
2015-09-22 21:27:56 -03:00
}
}
2015-11-07 08:00:29 -04:00
// return the innovations for the specified instance
void NavEKF2 : : getInnovations ( int8_t instance , Vector3f & velInnov , Vector3f & posInnov , Vector3f & magInnov , float & tasInnov , float & yawInnov )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getInnovations ( velInnov , posInnov , magInnov , tasInnov , yawInnov ) ;
2015-09-22 21:27:56 -03:00
}
}
2016-06-29 02:57:10 -03:00
// publish output observer angular, velocity and position tracking error
void NavEKF2 : : getOutputTrackingError ( int8_t instance , Vector3f & error ) const
{
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
if ( core ) {
core [ instance ] . getOutputTrackingError ( error ) ;
}
}
2015-09-22 21:27:56 -03:00
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getVariances ( int8_t instance , float & velVar , float & posVar , float & hgtVar , Vector3f & magVar , float & tasVar , Vector2f & offset )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getVariances ( velVar , posVar , hgtVar , magVar , tasVar , offset ) ;
2015-09-22 21:27:56 -03:00
}
}
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool NavEKF2 : : use_compass ( void ) const
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . use_compass ( ) ;
2015-09-22 21:27:56 -03:00
}
// write the raw optical flow measurements
// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
// rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
// rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
2016-10-11 18:23:39 -03:00
// posOffset is the XYZ flow sensor position in the body frame in m
2016-10-27 01:47:26 -03:00
void NavEKF2 : : writeOptFlowMeas ( uint8_t & rawFlowQuality , Vector2f & rawFlowRates , Vector2f & rawGyroRates , uint32_t & msecFlowMeas , const Vector3f & posOffset )
2015-09-22 21:27:56 -03:00
{
if ( core ) {
2015-11-10 03:17:16 -04:00
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
2016-10-11 18:23:39 -03:00
core [ i ] . writeOptFlowMeas ( rawFlowQuality , rawFlowRates , rawGyroRates , msecFlowMeas , posOffset ) ;
2015-11-10 03:17:16 -04:00
}
2015-09-22 21:27:56 -03:00
}
}
// return data for debugging optical flow fusion
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getFlowDebug ( int8_t instance , float & varFlow , float & gndOffset , float & flowInnovX , float & flowInnovY , float & auxInnov ,
float & HAGL , float & rngInnov , float & range , float & gndOffsetErr )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getFlowDebug ( varFlow , gndOffset , flowInnovX , flowInnovY , auxInnov , HAGL , rngInnov , range , gndOffsetErr ) ;
2015-09-22 21:27:56 -03:00
}
}
2016-10-25 17:54:29 -03:00
// return data for debugging range beacon fusion
2016-11-30 02:54:55 -04:00
bool NavEKF2 : : getRangeBeaconDebug ( int8_t instance , uint8_t & ID , float & rng , float & innov , float & innovVar , float & testRatio , Vector3f & beaconPosNED , float & offsetHigh , float & offsetLow )
2016-10-25 17:54:29 -03:00
{
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
if ( core ) {
2016-11-30 02:54:55 -04:00
return core [ instance ] . getRangeBeaconDebug ( ID , rng , innov , innovVar , testRatio , beaconPosNED , offsetHigh , offsetLow ) ;
} else {
return false ;
2016-10-25 17:54:29 -03:00
}
}
2015-09-22 21:27:56 -03:00
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2 : : setTakeoffExpected ( bool val )
{
if ( core ) {
2016-04-03 18:28:08 -03:00
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
core [ i ] . setTakeoffExpected ( val ) ;
}
2015-09-22 21:27:56 -03:00
}
}
// called by vehicle code to specify that a touchdown is expected to happen
// causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2 : : setTouchdownExpected ( bool val )
{
if ( core ) {
2016-04-03 18:28:08 -03:00
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
core [ i ] . setTouchdownExpected ( val ) ;
}
2015-09-22 21:27:56 -03:00
}
}
2016-07-12 05:56:58 -03:00
// Set to true if the terrain underneath is stable enough to be used as a height reference
// in combination with a range finder. Set to false if the terrain underneath the vehicle
// cannot be used as a height reference
void NavEKF2 : : setTerrainHgtStable ( bool val )
{
if ( core ) {
for ( uint8_t i = 0 ; i < num_cores ; i + + ) {
core [ i ] . setTerrainHgtStable ( val ) ;
}
}
}
2015-09-22 21:27:56 -03:00
/*
return the filter fault status as a bitmasked integer
0 = quaternions are NaN
1 = velocities are NaN
2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion
5 = badly conditioned Z magnetometer fusion
6 = badly conditioned airspeed fusion
7 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised
*/
2016-05-10 03:00:36 -03:00
void NavEKF2 : : getFilterFaults ( int8_t instance , uint16_t & faults )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getFilterFaults ( faults ) ;
2015-10-25 21:34:32 -03:00
} else {
faults = 0 ;
2015-09-22 21:27:56 -03:00
}
}
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
7 = unassigned
*/
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getFilterTimeouts ( int8_t instance , uint8_t & timeouts )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getFilterTimeouts ( timeouts ) ;
2015-10-25 21:34:32 -03:00
} else {
timeouts = 0 ;
2015-09-22 21:27:56 -03:00
}
}
/*
return filter status flags
*/
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getFilterStatus ( int8_t instance , nav_filter_status & status )
2015-09-22 21:27:56 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-09-22 21:27:56 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getFilterStatus ( status ) ;
2015-10-25 21:34:32 -03:00
} else {
memset ( & status , 0 , sizeof ( status ) ) ;
2015-09-22 21:27:56 -03:00
}
}
2015-10-07 21:38:26 -03:00
/*
return filter gps quality check status
*/
2015-11-07 08:00:29 -04:00
void NavEKF2 : : getFilterGpsStatus ( int8_t instance , nav_gps_status & status )
2015-10-07 21:38:26 -03:00
{
2015-11-07 08:00:29 -04:00
if ( instance < 0 | | instance > = num_cores ) instance = primary ;
2015-10-07 21:38:26 -03:00
if ( core ) {
2015-11-07 08:00:29 -04:00
core [ instance ] . getFilterGpsStatus ( status ) ;
2015-10-25 21:34:32 -03:00
} else {
memset ( & status , 0 , sizeof ( status ) ) ;
2015-10-07 21:38:26 -03:00
}
}
2015-09-22 21:27:56 -03:00
// send an EKF_STATUS_REPORT message to GCS
void NavEKF2 : : send_status_report ( mavlink_channel_t chan )
{
if ( core ) {
2015-11-04 21:00:57 -04:00
core [ primary ] . send_status_report ( chan ) ;
2015-09-22 21:27:56 -03:00
}
}
// provides the height limit to be observed by the control loops
// returns false if no height limiting is required
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
bool NavEKF2 : : getHeightControlLimit ( float & height ) const
{
if ( ! core ) {
return false ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . getHeightControlLimit ( height ) ;
2015-09-22 21:27:56 -03:00
}
2016-11-29 20:01:57 -04:00
// Returns the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch
// Returns the time of the last yaw angle reset or 0 if no reset or core switch has ever occurred
// Where there are multiple consumers, they must access this function on the same frame as each other
2016-08-25 09:10:22 -03:00
uint32_t NavEKF2 : : getLastYawResetAngle ( float & yawAngDelta )
2015-09-22 21:27:56 -03:00
{
if ( ! core ) {
2015-10-29 05:33:02 -03:00
return 0 ;
2015-09-22 21:27:56 -03:00
}
2016-09-20 15:23:50 -03:00
2016-11-29 20:01:57 -04:00
yawAngDelta = 0.0f ;
// Do the conversion to msec in one place
uint32_t now_time_ms = imuSampleTime_us / 1000 ;
// The last time we switched to the current primary core is the first reset event
uint32_t lastYawReset_ms = yaw_reset_data . last_primary_change ;
2016-09-20 15:23:50 -03:00
2016-11-22 06:43:32 -04:00
// There has been a change notification in the primary core that the controller has not consumed
2016-11-29 20:01:57 -04:00
// or this is a repeated acce
if ( yaw_reset_data . core_changed | | yaw_reset_data . last_function_call = = now_time_ms ) {
2016-09-20 15:23:50 -03:00
yawAngDelta = yaw_reset_data . core_delta ;
yaw_reset_data . core_changed = false ;
}
2016-11-29 20:01:57 -04:00
// Record last time controller got the yaw reset
yaw_reset_data . last_function_call = now_time_ms ;
// There has been a reset inside the core since we switched so update the time and delta
2016-11-22 06:43:32 -04:00
float temp_yawAng ;
uint32_t lastCoreYawReset_ms = core [ primary ] . getLastYawResetAngle ( temp_yawAng ) ;
2016-09-20 15:23:50 -03:00
if ( lastCoreYawReset_ms > lastYawReset_ms ) {
yawAngDelta = wrap_PI ( yawAngDelta + temp_yawAng ) ;
lastYawReset_ms = lastCoreYawReset_ms ;
}
return lastYawReset_ms ;
2015-09-22 21:27:56 -03:00
}
2016-11-29 20:02:41 -04:00
// Returns the amount of NE position change due to the last position reset or core switch in metres
// Returns the time of the last reset or 0 if no reset or core switch has ever occurred
// Where there are multiple consumers, they must access this function on the same frame as each other
2016-10-09 18:18:54 -03:00
uint32_t NavEKF2 : : getLastPosNorthEastReset ( Vector2f & posDelta )
2015-10-29 02:06:24 -03:00
{
if ( ! core ) {
2015-10-29 05:36:44 -03:00
return 0 ;
2015-10-29 02:06:24 -03:00
}
2016-10-09 18:18:54 -03:00
posDelta . zero ( ) ;
2016-11-29 20:02:41 -04:00
// Do the conversion to msec in one place
uint32_t now_time_ms = imuSampleTime_us / 1000 ;
// The last time we switched to the current primary core is the first reset event
uint32_t lastPosReset_ms = pos_reset_data . last_primary_change ;
2016-10-09 18:18:54 -03:00
2016-11-22 06:43:32 -04:00
// There has been a change in the primary core that the controller has not consumed
2016-11-29 20:02:41 -04:00
// allow for multiple consumers on the same frame
if ( pos_reset_data . core_changed | | pos_reset_data . last_function_call = = now_time_ms ) {
2016-10-09 18:18:54 -03:00
posDelta = pos_reset_data . core_delta ;
pos_reset_data . core_changed = false ;
}
2016-11-29 20:02:41 -04:00
// Record last time controller got the position reset
pos_reset_data . last_function_call = now_time_ms ;
// There has been a reset inside the core since we switched so update the time and delta
2016-11-22 06:43:32 -04:00
Vector2f tempPosDelta ;
uint32_t lastCorePosReset_ms = core [ primary ] . getLastPosNorthEastReset ( tempPosDelta ) ;
2016-10-09 18:18:54 -03:00
if ( lastCorePosReset_ms > lastPosReset_ms ) {
posDelta = posDelta + tempPosDelta ;
lastPosReset_ms = lastCorePosReset_ms ;
}
return lastPosReset_ms ;
2015-10-29 02:06:24 -03:00
}
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
2015-10-29 05:36:44 -03:00
uint32_t NavEKF2 : : getLastVelNorthEastReset ( Vector2f & vel ) const
2015-10-29 02:06:24 -03:00
{
if ( ! core ) {
2015-10-29 05:36:44 -03:00
return 0 ;
2015-10-29 02:06:24 -03:00
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . getLastVelNorthEastReset ( vel ) ;
2015-10-29 02:06:24 -03:00
}
2015-10-30 00:59:14 -03:00
// report the reason for why the backend is refusing to initialise
const char * NavEKF2 : : prearm_failure_reason ( void ) const
{
if ( ! core ) {
return nullptr ;
}
2015-11-04 21:00:57 -04:00
return core [ primary ] . prearm_failure_reason ( ) ;
2015-10-30 00:59:14 -03:00
}
2016-11-29 20:03:13 -04:00
// Returns the amount of vertical position change due to the last reset or core switch in metres
// Returns the time of the last reset or 0 if no reset or core switch has ever occurred
// Where there are multiple consumers, they must access this function on the same frame as each other
2016-11-22 06:29:30 -04:00
uint32_t NavEKF2 : : getLastPosDownReset ( float & posDelta )
{
if ( ! core ) {
return 0 ;
}
posDelta = 0.0f ;
2016-11-29 20:03:13 -04:00
// Do the conversion to msec in one place
uint32_t now_time_ms = imuSampleTime_us / 1000 ;
// The last time we switched to the current primary core is the first reset event
uint32_t lastPosReset_ms = pos_down_reset_data . last_primary_change ;
2016-11-22 06:29:30 -04:00
// There has been a change in the primary core that the controller has not consumed
2016-11-29 20:03:13 -04:00
// allow for multiple consumers on the same frame
if ( pos_down_reset_data . core_changed | | pos_down_reset_data . last_function_call = = now_time_ms ) {
2016-11-22 06:29:30 -04:00
posDelta = pos_down_reset_data . core_delta ;
pos_down_reset_data . core_changed = false ;
}
2016-11-29 20:03:13 -04:00
// Record last time controller got the position reset
pos_down_reset_data . last_function_call = now_time_ms ;
// There has been a reset inside the core since we switched so update the time and delta
2016-11-22 06:29:30 -04:00
float tempPosDelta ;
uint32_t lastCorePosReset_ms = core [ primary ] . getLastPosDownReset ( tempPosDelta ) ;
if ( lastCorePosReset_ms > lastPosReset_ms ) {
posDelta + = tempPosDelta ;
lastPosReset_ms = lastCorePosReset_ms ;
}
return lastPosReset_ms ;
}
2016-10-09 18:18:54 -03:00
// update the yaw reset data to capture changes due to a lane switch
2016-11-22 06:29:30 -04:00
void NavEKF2 : : updateLaneSwitchYawResetData ( uint8_t new_primary , uint8_t old_primary )
2016-10-09 18:18:54 -03:00
{
Vector3f eulers_old_primary , eulers_new_primary ;
float old_yaw_delta ;
// If core yaw reset data has been consumed reset delta to zero
if ( ! yaw_reset_data . core_changed ) {
yaw_reset_data . core_delta = 0 ;
}
// If current primary has reset yaw after controller got it, add it to the delta
2016-11-22 06:29:30 -04:00
if ( core [ old_primary ] . getLastYawResetAngle ( old_yaw_delta ) > yaw_reset_data . last_function_call ) {
2016-10-09 18:18:54 -03:00
yaw_reset_data . core_delta + = old_yaw_delta ;
}
// Record the yaw delta between current core and new primary core and the timestamp of the core change
// Add current delta in case it hasn't been consumed yet
core [ old_primary ] . getEulerAngles ( eulers_old_primary ) ;
core [ new_primary ] . getEulerAngles ( eulers_new_primary ) ;
yaw_reset_data . core_delta = wrap_PI ( eulers_new_primary . z - eulers_old_primary . z + yaw_reset_data . core_delta ) ;
yaw_reset_data . last_primary_change = imuSampleTime_us / 1000 ;
yaw_reset_data . core_changed = true ;
}
// update the position reset data to capture changes due to a lane switch
2016-11-22 06:43:32 -04:00
void NavEKF2 : : updateLaneSwitchPosResetData ( uint8_t new_primary , uint8_t old_primary )
2016-10-09 18:18:54 -03:00
{
Vector2f pos_old_primary , pos_new_primary , old_pos_delta ;
// If core position reset data has been consumed reset delta to zero
if ( ! pos_reset_data . core_changed ) {
pos_reset_data . core_delta . zero ( ) ;
}
// If current primary has reset position after controller got it, add it to the delta
2016-11-22 06:43:32 -04:00
if ( core [ old_primary ] . getLastPosNorthEastReset ( old_pos_delta ) > pos_reset_data . last_function_call ) {
2016-10-09 18:18:54 -03:00
pos_reset_data . core_delta + = old_pos_delta ;
}
// Record the position delta between current core and new primary core and the timestamp of the core change
// Add current delta in case it hasn't been consumed yet
2016-11-22 06:43:32 -04:00
core [ old_primary ] . getPosNE ( pos_old_primary ) ;
core [ new_primary ] . getPosNE ( pos_new_primary ) ;
2016-10-09 18:18:54 -03:00
pos_reset_data . core_delta = pos_new_primary - pos_old_primary + pos_reset_data . core_delta ;
pos_reset_data . last_primary_change = imuSampleTime_us / 1000 ;
pos_reset_data . core_changed = true ;
2016-11-22 06:43:32 -04:00
2016-10-09 18:18:54 -03:00
}
2016-11-22 06:29:30 -04:00
// Update the vertical position reset data to capture changes due to a core switch
// This should be called after the decision to switch cores has been made, but before the
// new primary EKF update has been run
void NavEKF2 : : updateLaneSwitchPosDownResetData ( uint8_t new_primary , uint8_t old_primary )
{
float posDownOldPrimary , posDownNewPrimary , oldPosDownDelta ;
// If core position reset data has been consumed reset delta to zero
if ( ! pos_down_reset_data . core_changed ) {
pos_down_reset_data . core_delta = 0.0f ;
}
// If current primary has reset position after controller got it, add it to the delta
if ( core [ old_primary ] . getLastPosDownReset ( oldPosDownDelta ) > pos_down_reset_data . last_function_call ) {
pos_down_reset_data . core_delta + = oldPosDownDelta ;
}
// Record the position delta between current core and new primary core and the timestamp of the core change
// Add current delta in case it hasn't been consumed yet
core [ old_primary ] . getPosD ( posDownOldPrimary ) ;
core [ new_primary ] . getPosD ( posDownNewPrimary ) ;
pos_down_reset_data . core_delta = posDownNewPrimary - posDownOldPrimary + pos_down_reset_data . core_delta ;
pos_down_reset_data . last_primary_change = imuSampleTime_us / 1000 ;
pos_down_reset_data . core_changed = true ;
}
2015-09-21 02:18:49 -03:00
# endif //HAL_CPU_CLASS