fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 05:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add filter control logic

Browse Source
This commit is contained in:
Paul Riseborough
2016-01-30 09:01:48 +11:00
parent 9236f11c80
commit 46b0e9654c
10 changed files with 309 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+2 -1
View File
@@ -54,7 +54,8 @@ px4_add_module(
EKF/covariance.cpp
EKF/vel_pos_fusion.cpp
EKF/mag_fusion.cpp
EKF/gps_checks.cpp
EKF/gps_checks.cpp
EKF/control.cpp
DEPENDS
platforms__common
)
EKF/control.cpp
+129
View File
@@ -0,0 +1,129 @@
/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file control.cpp
* Control functions for ekf attitude and position estimator.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "ekf.h"
void Ekf::controlFusionModes()
{
// Determine the vehicle status
calculateVehicleStatus();
// optical flow fusion mode selection logic
_control_status.flags.opt_flow = false;
// GPS fusion mode selection logic
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
if (!_control_status.flags.gps) {
if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _gps_initialised) {
_control_status.flags.gps = true;
resetPosition();
resetVelocity();
}
}
// decide when to start using optical flow data
if (!_control_status.flags.opt_flow) {
// TODO optical flow start logic
}
// handle the case when we are relying on GPS fusion and lose it
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
if (_time_last_imu - _time_last_gps > 5e5) {
// if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states
// and set the synthetic GPS position to the current estimate
_control_status.flags.gps = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
} else {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
}
}
}
// handle the case when we are relying on optical flow fusion and lose it
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
// TODO
}
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
// or the more accurate 3-axis fusion
if (!_control_status.flags.armed) {
// always use simple mag fusion for initial startup
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
} else {
if (_control_status.flags.in_air) {
// always use 3-axis mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true;
} else {
// always use simple heading fusion when on the ground
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
}
}
}
void Ekf::calculateVehicleStatus()
{
// determine if the vehicle is armed
_control_status.flags.armed = _vehicle_armed;
// record vertical position whilst disarmed to use as a height change reference
if (!_control_status.flags.armed) {
_last_disarmed_posD = _state.pos(2);
}
// Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) {
_control_status.flags.in_air = true;
}
// Transition to on-ground occurs when disarmed.
if (_control_status.flags.in_air && !_control_status.flags.armed) {
_control_status.flags.in_air = false;
}
}
EKF/ekf.cpp
+37 -15
View File
@@ -43,25 +43,31 @@
#include <drivers/drv_hrt.h>
Ekf::Ekf():
_filter_initialised(false),
_control_status{},
_filter_initialised(false),
_earth_rate_initialised(false),
_fuse_height(false),
_fuse_pos(false),
_fuse_hor_vel(false),
_fuse_vert_vel(false),
_time_last_fake_gps(0),
_vel_pos_innov{},
_time_last_fake_gps(0),
_time_last_pos_fuse(0),
_time_last_vel_fuse(0),
_time_last_hgt_fuse(0),
_time_last_of_fuse(0),
_vel_pos_innov{},
_mag_innov{},
_heading_innov{},
_vel_pos_innov_var{},
_mag_innov_var{},
_heading_innov_var{}
_heading_innov_var{}
{
_earth_rate_NED.setZero();
_R_prev = matrix::Dcm<float>();
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
_last_known_posNE.setZero();
}
@@ -91,28 +97,41 @@ bool Ekf::update()
predictCovariance();
}
// measurement updates
// control logic
controlFusionModes();
// measurement updates
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
//fuseHeading();
fuseMag();
if (_control_status.flags.mag_3D && _control_status.flags.angle_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
// TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding
// otherwise heading will slowly drift
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
fuseHeading();
}
}
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
_fuse_height = true;
}
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
// If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it
// if we aren't doing any aiding, fake GPS 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 (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
// if gps does not update then fake position and horizontal veloctiy measurements
_fuse_hor_vel = true; // we only fake horizontal velocity because we still have baro
_gps_sample_delayed.vel.setZero();
}
} else if (!_control_status.flags.gps && !_control_status.flags.opt_flow && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
_time_last_fake_gps = _time_last_imu;
}
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
@@ -178,6 +197,9 @@ bool Ekf::initialiseFilter(void)
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// TODO replace this with a conditional test based on fitered angle error states.
_control_status.flags.angle_align = true;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;
EKF/ekf.h
+75 -2
View File
@@ -36,6 +36,7 @@
* Class for core functions for ekf attitude and position estimator.
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
@@ -78,6 +79,28 @@ public:
// get the diagonal elements of the covariance matrix
void get_covariances(float *covariances);
// bitmask containing filter control status
union filter_control_status_u {
struct {
uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
uint8_t gps : 1; // 1 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused
uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 6 - true when the vehicle is airborne
uint8_t armed : 1; // 7 - true when the vehicle motors are armed
} flags;
uint16_t value;
};
filter_control_status_u _control_status={};
// get the ekf WGS-84 origin positoin and height and the system time it was last set
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
// set vehicle arm status data
void set_arm_status(bool data);
private:
static const uint8_t _k_num_states = 24;
@@ -91,7 +114,14 @@ private:
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
uint64_t _time_last_fake_gps;
uint64_t _time_last_fake_gps;
uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec)
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec)
uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec)
Vector2f _last_known_posNE; // last known local NE position vector (m)
float _last_disarmed_posD; // vertical position recorded at arming (m)
Vector3f _earth_rate_NED;
@@ -112,7 +142,40 @@ private:
Vector3f _delta_vel_corr;
Vector3f _vel_corr;
void calculateOutputStates();
// variables used for the GPS quality checks
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s)
float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s)
float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s)
float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground
// publish the status of various GPS quality checks
union gps_check_fail_status_u {
struct {
uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient
uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient
uint16_t hacc : 1; // 2 - true if reported horizontal accuracy is insufficient
uint16_t vacc : 1; // 3 - true if reported vertical accuracy is insufficient
uint16_t sacc : 1; // 4 - true if reported speed accuracy is insufficient
uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground)
uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive
} flags;
uint16_t value;
};
gps_check_fail_status_u _gps_check_fail_status;
void calculateOutputStates();
bool initialiseFilter(void);
@@ -156,4 +219,14 @@ private:
void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
void initialiseGPS(struct gps_message *gps);
// return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(struct gps_message *gps);
// Control the filter fusion modes
void controlFusionModes();
// Determine if we are airborne or motors are armed
void calculateVehicleStatus();
};
EKF/ekf_helper.cpp
+15
View File
@@ -254,3 +254,18 @@ void Ekf::get_covariances(float *covariances)
covariances[i] = P[i][i];
}
}
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
{
memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
}
// set the vehicle arm status
void Ekf::set_arm_status(bool data)
{
_vehicle_armed = data;
}
EKF/estimator_base.cpp
+2 -20
View File
@@ -165,22 +165,20 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
return;
}
if (time_usec - _time_last_gps > 70000) {
// Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz
if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) {
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
_gps_speed_valid = gps->vel_ned_valid;
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
@@ -293,7 +291,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
_gps_speed_valid = false;
_mag_healthy = false;
_in_air = false; // XXX get this flag from the application
_time_last_imu = 0;
_time_last_gps = 0;
@@ -306,21 +303,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
}
void EstimatorBase::initialiseGPS(struct gps_message *gps)
{
//Check if the GPS fix is good enough for us to use
if (gps_is_good(gps)) {
printf("gps is good\n");
// Initialise projection
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init(&_pos_ref, lat, lon);
_gps_alt_ref = gps->alt / 1e3f;
_gps_initialised = true;
_last_gps_origin_time_us = _time_last_imu;
}
}
bool EstimatorBase::position_is_valid()
{
// return true if the position estimate is valid
EKF/estimator_base.h
+7 -38
View File
@@ -159,6 +159,13 @@ public:
// return a address to the parameters struct
// in order to give access to the application
parameters *getParamHandle() {return &_params;}
// get the ekf WGS-84 origin positoin and height and the system time it was last set
virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
// set vehicle arm status data
void set_arm_status(bool data);
protected:
typedef matrix::Vector<float, 2> Vector2f;
@@ -267,8 +274,6 @@ protected:
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
bool _in_air = true; // indicates if the vehicle is in the air
RingBuffer<imuSample> _imu_buffer;
RingBuffer<gpsSample> _gps_buffer;
RingBuffer<magSample> _mag_buffer;
@@ -296,19 +301,6 @@ protected:
void initialiseVariables(uint64_t timestamp);
void initialiseGPS(struct gps_message *gps);
bool gps_is_good(struct gps_message *gps);
// variables used for the GPS quality checks
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s)
float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s)
float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s)
float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks
public:
void printIMU(struct imuSample *data);
void printStoredIMU();
@@ -345,29 +337,6 @@ public:
{
*time_us = _imu_time_last;
}
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground
// publish the status of various GPS quality checks
union gps_check_fail_status_u {
struct {
uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient
uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient
uint16_t hacc : 1; // 2 - true if reported horizontal accuracy is insufficient
uint16_t vacc : 1; // 3 - true if reported vertical accuracy is insufficient
uint16_t sacc : 1; // 4 - true if reported speed accuracy is insufficient
uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground)
uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive
} flags;
uint16_t value;
};
gps_check_fail_status_u _gps_check_fail_status;
};
EKF/gps_checks.cpp
+19 -4
View File
@@ -52,6 +52,21 @@
#define MASK_GPS_HSPD (1<<7)
#define MASK_GPS_VSPD (1<<8)
void Ekf::initialiseGPS(struct gps_message *gps)
{
//Check if the GPS fix is good enough for us to use
if (gps_is_good(gps)) {
printf("gps is good\n");
// Initialise projection
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init(&_pos_ref, lat, lon);
_gps_alt_ref = gps->alt / 1e3f;
_gps_initialised = true;
_last_gps_origin_time_us = _time_last_imu;
}
}
/*
* Return true if the GPS solution quality is adequate to set an origin for the EKF
* and start GPS aiding.
@@ -59,7 +74,7 @@
* Checks are activated using the EKF2_GPS_CHECKS bitmask parameter
* Checks are adjusted using the EKF2_REQ_* parameters
*/
bool EstimatorBase::gps_is_good(struct gps_message *gps)
bool Ekf::gps_is_good(struct gps_message *gps)
{
// Check the number of satellites
_gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats);
@@ -104,7 +119,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
// Calculate the horizontal drift speed and fail if too high
// This check can only be used if the vehicle is stationary during alignment
if(_vehicle_armed) {
if(!_control_status.flags.armed) {
float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
_gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
} else {
@@ -127,7 +142,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
// Fail if the vertical drift speed is too high
// This check can only be used if the vehicle is stationary during alignment
if(_vehicle_armed) {
if(!_control_status.flags.armed) {
_gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
} else {
_gps_check_fail_status.flags.vdrift = false;
@@ -135,7 +150,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
// Check the magnitude of the filtered horizontal GPS velocity
// This check can only be used if the vehicle is stationary during alignment
if (_vehicle_armed) {
if (!_control_status.flags.armed) {
vel_limit = 10.0f * _params.req_hdrift;
float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit);
float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit);
EKF/mag_fusion.cpp
+1 -1
View File
@@ -561,7 +561,7 @@ void Ekf::fuseHeading()
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (_in_air) {
if (_control_status.flags.in_air) {
return;
}
EKF/vel_pos_fusion.cpp
+22 -3
View File
@@ -49,7 +49,7 @@ void Ekf::fuseVelPosHeight()
float gate_size[6] = {};
float Kfusion[24] = {};
// calculate innovations
// calculate innovations and gate sizes
if (_fuse_hor_vel) {
fuse_map[0] = fuse_map[1] = true;
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
@@ -97,10 +97,29 @@ void Ekf::fuseVelPosHeight()
// check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[4] = innov_check_pass_map[3] = (_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f);
// always pass position checks if using synthetic position measurements
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || using_synthetic_measurements;
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
}
// record the successful position fusion time
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
}
// record the successful height fusion time
if (innov_check_pass_map[5] && _fuse_height) {
_time_last_hgt_fuse = _time_last_imu;
}
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
// skip fusion if not requested or checks have failed
if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {