forked from Archive/PX4-Autopilot
Replace EKF/common.h #defines with enums.
This commit is contained in:
mcsauder
Daniel Agar
parent
8f2c84d36f
commit
ea10eacb99
|
|
@ -56,26 +56,81 @@ using matrix::Vector2f;
|
|||
using matrix::Vector3f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
enum class velocity_frame_t : uint8_t {
|
||||
LOCAL_FRAME_FRD,
|
||||
BODY_FRAME_FRD
|
||||
// maximum sensor intervals in usec
|
||||
#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
|
||||
#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
|
||||
#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
|
||||
#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec)
|
||||
|
||||
// bad accelerometer detection and mitigation
|
||||
#define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
|
||||
#define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
|
||||
|
||||
// ground effect compensation
|
||||
#define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
|
||||
|
||||
enum class VelocityFrame : uint8_t {
|
||||
LOCAL_FRAME_FRD = 0,
|
||||
BODY_FRAME_FRD = 1
|
||||
};
|
||||
|
||||
struct gps_message {
|
||||
enum GeoDeclinationMask : uint8_t {
|
||||
// Bit locations for mag_declination_source
|
||||
USE_GEO_DECL = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
|
||||
SAVE_GEO_DECL = (1<<1), ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
|
||||
FUSE_DECL = (1<<2) ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
|
||||
};
|
||||
|
||||
enum MagFuseType : uint8_t {
|
||||
// Integer definitions for mag_fusion_type
|
||||
AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
MAG_3D = 2, ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
INDOOR = 3, ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
|
||||
NONE = 4 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
|
||||
};
|
||||
|
||||
enum TerrainFusionMask : uint8_t {
|
||||
TerrainFuseRangeFinder = (1 << 0),
|
||||
TerrainFuseOpticalFlow = (1 << 1)
|
||||
};
|
||||
|
||||
enum VerticalHeightSensor : uint8_t {
|
||||
// Integer definitions for vdist_sensor_type
|
||||
BARO = 0, ///< Use baro height
|
||||
GPS = 1, ///< Use GPS height
|
||||
RANGE = 2, ///< Use range finder height
|
||||
EV = 3 ///< Use external vision
|
||||
};
|
||||
|
||||
enum SensorFusionMask : uint16_t {
|
||||
// Bit locations for fusion_mode
|
||||
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_EXT_VIS_VEL = (1<<8) ///< set to true to use external vision velocity data
|
||||
};
|
||||
|
||||
struct gpsMessage {
|
||||
uint64_t time_usec{};
|
||||
int32_t lat{}; ///< Latitude in 1E-7 degrees
|
||||
int32_t lon{}; ///< Longitude in 1E-7 degrees
|
||||
int32_t alt{}; ///< Altitude in 1E-3 meters (millimeters) above MSL
|
||||
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
|
||||
float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
|
||||
uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
|
||||
float eph{}; ///< GPS horizontal position accuracy in m
|
||||
float epv{}; ///< GPS vertical position accuracy in m
|
||||
float sacc{}; ///< GPS speed accuracy in m/s
|
||||
float vel_m_s{}; ///< GPS ground speed (m/sec)
|
||||
Vector3f vel_ned{}; ///< GPS ground speed NED
|
||||
bool vel_ned_valid{}; ///< GPS ground speed is valid
|
||||
uint8_t nsats{}; ///< number of satellites used
|
||||
int32_t lat{}; ///< Latitude in 1E-7 degrees
|
||||
int32_t lon{}; ///< Longitude in 1E-7 degrees
|
||||
int32_t alt{}; ///< Altitude in 1E-3 meters (millimeters) above MSL
|
||||
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
|
||||
float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
|
||||
uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
|
||||
float eph{}; ///< GPS horizontal position accuracy in m
|
||||
float epv{}; ///< GPS vertical position accuracy in m
|
||||
float sacc{}; ///< GPS speed accuracy in m/s
|
||||
float vel_m_s{}; ///< GPS ground speed (m/sec)
|
||||
Vector3f vel_ned{}; ///< GPS ground speed NED
|
||||
bool vel_ned_valid{}; ///< GPS ground speed is valid
|
||||
uint8_t nsats{}; ///< number of satellites used
|
||||
float pdop{}; ///< position dilution of precision
|
||||
};
|
||||
|
||||
|
|
@ -151,7 +206,7 @@ struct extVisionSample {
|
|||
Vector3f posVar{}; ///< XYZ position variances (m**2)
|
||||
Matrix3f velCov{}; ///< XYZ velocity covariances ((m/sec)**2)
|
||||
float angVar{}; ///< angular heading variance (rad**2)
|
||||
velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
|
||||
VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
||||
uint8_t reset_counter{};
|
||||
};
|
||||
|
||||
|
|
@ -177,61 +232,13 @@ struct stateSample {
|
|||
Vector2f wind_vel{}; ///< horizontal wind velocity in earth frame in m/s
|
||||
};
|
||||
|
||||
// Integer definitions for vdist_sensor_type
|
||||
#define VDIST_SENSOR_BARO 0 ///< Use baro height
|
||||
#define VDIST_SENSOR_GPS 1 ///< Use GPS height
|
||||
#define VDIST_SENSOR_RANGE 2 ///< Use range finder height
|
||||
#define VDIST_SENSOR_EV 3 ///< Use external vision
|
||||
|
||||
// Bit locations for mag_declination_source
|
||||
#define MASK_USE_GEO_DECL (1<<0) ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
|
||||
#define MASK_SAVE_GEO_DECL (1<<1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
|
||||
#define MASK_FUSE_DECL (1<<2) ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
|
||||
|
||||
// Bit locations for fusion_mode
|
||||
#define MASK_USE_GPS (1<<0) ///< set to true to use GPS data
|
||||
#define MASK_USE_OF (1<<1) ///< set to true to use optical flow data
|
||||
#define MASK_INHIBIT_ACC_BIAS (1<<2) ///< set to true to inhibit estimation of accelerometer delta velocity bias
|
||||
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision position data
|
||||
#define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw
|
||||
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
|
||||
#define MASK_ROTATE_EV (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
|
||||
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
|
||||
#define MASK_USE_EVVEL (1<<8) ///< set to true to use external vision velocity data
|
||||
|
||||
enum TerrainFusionMask : int32_t {
|
||||
TerrainFuseRangeFinder = (1 << 0),
|
||||
TerrainFuseOpticalFlow = (1 << 1)
|
||||
};
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
#define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
#define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
#define MAG_FUSE_TYPE_UNUSED 3 ///< Not implemented
|
||||
#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
|
||||
#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
|
||||
|
||||
// Maximum sensor intervals in usec
|
||||
#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
|
||||
#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
|
||||
#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec)
|
||||
#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
|
||||
|
||||
// bad accelerometer detection and mitigation
|
||||
#define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
|
||||
#define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
|
||||
|
||||
// ground effect compensation
|
||||
#define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
|
||||
|
||||
struct parameters {
|
||||
|
||||
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
|
||||
|
||||
// measurement source control
|
||||
int32_t fusion_mode{MASK_USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used
|
||||
int32_t vdist_sensor_type{VDIST_SENSOR_BARO}; ///< selects the primary source for height data
|
||||
int32_t fusion_mode{SensorFusionMask::USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used
|
||||
int32_t vdist_sensor_type{VerticalHeightSensor::BARO}; ///< selects the primary source for height data
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
|
||||
|
|
@ -501,7 +508,7 @@ union filter_control_status_u {
|
|||
};
|
||||
|
||||
// Mavlink bitmask containing state of estimator solution
|
||||
union ekf_solution_status {
|
||||
union ekf_solution_status_u {
|
||||
struct {
|
||||
uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good
|
||||
uint16_t velocity_horiz : 1; ///< 1 - True if the horizontal velocity estimate is good
|
||||
|
|
|
|||
|
|
@ -215,8 +215,8 @@ void Ekf::controlExternalVisionFusion()
|
|||
|
||||
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
|
||||
// needs to be calculated and the observations rotated into the EKF frame of reference
|
||||
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS)
|
||||
|| (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
|
||||
if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
|
||||
|| (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) {
|
||||
|
||||
// rotate EV measurements into the EKF Navigation frame
|
||||
calcExtVisRotMat();
|
||||
|
|
@ -228,19 +228,19 @@ void Ekf::controlExternalVisionFusion()
|
|||
// check for a external vision measurement that has fallen behind the fusion time horizon
|
||||
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
|
||||
// turn on use of external vision measurements for position
|
||||
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
|
||||
startEvPosFusion();
|
||||
}
|
||||
|
||||
// turn on use of external vision measurements for velocity
|
||||
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) {
|
||||
startEvVelFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// external vision yaw aiding selection logic
|
||||
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
|
||||
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
|
||||
&& _control_status.flags.tilt_align) {
|
||||
|
||||
// don't start using EV data unless data is arriving frequently
|
||||
|
|
@ -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 & MASK_USE_GPS) {
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
|
||||
_fuse_hpos_as_odom = true;
|
||||
|
||||
} else {
|
||||
|
|
@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion()
|
|||
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
|
||||
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
|
||||
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
|
||||
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
|
||||
}
|
||||
|
|
@ -471,7 +471,7 @@ void Ekf::controlOpticalFlowFusion()
|
|||
|
||||
// Handle cases where we are using optical flow but we should not use it anymore
|
||||
if (_control_status.flags.opt_flow) {
|
||||
if (!(_params.fusion_mode & MASK_USE_OF)
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
|
||||
|| _inhibit_flow_use) {
|
||||
|
||||
stopFlowFusion();
|
||||
|
|
@ -480,11 +480,11 @@ void Ekf::controlOpticalFlowFusion()
|
|||
}
|
||||
|
||||
// optical flow fusion mode selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
|
||||
if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
|
||||
&& !_control_status.flags.opt_flow // we are not yet using flow data
|
||||
&& !_inhibit_flow_use) {
|
||||
// If the heading is valid and use is not inhibited , start using optical flow aiding
|
||||
if (_control_status.flags.yaw_align || _params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
|
||||
if (_control_status.flags.yaw_align || _params.mag_fusion_type == MagFuseType::NONE) {
|
||||
// set the flag and reset the fusion timeout
|
||||
ECL_INFO("starting optical flow fusion");
|
||||
_control_status.flags.opt_flow = true;
|
||||
|
|
@ -555,7 +555,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
|||
|
||||
void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
{
|
||||
if (!(_params.fusion_mode & MASK_USE_GPSYAW)
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_YAW)
|
||||
|| _control_status.flags.gps_yaw_fault) {
|
||||
|
||||
stopGpsYawFusion();
|
||||
|
|
@ -842,7 +842,7 @@ void Ekf::controlHeightFusion()
|
|||
ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type);
|
||||
|
||||
// FALLTHROUGH
|
||||
case VDIST_SENSOR_BARO:
|
||||
case VerticalHeightSensor::BARO:
|
||||
if (do_range_aid) {
|
||||
if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
startRngAidHgtFusion();
|
||||
|
|
@ -862,7 +862,7 @@ void Ekf::controlHeightFusion()
|
|||
|
||||
break;
|
||||
|
||||
case VDIST_SENSOR_RANGE:
|
||||
case VerticalHeightSensor::RANGE:
|
||||
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
|
|
@ -883,7 +883,7 @@ void Ekf::controlHeightFusion()
|
|||
|
||||
break;
|
||||
|
||||
case VDIST_SENSOR_GPS:
|
||||
case VerticalHeightSensor::GPS:
|
||||
|
||||
// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
|
||||
// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
|
||||
|
|
@ -909,7 +909,7 @@ void Ekf::controlHeightFusion()
|
|||
|
||||
break;
|
||||
|
||||
case VDIST_SENSOR_EV:
|
||||
case VerticalHeightSensor::EV:
|
||||
|
||||
// don't start using EV data unless data is arriving frequently
|
||||
if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
|
||||
|
|
@ -986,7 +986,7 @@ void Ekf::controlAirDataFusion()
|
|||
const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
|
||||
const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
|
||||
|
||||
if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) {
|
||||
if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
|
||||
_control_status.flags.wind = false;
|
||||
}
|
||||
|
||||
|
|
@ -1053,7 +1053,7 @@ void Ekf::controlBetaFusion()
|
|||
|
||||
void Ekf::controlDragFusion()
|
||||
{
|
||||
if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer &&
|
||||
if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
|
||||
!_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
|
||||
|
||||
if (!_control_status.flags.wind) {
|
||||
|
|
|
|||
|
|
@ -142,7 +142,7 @@ void Ekf::predictCovariance()
|
|||
const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
|
||||
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
|
||||
|
||||
const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)
|
||||
const bool do_inhibit_all_axes = (_params.fusion_mode & SensorFusionMask::INHIBIT_ACC_BIAS)
|
||||
|| is_manoeuvre_level_high
|
||||
|| _fault_status.flags.bad_acc_vertical;
|
||||
|
||||
|
|
|
|||
|
|
@ -174,7 +174,7 @@ bool Ekf::initialiseFilter()
|
|||
}
|
||||
}
|
||||
|
||||
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
|
||||
if (_mag_counter < _obs_buffer_length) {
|
||||
// not enough mag samples accumulated
|
||||
return false;
|
||||
|
|
@ -202,7 +202,7 @@ bool Ekf::initialiseFilter()
|
|||
initialiseCovariance();
|
||||
|
||||
// update the yaw angle variance using the variance of the measurement
|
||||
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
|
||||
// using magnetic heading tuning parameter
|
||||
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -160,7 +160,7 @@ public:
|
|||
matrix::SquareMatrix<float, 3> position_covariances() const { return P.slice<3, 3>(7, 7); }
|
||||
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
bool collect_gps(const gps_message &gps) override;
|
||||
bool collect_gps(const gpsMessage &gps) override;
|
||||
|
||||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
// return true if the origin is valid
|
||||
|
|
@ -814,7 +814,7 @@ private:
|
|||
Vector3f calcEarthRateNED(float lat_rad) const;
|
||||
|
||||
// return true id the GPS quality is good enough to set an origin and start aiding
|
||||
bool gps_is_good(const gps_message &gps);
|
||||
bool gps_is_good(const gpsMessage &gps);
|
||||
|
||||
// Control the filter fusion modes
|
||||
void controlFusionModes();
|
||||
|
|
|
|||
|
|
@ -158,7 +158,7 @@ void Ekf::resetHorizontalPositionToVision()
|
|||
ECL_INFO("reset position to ev position");
|
||||
Vector3f _ev_pos = _ev_sample_delayed.pos;
|
||||
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
|
||||
}
|
||||
|
||||
|
|
@ -467,7 +467,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
|
|||
|
||||
const bool heading_required_for_navigation = _control_status.flags.gps || _control_status.flags.ev_pos;
|
||||
|
||||
if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) {
|
||||
if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
|
||||
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
|
||||
|
|
@ -480,7 +480,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
|
|||
yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
|
||||
}
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) {
|
||||
} else if (_params.mag_fusion_type == MagFuseType::INDOOR) {
|
||||
// we are operating temporarily without knowing the earth frame yaw angle
|
||||
return true;
|
||||
|
||||
|
|
@ -522,7 +522,7 @@ float Ekf::getMagDeclination()
|
|||
// Use value consistent with earth field state
|
||||
return atan2f(_state.mag_I(1), _state.mag_I(0));
|
||||
|
||||
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
|
||||
} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
|
||||
// use parameter value until GPS is available, then use value returned by geo library
|
||||
if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
|
||||
return _mag_declination_gps;
|
||||
|
|
@ -953,7 +953,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
// return a bitmask integer that describes which state estimates are valid
|
||||
void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
{
|
||||
ekf_solution_status soln_status;
|
||||
ekf_solution_status_u 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 = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
|
|
@ -1398,14 +1398,14 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
|
|||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch (_ev_sample_delayed.vel_frame) {
|
||||
case velocity_frame_t::BODY_FRAME_FRD:
|
||||
case VelocityFrame::BODY_FRAME_FRD:
|
||||
vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
|
||||
break;
|
||||
|
||||
case velocity_frame_t::LOCAL_FRAME_FRD:
|
||||
case VelocityFrame::LOCAL_FRAME_FRD:
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
|
||||
|
||||
} else {
|
||||
|
|
@ -1424,12 +1424,12 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
|
|||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch (_ev_sample_delayed.vel_frame) {
|
||||
case velocity_frame_t::BODY_FRAME_FRD:
|
||||
case VelocityFrame::BODY_FRAME_FRD:
|
||||
ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
|
||||
break;
|
||||
|
||||
case velocity_frame_t::LOCAL_FRAME_FRD:
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
case VelocityFrame::LOCAL_FRAME_FRD:
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -129,7 +129,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
|
|||
}
|
||||
}
|
||||
|
||||
void EstimatorInterface::setGpsData(const gps_message &gps)
|
||||
void EstimatorInterface::setGpsData(const gpsMessage &gps)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
|
|
@ -382,7 +382,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
|
|||
{
|
||||
// down-sample the drag specific force data by accumulating and calculating the mean when
|
||||
// sufficient samples have been collected
|
||||
if ((_params.fusion_mode & MASK_USE_DRAG)) {
|
||||
if ((_params.fusion_mode & SensorFusionMask::USE_DRAG)) {
|
||||
|
||||
// Allocate the required buffer size if not previously done
|
||||
if (_drag_buffer == nullptr) {
|
||||
|
|
@ -447,24 +447,24 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
}
|
||||
|
||||
// mag mode
|
||||
if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
|
||||
if (_params.mag_fusion_type != MagFuseType::NONE) {
|
||||
max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
// range aid or range height
|
||||
if (_params.range_aid || (_params.vdist_sensor_type == VDIST_SENSOR_RANGE)) {
|
||||
if (_params.range_aid || (_params.vdist_sensor_type == VerticalHeightSensor::RANGE)) {
|
||||
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
if (_params.fusion_mode & MASK_USE_GPS) {
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
|
||||
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
if (_params.fusion_mode & MASK_USE_OF) {
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
|
||||
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
if (_params.fusion_mode & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
|
||||
if (_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW | SensorFusionMask::USE_EXT_VIS_VEL)) {
|
||||
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -78,13 +78,13 @@ class EstimatorInterface
|
|||
{
|
||||
public:
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
virtual bool collect_gps(const gps_message &gps) = 0;
|
||||
virtual bool collect_gps(const gpsMessage &gps) = 0;
|
||||
|
||||
void setIMUData(const imuSample &imu_sample);
|
||||
|
||||
void setMagData(const magSample &mag_sample);
|
||||
|
||||
void setGpsData(const gps_message &gps);
|
||||
void setGpsData(const gpsMessage &gps);
|
||||
|
||||
void setBaroData(const baroSample &baro_sample);
|
||||
|
||||
|
|
@ -206,7 +206,7 @@ public:
|
|||
// At the next startup, set param.mag_declination_deg to the value saved
|
||||
bool get_mag_decl_deg(float *val) const
|
||||
{
|
||||
if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
|
||||
if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
|
||||
*val = math::degrees(_mag_declination_gps);
|
||||
return true;
|
||||
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@
|
|||
#define MASK_GPS_HSPD (1<<7)
|
||||
#define MASK_GPS_VSPD (1<<8)
|
||||
|
||||
bool Ekf::collect_gps(const gps_message &gps)
|
||||
bool Ekf::collect_gps(const gpsMessage &gps)
|
||||
{
|
||||
// Run GPS checks always
|
||||
_gps_checks_passed = gps_is_good(gps);
|
||||
|
|
@ -92,7 +92,7 @@ bool Ekf::collect_gps(const gps_message &gps)
|
|||
_mag_strength_gps = get_mag_strength_gauss(lat, lon);
|
||||
|
||||
// request a reset of the yaw using the new declination
|
||||
if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE)
|
||||
if ((_params.mag_fusion_type != MagFuseType::NONE)
|
||||
&& !declination_was_valid) {
|
||||
_mag_yaw_reset_req = true;
|
||||
}
|
||||
|
|
@ -120,7 +120,7 @@ bool Ekf::collect_gps(const gps_message &gps)
|
|||
_mag_strength_gps = get_mag_strength_gauss(lat, lon);
|
||||
|
||||
// request mag yaw reset if there's a mag declination for the first time
|
||||
if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
|
||||
if (_params.mag_fusion_type != MagFuseType::NONE) {
|
||||
if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) {
|
||||
_mag_yaw_reset_req = true;
|
||||
}
|
||||
|
|
@ -141,7 +141,7 @@ bool Ekf::collect_gps(const gps_message &gps)
|
|||
* Checks are activated using the EKF2_GPS_CHECK bitmask parameter
|
||||
* Checks are adjusted using the EKF2_REQ_* parameters
|
||||
*/
|
||||
bool Ekf::gps_is_good(const gps_message &gps)
|
||||
bool Ekf::gps_is_good(const gpsMessage &gps)
|
||||
{
|
||||
// Check the fix type
|
||||
_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@
|
|||
|
||||
void Ekf::controlGpsFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & MASK_USE_GPS)) {
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS)) {
|
||||
stopGpsFusion();
|
||||
return;
|
||||
}
|
||||
|
|
@ -109,7 +109,7 @@ void Ekf::controlGpsFusion()
|
|||
|
||||
// TODO: move this to EV control logic
|
||||
// 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)) {
|
||||
if (_control_status.flags.ev_pos && !(_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS)) {
|
||||
resetHorizontalPositionToVision();
|
||||
}
|
||||
}
|
||||
|
|
@ -136,7 +136,7 @@ void Ekf::controlGpsFusion()
|
|||
startGpsFusion();
|
||||
}
|
||||
|
||||
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
|
||||
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
|
||||
// If no mag is used, align using the yaw estimator (if available)
|
||||
if (resetYawToEKFGSF()) {
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ void Ekf::controlMagFusion()
|
|||
|
||||
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
|
||||
// this is useful if there is a lot of interference on the sensor measurement.
|
||||
if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL)
|
||||
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
|
||||
) {
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
|
||||
|
|
@ -82,7 +82,7 @@ void Ekf::controlMagFusion()
|
|||
// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
|
||||
// ground and to prevent uncontrolled yaw variance growth
|
||||
// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
|
||||
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE
|
||||
if (_params.mag_fusion_type >= MagFuseType::NONE
|
||||
|| _control_status.flags.mag_fault
|
||||
|| !_control_status.flags.tilt_align) {
|
||||
|
||||
|
|
@ -111,18 +111,18 @@ void Ekf::controlMagFusion()
|
|||
default:
|
||||
|
||||
/* fallthrough */
|
||||
case MAG_FUSE_TYPE_AUTO:
|
||||
case MagFuseType::AUTO:
|
||||
selectMagAuto();
|
||||
break;
|
||||
|
||||
case MAG_FUSE_TYPE_INDOOR:
|
||||
case MagFuseType::INDOOR:
|
||||
|
||||
/* fallthrough */
|
||||
case MAG_FUSE_TYPE_HEADING:
|
||||
case MagFuseType::HEADING:
|
||||
startMagHdgFusion();
|
||||
break;
|
||||
|
||||
case MAG_FUSE_TYPE_3D:
|
||||
case MagFuseType::MAG_3D:
|
||||
startMag3DFusion();
|
||||
break;
|
||||
}
|
||||
|
|
@ -188,7 +188,7 @@ void Ekf::runOnGroundYawReset()
|
|||
|
||||
bool Ekf::canResetMagHeading() const
|
||||
{
|
||||
return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
|
||||
return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MagFuseType::NONE);
|
||||
}
|
||||
|
||||
void Ekf::runInAirYawReset(const Vector3f &mag_sample)
|
||||
|
|
@ -280,7 +280,7 @@ void Ekf::checkMagDeclRequired()
|
|||
// then the declination must be fused as an observation to prevent long term heading drift
|
||||
// fusing declination when gps aiding is available is optional, but recommended to prevent
|
||||
// problem if the vehicle is static for extended periods of time
|
||||
const bool user_selected = (_params.mag_declination_source & MASK_FUSE_DECL);
|
||||
const bool user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
|
||||
const bool not_using_ne_aiding = !_control_status.flags.gps;
|
||||
_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
|
||||
}
|
||||
|
|
@ -306,7 +306,7 @@ bool Ekf::shouldInhibitMag() const
|
|||
// is available, assume that we are operating indoors and the magnetometer should not be used.
|
||||
// Also inhibit mag fusion when a strong magnetic field interference is detected or the user
|
||||
// has explicitly stopped magnetometer use.
|
||||
const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
|
||||
const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
|
||||
|
||||
const bool heading_not_required_for_navigation = !_control_status.flags.gps
|
||||
&& !_control_status.flags.ev_pos
|
||||
|
|
@ -319,7 +319,7 @@ bool Ekf::shouldInhibitMag() const
|
|||
void Ekf::checkMagFieldStrength(const Vector3f &mag_sample)
|
||||
{
|
||||
if (_params.check_mag_strength
|
||||
&& ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
|
||||
&& ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
|
||||
|
||||
if (PX4_ISFINITE(_mag_strength_gps)) {
|
||||
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
|
||||
|
|
|
|||
|
|
@ -852,7 +852,7 @@ void Ekf::limitDeclination()
|
|||
float decl_reference;
|
||||
float h_field_min = 0.001f;
|
||||
|
||||
if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
|
||||
if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
|
||||
// use parameter value until GPS is available, then use value returned by geo library
|
||||
if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
|
||||
decl_reference = _mag_declination_gps;
|
||||
|
|
|
|||
|
|
@ -308,7 +308,7 @@ void EKF2::Run()
|
|||
}
|
||||
|
||||
// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
|
||||
if (_params->mag_fusion_type != MAG_FUSE_TYPE_NONE) {
|
||||
if (_params->mag_fusion_type != MagFuseType::NONE) {
|
||||
float sens_mag_rate = 0.f;
|
||||
|
||||
if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) {
|
||||
|
|
@ -1153,7 +1153,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
|
|||
_last_gyro_bias_published = gyro_bias;
|
||||
}
|
||||
|
||||
if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
|
||||
if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS)) {
|
||||
bias.accel_device_id = _device_id_accel;
|
||||
accel_bias.copyTo(bias.accel_bias);
|
||||
bias.accel_bias_limit = _params->acc_bias_lim;
|
||||
|
|
@ -1596,10 +1596,10 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
|
|||
ev_data.vel(2) = ev_odom.vz;
|
||||
|
||||
if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
|
||||
ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
|
||||
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
||||
|
||||
} else {
|
||||
ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
|
||||
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
||||
}
|
||||
|
||||
// velocity measurement error from ev_data or parameters
|
||||
|
|
@ -1735,7 +1735,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
perf_count(_msg_missed_gps_perf);
|
||||
}
|
||||
|
||||
gps_message gps_msg{
|
||||
gpsMessage gps_msg{
|
||||
.time_usec = vehicle_gps_position.timestamp,
|
||||
.lat = vehicle_gps_position.lat,
|
||||
.lon = vehicle_gps_position.lon,
|
||||
|
|
@ -1878,7 +1878,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
|
||||
void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
|
||||
if (_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS) {
|
||||
_accel_cal.cal_available = false;
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ EkfWrapper::~EkfWrapper()
|
|||
|
||||
void EkfWrapper::setBaroHeight()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VDIST_SENSOR_BARO;
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingBaroHeightFusion() const
|
||||
|
|
@ -22,7 +22,7 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
|
|||
|
||||
void EkfWrapper::setGpsHeight()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VDIST_SENSOR_GPS;
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsHeightFusion() const
|
||||
|
|
@ -32,7 +32,7 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
|
|||
|
||||
void EkfWrapper::setRangeHeight()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VDIST_SENSOR_RANGE;
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingRangeHeightFusion() const
|
||||
|
|
@ -42,7 +42,7 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
|
|||
|
||||
void EkfWrapper::setVisionHeight()
|
||||
{
|
||||
_ekf_params->vdist_sensor_type = VDIST_SENSOR_EV;
|
||||
_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingVisionHeightFusion() const
|
||||
|
|
@ -52,12 +52,12 @@ bool EkfWrapper::isIntendingVisionHeightFusion() const
|
|||
|
||||
void EkfWrapper::enableGpsFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_USE_GPS;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableGpsFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_USE_GPS;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsFusion() const
|
||||
|
|
@ -67,12 +67,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
|
|||
|
||||
void EkfWrapper::enableGpsHeadingFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_USE_GPSYAW;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_YAW;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableGpsHeadingFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_USE_GPSYAW;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_YAW;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingGpsHeadingFusion() const
|
||||
|
|
@ -82,12 +82,12 @@ bool EkfWrapper::isIntendingGpsHeadingFusion() const
|
|||
|
||||
void EkfWrapper::enableFlowFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_USE_OF;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_OPT_FLOW;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableFlowFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_USE_OF;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_OPT_FLOW;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingFlowFusion() const
|
||||
|
|
@ -102,12 +102,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
|
|||
|
||||
void EkfWrapper::enableExternalVisionPositionFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_USE_EVPOS;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableExternalVisionPositionFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_USE_EVPOS;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
|
||||
|
|
@ -117,12 +117,12 @@ bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
|
|||
|
||||
void EkfWrapper::enableExternalVisionVelocityFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_USE_EVVEL;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_VEL;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableExternalVisionVelocityFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_USE_EVVEL;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_VEL;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
|
||||
|
|
@ -132,12 +132,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
|
|||
|
||||
void EkfWrapper::enableExternalVisionHeadingFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_USE_EVYAW;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableExternalVisionHeadingFusion()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_USE_EVYAW;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
|
||||
|
|
@ -147,12 +147,12 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
|
|||
|
||||
void EkfWrapper::enableExternalVisionAlignment()
|
||||
{
|
||||
_ekf_params->fusion_mode |= MASK_ROTATE_EV;
|
||||
_ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS;
|
||||
}
|
||||
|
||||
void EkfWrapper::disableExternalVisionAlignment()
|
||||
{
|
||||
_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
|
||||
_ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isIntendingMagHeadingFusion() const
|
||||
|
|
@ -167,7 +167,7 @@ bool EkfWrapper::isIntendingMag3DFusion() const
|
|||
|
||||
void EkfWrapper::setMagFuseTypeNone()
|
||||
{
|
||||
_ekf_params->mag_fusion_type = MAG_FUSE_TYPE_NONE;
|
||||
_ekf_params->mag_fusion_type = MagFuseType::NONE;
|
||||
}
|
||||
|
||||
bool EkfWrapper::isWindVelocityEstimated() const
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ void Gps::send(const uint64_t time)
|
|||
_ekf->setGpsData(_gps_data);
|
||||
}
|
||||
|
||||
void Gps::setData(const gps_message &gps)
|
||||
void Gps::setData(const gpsMessage &gps)
|
||||
{
|
||||
_gps_data = gps;
|
||||
}
|
||||
|
|
@ -108,9 +108,9 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
|
|||
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
|
||||
}
|
||||
|
||||
gps_message Gps::getDefaultGpsData()
|
||||
gpsMessage Gps::getDefaultGpsData()
|
||||
{
|
||||
gps_message gps_data{};
|
||||
gpsMessage gps_data{};
|
||||
gps_data.time_usec = 0;
|
||||
gps_data.lat = 473566094;
|
||||
gps_data.lon = 85190237;
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ public:
|
|||
Gps(std::shared_ptr<Ekf> ekf);
|
||||
~Gps();
|
||||
|
||||
void setData(const gps_message &gps);
|
||||
void setData(const gpsMessage &gps);
|
||||
void stepHeightByMeters(const float hgt_change);
|
||||
void stepHorizontalPositionByMeters(const Vector2f hpos_change);
|
||||
void setPositionRateNED(const Vector3f &rate);
|
||||
|
|
@ -65,12 +65,12 @@ public:
|
|||
void setNumberOfSatellites(const int num_satellites);
|
||||
void setPdop(const float pdop);
|
||||
|
||||
gps_message getDefaultGpsData();
|
||||
gpsMessage getDefaultGpsData();
|
||||
|
||||
private:
|
||||
void send(uint64_t time) override;
|
||||
|
||||
gps_message _gps_data{};
|
||||
gpsMessage _gps_data{};
|
||||
Vector3f _gps_pos_rate{};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -61,12 +61,12 @@ void Vio::setOrientation(const Quatf &quat)
|
|||
|
||||
void Vio::setVelocityFrameToBody()
|
||||
{
|
||||
_vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
|
||||
_vio_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
||||
}
|
||||
|
||||
void Vio::setVelocityFrameToLocal()
|
||||
{
|
||||
_vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
|
||||
_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
||||
}
|
||||
|
||||
extVisionSample Vio::dataAtRest()
|
||||
|
|
@ -78,7 +78,7 @@ extVisionSample Vio::dataAtRest()
|
|||
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
|
||||
vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
|
||||
vio_data.angVar = 0.05f;
|
||||
vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
|
||||
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
||||
return vio_data;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue