mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 11:54:07 +08:00
ekf2: GPS, baro and range finder control parameters
Also remove the legacy "range aid" than can be achieved by setting the height reference to range finder and the range finder control parameter to "conditional". Conditional range aiding cal also be set when the height reference isn't the range finder. This prevents the ratchetting effect due to switching between references.
This commit is contained in:
bresch
Daniel Agar
parent
578e1339ca
commit
8962cf2d25
@ -73,7 +73,7 @@ param set-default EKF2_PCOEF_YN -0.4
|
||||
param set-default EKF2_PCOEF_YP -0.4
|
||||
|
||||
param set-default EKF2_RNG_A_VMAX 1.0
|
||||
param set-default EKF2_RNG_AID 0
|
||||
param set-default EKF2_RNG_CTRL 0
|
||||
param set-default EKF2_RNG_DELAY 55
|
||||
param set-default EKF2_RNG_POS_X -0.035
|
||||
param set-default EKF2_RNG_POS_Y 0.0
|
||||
|
||||
@ -113,21 +113,30 @@ enum HeightSensor : uint8_t {
|
||||
UNKNOWN = 4
|
||||
};
|
||||
|
||||
enum GnssCtrl : uint8_t {
|
||||
HPOS = (1<<0),
|
||||
VPOS = (1<<1),
|
||||
VEL = (1<<2),
|
||||
YAW = (1<<3)
|
||||
};
|
||||
|
||||
enum RngCtrl : uint8_t {
|
||||
DISABLED = 0,
|
||||
CONDITIONAL = 1,
|
||||
ENABLED = 2
|
||||
};
|
||||
|
||||
enum SensorFusionMask : uint16_t {
|
||||
// Bit locations for fusion_mode
|
||||
USE_GPS = (1<<0), ///< set to true to use GPS data
|
||||
/* USE_GPS = (1<<0), ///< set to true to use GPS data */
|
||||
USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data
|
||||
INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
|
||||
USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
|
||||
USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
|
||||
USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind
|
||||
ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available
|
||||
/* USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available */
|
||||
USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
|
||||
USE_BARO_HGT = (1<<9), ///< set to true to use baro data
|
||||
USE_GPS_HGT = (1<<10), ///< set to true to use GPS height data
|
||||
USE_RNG_HGT = (1<<11), ///< set to true to use range height data
|
||||
USE_EXT_VIS_HGT = (1<<12) ///< set to true to use external vision height data
|
||||
};
|
||||
|
||||
struct gpsMessage {
|
||||
@ -251,9 +260,11 @@ struct parameters {
|
||||
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
|
||||
|
||||
// measurement source control
|
||||
int32_t fusion_mode{SensorFusionMask::USE_GPS |
|
||||
SensorFusionMask::USE_BARO_HGT}; ///< bitmasked integer that selects which aiding sources will be used
|
||||
int32_t fusion_mode{}; ///< bitmasked integer that selects some aiding sources
|
||||
int32_t height_sensor_ref{HeightSensor::BARO};
|
||||
int32_t baro_ctrl{1};
|
||||
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
|
||||
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
|
||||
@ -335,9 +346,8 @@ struct parameters {
|
||||
float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
|
||||
float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
|
||||
const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
|
||||
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
|
||||
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
|
||||
int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met
|
||||
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
|
||||
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
|
||||
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
|
||||
float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
|
||||
float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
|
||||
|
||||
@ -268,7 +268,7 @@ void Ekf::controlExternalVisionFusion()
|
||||
_ev_sample_delayed.pos -= pos_offset_earth;
|
||||
|
||||
// Use an incremental position fusion method for EV position data if GPS is also used
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
|
||||
if (_params.gnss_ctrl & GnssCtrl::HPOS) {
|
||||
_fuse_hpos_as_odom = true;
|
||||
|
||||
} else {
|
||||
@ -572,7 +572,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
||||
|
||||
void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_YAW)
|
||||
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
|
||||
|| _control_status.flags.gps_yaw_fault) {
|
||||
|
||||
stopGpsYawFusion();
|
||||
@ -731,7 +731,7 @@ void Ekf::checkHeightSensorRefFallback()
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isRangeAidSuitable()
|
||||
bool Ekf::isConditionalRangeAidSuitable()
|
||||
{
|
||||
bool is_range_aid_suitable = false;
|
||||
|
||||
@ -764,7 +764,7 @@ bool Ekf::isRangeAidSuitable()
|
||||
|
||||
void Ekf::controlBaroHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_BARO_HGT)) {
|
||||
if (!(_params.baro_ctrl == 1)) {
|
||||
stopBaroHgtFusion();
|
||||
return;
|
||||
}
|
||||
@ -812,7 +812,7 @@ void Ekf::controlBaroHeightFusion()
|
||||
|
||||
void Ekf::controlGnssHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_HGT)) {
|
||||
if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) {
|
||||
stopGpsHgtFusion();
|
||||
return;
|
||||
}
|
||||
@ -855,7 +855,7 @@ void Ekf::controlGnssHeightFusion()
|
||||
|
||||
void Ekf::controlRangeHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_RNG_HGT) && (_params.range_aid == 0)) {
|
||||
if (!((_params.rng_ctrl == RngCtrl::CONDITIONAL) || (_params.rng_ctrl == RngCtrl::ENABLED))) {
|
||||
stopRngHgtFusion();
|
||||
return;
|
||||
}
|
||||
@ -876,18 +876,13 @@ void Ekf::controlRangeHeightFusion()
|
||||
if (_rng_data_ready) {
|
||||
updateRngHgt(_aid_src_rng_hgt);
|
||||
|
||||
const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();
|
||||
const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable();
|
||||
|
||||
const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && ((_params.range_aid == 0) || do_range_aid);
|
||||
const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid);
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _range_sensor.isRegularlySendingData();
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
if (do_range_aid) {
|
||||
// Force to be the height reference
|
||||
_height_sensor_ref = HeightSensorRef::RANGE;
|
||||
}
|
||||
|
||||
if (continuing_conditions_passing) {
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
|
||||
@ -922,7 +917,7 @@ void Ekf::controlRangeHeightFusion()
|
||||
|
||||
void Ekf::controlEvHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_HGT)) {
|
||||
if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter
|
||||
stopEvHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -948,8 +948,7 @@ private:
|
||||
void controlRangeHeightFusion();
|
||||
void controlEvHeightFusion();
|
||||
|
||||
// determine if flight condition is suitable to use range finder instead of the primary height sensor
|
||||
bool isRangeAidSuitable();
|
||||
bool isConditionalRangeAidSuitable();
|
||||
|
||||
void stopMagFusion();
|
||||
void stopMag3DFusion();
|
||||
|
||||
@ -860,7 +860,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
||||
const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal();
|
||||
|
||||
// TODO : calculate visual odometry limits
|
||||
const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
|
||||
const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);
|
||||
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
|
||||
|
||||
// Keep within range sensor limit when using rangefinder as primary height source
|
||||
|
||||
@ -437,7 +437,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
|
||||
|
||||
// using baro
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_BARO_HGT) {
|
||||
if (_params.baro_ctrl > 0) {
|
||||
max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
@ -451,12 +451,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
// range aid or range height
|
||||
if (_params.range_aid || (_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
|
||||
// using range finder
|
||||
if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
|
||||
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
|
||||
if (_params.gnss_ctrl > 0) {
|
||||
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
@ -539,6 +539,11 @@ bool EstimatorInterface::isVerticalPositionAidingActive() const
|
||||
return getNumberOfActiveVerticalPositionAidingSources() > 0;
|
||||
}
|
||||
|
||||
bool EstimatorInterface::isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const
|
||||
{
|
||||
return aiding_flag && !isOtherSourceOfVerticalPositionAidingThan(aiding_flag);
|
||||
}
|
||||
|
||||
int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
|
||||
{
|
||||
return int(_control_status.flags.gps_hgt)
|
||||
|
||||
@ -183,6 +183,7 @@ public:
|
||||
|
||||
bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const;
|
||||
bool isVerticalPositionAidingActive() const;
|
||||
bool isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const;
|
||||
int getNumberOfActiveVerticalPositionAidingSources() const;
|
||||
|
||||
bool isVerticalVelocityAidingActive() const;
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
|
||||
void Ekf::controlGpsFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS)) {
|
||||
if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
|
||||
stopGpsFusion();
|
||||
return;
|
||||
}
|
||||
@ -72,8 +72,13 @@ void Ekf::controlGpsFusion()
|
||||
if (continuing_conditions_passing
|
||||
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
|
||||
fuseGpsVel();
|
||||
fuseGpsPos();
|
||||
if (_params.gnss_ctrl & GnssCtrl::VEL) {
|
||||
fuseGpsVel();
|
||||
}
|
||||
|
||||
if ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VPOS)) {
|
||||
fuseGpsPos();
|
||||
}
|
||||
|
||||
if (shouldResetGpsFusion()) {
|
||||
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
|
||||
|
||||
@ -106,7 +106,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_req_hdrift(_params->req_hdrift),
|
||||
_param_ekf2_req_vdrift(_params->req_vdrift),
|
||||
_param_ekf2_aid_mask(_params->fusion_mode),
|
||||
_param_ekf2_hgt_mode(_params->height_sensor_ref),
|
||||
_param_ekf2_hgt_ref(_params->height_sensor_ref),
|
||||
_param_ekf2_baro_ctrl(_params->baro_ctrl),
|
||||
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
|
||||
_param_ekf2_rng_ctrl(_params->rng_ctrl),
|
||||
_param_ekf2_terr_mask(_params->terrain_fusion_mode),
|
||||
_param_ekf2_noaid_tout(_params->valid_timeout_max),
|
||||
_param_ekf2_rng_noise(_params->range_noise),
|
||||
@ -114,7 +117,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_rng_gate(_params->range_innov_gate),
|
||||
_param_ekf2_min_rng(_params->rng_gnd_clearance),
|
||||
_param_ekf2_rng_pitch(_params->rng_sens_pitch),
|
||||
_param_ekf2_rng_aid(_params->range_aid),
|
||||
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
|
||||
_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
|
||||
_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
|
||||
@ -292,7 +294,7 @@ void EKF2::Run()
|
||||
}
|
||||
|
||||
// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
|
||||
if (_params->fusion_mode & SensorFusionMask::USE_BARO_HGT) {
|
||||
if (_params->baro_ctrl == 1) {
|
||||
float sens_baro_rate = 0.f;
|
||||
|
||||
if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
|
||||
|
||||
@ -456,7 +456,10 @@ private:
|
||||
// measurement source control
|
||||
(ParamExtInt<px4::params::EKF2_AID_MASK>)
|
||||
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
|
||||
(ParamExtInt<px4::params::EKF2_HGT_MODE>) _param_ekf2_hgt_mode, ///< selects the primary source for height data
|
||||
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
|
||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
|
||||
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl, ///< GPS control selection
|
||||
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl, ///< range finder control selection
|
||||
(ParamExtInt<px4::params::EKF2_TERR_MASK>)
|
||||
_param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation
|
||||
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
|
||||
@ -470,8 +473,6 @@ private:
|
||||
_param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad)
|
||||
(ParamExtInt<px4::params::EKF2_RNG_AID>)
|
||||
_param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>)
|
||||
_param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
|
||||
|
||||
@ -600,44 +600,39 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
|
||||
* Integer bitmask controlling data fusion and aiding methods.
|
||||
*
|
||||
* Set bits in the following positions to enable:
|
||||
* 0 : Set to true to use GPS data if available
|
||||
* 0 : Deprecated, use EKF2_GPS_CTRL instead
|
||||
* 1 : Set to true to use optical flow data if available
|
||||
* 2 : Set to true to inhibit IMU delta velocity bias estimation
|
||||
* 3 : Set to true to enable vision position fusion
|
||||
* 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
|
||||
* 5 : Set to true to enable multi-rotor drag specific force fusion
|
||||
* 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
* 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
|
||||
* 7 : Deprecated, use EKF2_GPS_CTRL instead
|
||||
* 8 : Set to true to enable vision velocity fusion
|
||||
* 9 : Set to true to enable barometer height fusion.
|
||||
* 10 : Set to true to enable GPS height fusion.
|
||||
* 11 : Set to true to enable range finder height fusion.
|
||||
* 12 : Set to true to enable vision height fusion.
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 8091
|
||||
* @bit 0 position: GPS
|
||||
* @bit 1 velocity: optical flow
|
||||
* @max 511
|
||||
* @bit 0 unused
|
||||
* @bit 1 use optical flow
|
||||
* @bit 2 inhibit IMU bias estimation
|
||||
* @bit 3 position: vision
|
||||
* @bit 4 yaw: vision
|
||||
* @bit 5 wind: multi-rotor drag fusion
|
||||
* @bit 3 vision position fusion
|
||||
* @bit 4 vision yaw fusion
|
||||
* @bit 5 multi-rotor drag fusion
|
||||
* @bit 6 rotate external vision
|
||||
* @bit 7 yaw: GPS
|
||||
* @bit 8 velocity: vision
|
||||
* @bit 9 height: barometer
|
||||
* @bit 10 height: GPS
|
||||
* @bit 11 height: range finder
|
||||
* @bit 12 height: vision
|
||||
* @bit 7 unused
|
||||
* @bit 8 vision velocity fusion
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_AID_MASK, 513);
|
||||
PARAM_DEFINE_INT32(EKF2_AID_MASK, 0);
|
||||
|
||||
/**
|
||||
* Determines the primary source of height data used by the EKF.
|
||||
* Determines the reference source of height data used by the EKF.
|
||||
*
|
||||
* The range sensor option should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level.
|
||||
* When multiple height sources are enabled at the same time, the height estimate will
|
||||
* always converge towards the reference height source selected by this parameter.
|
||||
*
|
||||
* The range sensor and vision options should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level.
|
||||
*
|
||||
* @group EKF2
|
||||
* @value 0 Barometric pressure
|
||||
@ -646,7 +641,61 @@ PARAM_DEFINE_INT32(EKF2_AID_MASK, 513);
|
||||
* @value 3 Vision
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0);
|
||||
PARAM_DEFINE_INT32(EKF2_HGT_REF, 1);
|
||||
|
||||
/**
|
||||
* Barometric sensor height aiding
|
||||
*
|
||||
* If this parameter is enabled then the estimator will make use of the barometric height measurements to estimate it's height in addition to other
|
||||
* height sources (if activated).
|
||||
*
|
||||
* @group EKF2
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1);
|
||||
|
||||
/**
|
||||
* GNSS sensor aiding
|
||||
*
|
||||
* Set bits in the following positions to enable:
|
||||
* 0 : Longitude and latitude fusion
|
||||
* 1 : Altitude fusion
|
||||
* 2 : 3D velocity fusion
|
||||
* 3 : Dual antenna heading fusion
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 15
|
||||
* @bit 0 Lon/lat
|
||||
* @bit 1 Altitude
|
||||
* @bit 2 3D velocity
|
||||
* @bit 3 Dual antenna heading
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_GPS_CTRL, 7);
|
||||
|
||||
/**
|
||||
* Range sensor height aiding
|
||||
*
|
||||
* WARNING: Range finder measurements are less reliable and can experience unexpected errors.
|
||||
* For these reasons, if accurate control of height relative to ground is required, it is recommended to use the MPC_ALT_MODE parameter instead,
|
||||
* unless baro errors are severe enough to cause problems with landing and takeoff.
|
||||
*
|
||||
* To en-/disable range finder for terrain height estimation, use EKF2_TERR_MASK instead.
|
||||
*
|
||||
* If this parameter is enabled then the estimator will make use of the range finder measurements to estimate it's height in addition to other
|
||||
* height sources (if activated). Range sensor aiding can be enabled (i.e.: always use) or set in "conditional" mode.
|
||||
*
|
||||
* Conditional mode: This enables the range finder to be used during low speed (< EKF2_RNG_A_VMAX) and low altitude (< EKF2_RNG_A_HMAX)
|
||||
* operation, eg takeoff and landing, where baro interference from rotor wash is excessive and can corrupt EKF state
|
||||
* estimates. It is intended to be used where a vertical takeoff and landing is performed, and horizontal flight does
|
||||
* not occur until above EKF2_RNG_A_HMAX.
|
||||
*
|
||||
* @group EKF2
|
||||
* @value 0 Disable range fusion
|
||||
* @value 1 Enabled (conditional mode)
|
||||
* @value 2 Enabled
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_RNG_CTRL, 1);
|
||||
|
||||
/**
|
||||
* Integer bitmask controlling fusion sources of the terrain estimator
|
||||
@ -1056,30 +1105,10 @@ PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
|
||||
|
||||
/**
|
||||
* Range sensor aid.
|
||||
*
|
||||
* If this parameter is enabled then the estimator will make use of the range finder measurements
|
||||
* to estimate it's height even if range sensor is not the primary height source. It will only do so if conditions
|
||||
* for range measurement fusion are met. This enables the range finder to be used during low speed and low altitude
|
||||
* operation, eg takeoff and landing, where baro interference from rotor wash is excessive and can corrupt EKF state
|
||||
* estimates. It is intended to be used where a vertical takeoff and landing is performed, and horizontal flight does
|
||||
* not occur until above EKF2_RNG_A_HMAX. If vehicle motion causes repeated switching between the primary height
|
||||
* sensor and range finder, an offset in the local position origin can accumulate. Also range finder measurements
|
||||
* are less reliable and can experience unexpected errors. For these reasons, if accurate control of height
|
||||
* relative to ground is required, it is recommended to use the MPC_ALT_MODE parameter instead, unless baro errors
|
||||
* are severe enough to cause problems with landing and takeoff.
|
||||
*
|
||||
* @group EKF2
|
||||
* @value 0 Range aid disabled
|
||||
* @value 1 Range aid enabled
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_RNG_AID, 1);
|
||||
|
||||
/**
|
||||
* Maximum horizontal velocity allowed for range aid mode.
|
||||
* Maximum horizontal velocity allowed for conditional range aid mode.
|
||||
*
|
||||
* If the vehicle horizontal speed exceeds this value then the estimator will not fuse range measurements
|
||||
* to estimate it's height. This only applies when range aid mode is activated (EKF2_RNG_AID = enabled).
|
||||
* to estimate it's height. This only applies when conditional range aid mode is activated (EKF2_RNG_CTRL = 1).
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0.1
|
||||
@ -1089,10 +1118,10 @@ PARAM_DEFINE_INT32(EKF2_RNG_AID, 1);
|
||||
PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Maximum absolute altitude (height above ground level) allowed for range aid mode.
|
||||
* Maximum absolute altitude (height above ground level) allowed for conditional range aid mode.
|
||||
*
|
||||
* If the vehicle absolute altitude exceeds this value then the estimator will not fuse range measurements
|
||||
* to estimate it's height. This only applies when range aid mode is activated (EKF2_RNG_AID = enabled).
|
||||
* to estimate it's height. This only applies when conditional range aid mode is activated (EKF2_RNG_CTRL = 1).
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 1.0
|
||||
|
||||
@ -12,17 +12,17 @@ EkfWrapper::~EkfWrapper()
|
||||
|
||||
void EkfWrapper::setBaroHeightRef()
|
||||
{
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::BARO;
|
||||
_ekf_params->height_sensor_ref = HeightSensor::BARO;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableBaroHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT;
|
||||
_ekf_params->baro_ctrl = 1;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableBaroHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT;
|
||||
_ekf_params->baro_ctrl = 0;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingBaroHeightFusion() const
|
||||
@ -32,17 +32,17 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
|
||||
|
||||
void EkfWrapper::setGpsHeightRef()
|
||||
{
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::GPS;
|
||||
_ekf_params->height_sensor_ref = HeightSensor::GNSS;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableGpsHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT;
|
||||
_ekf_params->gnss_ctrl |= GnssCtrl::VPOS;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableGpsHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT;
|
||||
_ekf_params->gnss_ctrl &= ~GnssCtrl::VPOS;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsHeightFusion() const
|
||||
@ -52,17 +52,17 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
|
||||
|
||||
void EkfWrapper::setRangeHeightRef()
|
||||
{
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::RANGE;
|
||||
_ekf_params->height_sensor_ref = HeightSensor::RANGE;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableRangeHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT;
|
||||
_ekf_params->rng_ctrl = RngCtrl::ENABLED;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableRangeHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT;
|
||||
_ekf_params->rng_ctrl = RngCtrl::DISABLED;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingRangeHeightFusion() const
|
||||
@ -72,17 +72,12 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
|
||||
|
||||
void EkfWrapper::setExternalVisionHeightRef()
|
||||
{
|
||||
_ekf_params->height_sensor_ref |= HeightSensorRef::EV;
|
||||
_ekf_params->height_sensor_ref = HeightSensor::EV;
|
||||
}
|
||||
|
||||
void EkfWrapper::enableExternalVisionHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableExternalVisionHeightFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT;
|
||||
setExternalVisionHeightRef(); // TODO: replace by EV control parameter
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
|
||||
@ -92,12 +87,12 @@ bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
|
||||
|
||||
void EkfWrapper::enableGpsFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS;
|
||||
_ekf_params->gnss_ctrl |= GnssCtrl::HPOS | GnssCtrl::VEL;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableGpsFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS;
|
||||
_ekf_params->gnss_ctrl &= ~(GnssCtrl::HPOS | GnssCtrl::VEL);
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsFusion() const
|
||||
@ -107,12 +102,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
|
||||
|
||||
void EkfWrapper::enableGpsHeadingFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_YAW;
|
||||
_ekf_params->gnss_ctrl |= GnssCtrl::YAW;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableGpsHeadingFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_YAW;
|
||||
_ekf_params->gnss_ctrl &= ~GnssCtrl::YAW;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsHeadingFusion() const
|
||||
|
||||
@ -66,7 +66,7 @@ public:
|
||||
|
||||
void setExternalVisionHeightRef();
|
||||
void enableExternalVisionHeightFusion();
|
||||
void disableExternalVisionHeightFusion();
|
||||
/* void disableExternalVisionHeightFusion(); */
|
||||
bool isIntendingExternalVisionHeightFusion() const;
|
||||
|
||||
void enableGpsFusion();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user