forked from Archive/PX4-Autopilot
Ekf: use helper functions to centralize the checks of horizontal aiding (gps, flow, ev_pos, ev_vel)
This commit is contained in:
parent
a3e1eb9d50
commit
e3af91c9ce
|
@ -440,7 +440,7 @@ void Ekf::controlOpticalFlowFusion()
|
|||
// Check if we are in-air and require optical flow to control position drift
|
||||
bool flow_required = _control_status.flags.in_air &&
|
||||
(_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
||||
|| (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel) // is completely reliant on optical flow
|
||||
|| (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow))
|
||||
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
|
||||
|
||||
if (!_inhibit_flow_use && _control_status.flags.opt_flow) {
|
||||
|
@ -491,7 +491,7 @@ void Ekf::controlOpticalFlowFusion()
|
|||
_time_last_of_fuse = _time_last_imu;
|
||||
|
||||
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
|
||||
const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
|
||||
const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
|
||||
if (flow_aid_only) {
|
||||
resetVelocity();
|
||||
resetPosition();
|
||||
|
@ -506,10 +506,7 @@ void Ekf::controlOpticalFlowFusion()
|
|||
}
|
||||
|
||||
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
|
||||
if (_control_status.flags.opt_flow
|
||||
&& !_control_status.flags.gps
|
||||
&& !_control_status.flags.ev_pos
|
||||
&& !_control_status.flags.ev_vel) {
|
||||
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
|
||||
|
||||
bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
|
||||
|
||||
|
@ -639,7 +636,7 @@ void Ekf::controlGpsFusion()
|
|||
}
|
||||
|
||||
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
|
||||
if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
|
||||
if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
stopGpsFusion();
|
||||
// Reset position state to external vision if we are going to use absolute values
|
||||
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
|
||||
|
@ -703,7 +700,7 @@ void Ekf::controlGpsFusion()
|
|||
|
||||
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
|
||||
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
|
||||
if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
// if we are using other sources of aiding, then relax the upper observation
|
||||
// noise limit which prevents bad GPS perturbing the position estimate
|
||||
gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit));
|
||||
|
@ -738,7 +735,7 @@ void Ekf::controlGpsFusion()
|
|||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
|
||||
stopGpsFusion();
|
||||
ECL_WARN_TIMESTAMPED("GPS data stopped");
|
||||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
|
||||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
// Handle the case where we are fusing another position source along GPS,
|
||||
// stop waiting for GPS after 1 s of lost signal
|
||||
stopGpsFusion();
|
||||
|
@ -1109,8 +1106,6 @@ void Ekf::controlHeightFusion()
|
|||
|
||||
if (_fuse_height) {
|
||||
|
||||
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
Vector2f baro_hgt_innov_gate;
|
||||
Vector3f baro_hgt_obs_var;
|
||||
|
@ -1192,15 +1187,10 @@ void Ekf::controlHeightFusion()
|
|||
|
||||
void Ekf::checkRangeAidSuitability()
|
||||
{
|
||||
const bool horz_vel_valid = _control_status.flags.gps
|
||||
|| _control_status.flags.ev_pos
|
||||
|| _control_status.flags.ev_vel
|
||||
|| _control_status.flags.opt_flow;
|
||||
|
||||
if (_control_status.flags.in_air
|
||||
&& _rng_hgt_valid
|
||||
&& isTerrainEstimateValid()
|
||||
&& horz_vel_valid) {
|
||||
&& isHorizontalAidingActive()) {
|
||||
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
|
||||
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
|
||||
const bool is_in_range = _is_range_aid_suitable
|
||||
|
@ -1328,11 +1318,8 @@ void Ekf::controlFakePosFusion()
|
|||
// if we aren't doing any aiding, fake position measurements at the last known position to constrain drift
|
||||
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
|
||||
|
||||
if (!_control_status.flags.gps &&
|
||||
!_control_status.flags.opt_flow &&
|
||||
!_control_status.flags.ev_pos &&
|
||||
!_control_status.flags.ev_vel &&
|
||||
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
|
||||
if (!isHorizontalAidingActive()
|
||||
&& !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
|
||||
|
||||
// We now need to use a synthetic position observation to prevent unconstrained drift of the INS states.
|
||||
_using_synthetic_position = true;
|
||||
|
@ -1383,9 +1370,8 @@ void Ekf::controlFakePosFusion()
|
|||
void Ekf::controlAuxVelFusion()
|
||||
{
|
||||
bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
|
||||
bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow;
|
||||
|
||||
if (data_ready && primary_aiding) {
|
||||
if (data_ready && isHorizontalAidingActive()) {
|
||||
|
||||
Vector2f aux_vel_innov_gate;
|
||||
Vector3f aux_vel_obs_var;
|
||||
|
|
|
@ -1231,7 +1231,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
|
||||
const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
|
||||
|
||||
const bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
|
||||
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
|
||||
|
||||
// Do not require limiting by default
|
||||
*vxy_max = NAN;
|
||||
|
@ -1313,7 +1313,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
|
|||
ekf_solution_status soln_status;
|
||||
// TODO: Is this accurate enough?
|
||||
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos|| _control_status.flags.ev_vel || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
|
||||
|
@ -1375,8 +1375,6 @@ void Ekf::uncorrelateQuatStates()
|
|||
P.uncorrelateCovariance<4>(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool Ekf::global_position_is_valid()
|
||||
{
|
||||
// return true if the origin is set we are not doing unconstrained free inertial navigation
|
||||
|
|
|
@ -374,7 +374,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
|
|||
delta_time = delta_time_min;
|
||||
}
|
||||
|
||||
const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
|
||||
const bool relying_on_flow = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
|
||||
|
||||
const bool flow_quality_good = (flow.quality >= _params.flow_qual_min);
|
||||
|
||||
|
@ -571,6 +571,30 @@ bool EstimatorInterface::local_position_is_valid()
|
|||
return !_deadreckon_time_exceeded;
|
||||
}
|
||||
|
||||
bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
|
||||
{
|
||||
return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
|
||||
}
|
||||
|
||||
bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
|
||||
{
|
||||
const int nb_sources = getNumberOfActiveHorizontalAidingSources();
|
||||
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
|
||||
}
|
||||
|
||||
int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
|
||||
{
|
||||
return int(_control_status.flags.gps)
|
||||
+ int(_control_status.flags.opt_flow)
|
||||
+ int(_control_status.flags.ev_pos)
|
||||
+ int(_control_status.flags.ev_vel);
|
||||
}
|
||||
|
||||
bool EstimatorInterface::isHorizontalAidingActive() const
|
||||
{
|
||||
return getNumberOfActiveHorizontalAidingSources() > 0;
|
||||
}
|
||||
|
||||
void EstimatorInterface::printBufferAllocationFailed(const char * buffer_name)
|
||||
{
|
||||
if(buffer_name)
|
||||
|
|
|
@ -252,6 +252,27 @@ public:
|
|||
// return true if the global position estimate is valid
|
||||
virtual bool global_position_is_valid() = 0;
|
||||
|
||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
||||
bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
|
||||
|
||||
/*
|
||||
* Check if there are any other active source of horizontal aiding
|
||||
* Warning: does not tell if the selected source is
|
||||
* active, use isOnlyActiveSourceOfHorizontalAiding() for this
|
||||
*
|
||||
* The flags considered are opt_flow, gps, ev_vel and ev_pos
|
||||
*
|
||||
* @param aiding_flag a flag in _control_status.flags
|
||||
* @return true if an other source than aiding_flag is active
|
||||
*/
|
||||
bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const;
|
||||
|
||||
// Return true if at least one source of horizontal aiding is active
|
||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
||||
bool isHorizontalAidingActive() const;
|
||||
|
||||
int getNumberOfActiveHorizontalAidingSources() const;
|
||||
|
||||
// return true if the EKF is dead reckoning the position using inertial data only
|
||||
bool inertial_dead_reckoning() {return _is_dead_reckoning;}
|
||||
|
||||
|
|
|
@ -67,7 +67,7 @@ bool Ekf::collect_gps(const gps_message &gps)
|
|||
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
|
||||
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigationg
|
||||
if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
|
||||
if (isHorizontalAidingActive()) {
|
||||
double est_lat, est_lon;
|
||||
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
|
||||
map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
|
||||
|
|
Loading…
Reference in New Issue