Compare commits

...

5 Commits

Author SHA1 Message Date
Daniel Agar 423c43ad8e boards: ARK CAN Flow enable ekf2 2023-10-17 16:23:35 -04:00
Daniel Agar 63c4465037 boards: ARK CAN RTK GPS enable ekf2 2023-10-17 16:21:52 -04:00
Daniel Agar 4eaa932389 ekf2: fix barometer kconfig 2023-10-17 16:21:48 -04:00
Daniel Agar fe3314e33e ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS 2023-10-17 14:09:47 -04:00
Daniel Agar 755b45441f ekf2: add kconfig to disable gravity fusion 2023-10-17 14:09:37 -04:00
21 changed files with 200 additions and 203 deletions

View File

@ -67,6 +67,9 @@ then
fi
unset BOARD_RC_DEFAULTS
# start uavcannode as early as possible to pet the watchdog
uavcannode start
#
# Start system state indicator.
#
@ -114,5 +117,6 @@ param set-default SENS_MAG_RATE 100
sensors start
uavcannode start
ekf2 start
unset R

View File

@ -6,27 +6,25 @@ CONFIG_BOARD_NO_HELP=y
CONFIG_BOARD_CONSTRAINED_MEMORY=y
CONFIG_DRIVERS_BOOTLOADERS=y
CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y
CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
CONFIG_BOARD_UAVCAN_INTERFACES=1
CONFIG_DRIVERS_UAVCANNODE=y
CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BAROMETER is not set
# CONFIG_EKF2_EXTERNAL_VISION is not set
# CONFIG_EKF2_GNSS is not set
# CONFIG_EKF2_GRAVITY_FUSION is not set
# CONFIG_EKF2_MAGNETOMETER is not set
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
# CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY is not set
# CONFIG_SENSORS_VEHICLE_ACCELERATION is not set
# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_REBOOT=y
CONFIG_SYSTEMCMDS_TOP=y
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y

View File

@ -27,7 +27,6 @@
# CONFIG_NSH_DISABLE_LOOPS is not set
# CONFIG_NSH_DISABLE_LS is not set
# CONFIG_NSH_DISABLE_MKDIR is not set
# CONFIG_NSH_DISABLE_MKFATFS is not set
# CONFIG_NSH_DISABLE_MOUNT is not set
# CONFIG_NSH_DISABLE_MV is not set
# CONFIG_NSH_DISABLE_PS is not set
@ -66,17 +65,8 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y
CONFIG_DEBUG_SYMBOLS=y
CONFIG_DEBUG_TCBINFO=y
CONFIG_DEFAULT_SMALL=y
CONFIG_DEV_FIFO_SIZE=0
CONFIG_DEV_PIPE_MAXSIZE=1024
CONFIG_DEV_PIPE_SIZE=70
CONFIG_FDCLONE_STDIO=y
CONFIG_FS_BINFS=y
CONFIG_FS_CROMFS=y
CONFIG_FS_FAT=y
CONFIG_FS_FATTIME=y
CONFIG_FS_PROCFS=y
CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
CONFIG_FS_PROCFS_REGISTER=y
CONFIG_FS_ROMFS=y
CONFIG_GRAN=y
CONFIG_GRAN_INTR=y
@ -88,14 +78,9 @@ CONFIG_INIT_STACKSIZE=2624
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_LIBC_LONG_LONG=y
CONFIG_LIBC_MAX_EXITFUNS=1
CONFIG_LIBC_STRERROR=y
CONFIG_MEMSET_64BIT=y
CONFIG_MEMSET_OPTSPEED=y
CONFIG_MM_REGIONS=2
CONFIG_MTD=y
CONFIG_MTD_BYTE_WRITE=y
CONFIG_MTD_PARTITION=y
CONFIG_MTD_RAMTRON=y
CONFIG_NAME_MAX=40
CONFIG_NSH_ARCHINIT=y
CONFIG_NSH_ARGCAT=y
@ -108,31 +93,20 @@ CONFIG_NSH_NESTDEPTH=8
CONFIG_NSH_QUOTE=y
CONFIG_NSH_ROMFSETC=y
CONFIG_NSH_ROMFSSECTSIZE=128
CONFIG_NSH_STRERROR=y
CONFIG_NSH_VARS=y
CONFIG_PIPES=y
CONFIG_PREALLOC_TIMERS=50
CONFIG_PRIORITY_INHERITANCE=y
CONFIG_PTHREAD_MUTEX_ROBUST=y
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
CONFIG_RAMTRON_SETSPEED=y
CONFIG_RAM_SIZE=262144
CONFIG_RAM_START=0x20000000
CONFIG_RAW_BINARY=y
CONFIG_RTC_DATETIME=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_HPWORKPRIORITY=254
CONFIG_SCHED_HPWORKSTACKSIZE=3000
CONFIG_SCHED_INSTRUMENTATION=y
CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKSTACKSIZE=1632
CONFIG_SCHED_WAITPID=y
CONFIG_SEM_PREALLOCHOLDERS=32
CONFIG_SERIAL_TERMIOS=y
CONFIG_SIG_DEFAULT=y
CONFIG_SIG_SIGALRM_ACTION=y
@ -151,11 +125,6 @@ CONFIG_STM32_FLASH_PREFETCH=y
CONFIG_STM32_FLOWCONTROL_BROKEN=y
CONFIG_STM32_JTAG_SW_ENABLE=y
CONFIG_STM32_PWR=y
CONFIG_STM32_RTC=y
CONFIG_STM32_RTC_HSECLOCK=y
CONFIG_STM32_RTC_MAGIC=0xfacefeee
CONFIG_STM32_RTC_MAGIC_REG=1
CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32_SPI1=y

View File

@ -21,15 +21,21 @@ CONFIG_UAVCANNODE_RTK_DATA=y
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_EXTERNAL_VISION is not set
# CONFIG_EKF2_GNSS_YAW is not set
# CONFIG_EKF2_GRAVITY_FUSION is not set
# CONFIG_EKF2_RANGE_FINDER is not set
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
# CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY is not set
# CONFIG_SENSORS_VEHICLE_ACCELERATION is not set
# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_REBOOT=y
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
CONFIG_SYSTEMCMDS_TOP=y
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

View File

@ -27,7 +27,6 @@
# CONFIG_NSH_DISABLE_LOOPS is not set
# CONFIG_NSH_DISABLE_LS is not set
# CONFIG_NSH_DISABLE_MKDIR is not set
# CONFIG_NSH_DISABLE_MKFATFS is not set
# CONFIG_NSH_DISABLE_MOUNT is not set
# CONFIG_NSH_DISABLE_MV is not set
# CONFIG_NSH_DISABLE_PS is not set
@ -66,17 +65,8 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y
CONFIG_DEBUG_SYMBOLS=y
CONFIG_DEBUG_TCBINFO=y
CONFIG_DEFAULT_SMALL=y
CONFIG_DEV_FIFO_SIZE=0
CONFIG_DEV_PIPE_MAXSIZE=1024
CONFIG_DEV_PIPE_SIZE=70
CONFIG_FDCLONE_STDIO=y
CONFIG_FS_BINFS=y
CONFIG_FS_CROMFS=y
CONFIG_FS_FAT=y
CONFIG_FS_FATTIME=y
CONFIG_FS_PROCFS=y
CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
CONFIG_FS_PROCFS_REGISTER=y
CONFIG_FS_ROMFS=y
CONFIG_GRAN=y
CONFIG_GRAN_INTR=y
@ -90,14 +80,9 @@ CONFIG_INIT_STACKSIZE=2624
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_LIBC_LONG_LONG=y
CONFIG_LIBC_MAX_EXITFUNS=1
CONFIG_LIBC_STRERROR=y
CONFIG_MEMSET_64BIT=y
CONFIG_MEMSET_OPTSPEED=y
CONFIG_MM_REGIONS=2
CONFIG_MTD=y
CONFIG_MTD_BYTE_WRITE=y
CONFIG_MTD_PARTITION=y
CONFIG_MTD_RAMTRON=y
CONFIG_NAME_MAX=40
CONFIG_NSH_ARCHINIT=y
CONFIG_NSH_ARGCAT=y
@ -110,37 +95,22 @@ CONFIG_NSH_NESTDEPTH=8
CONFIG_NSH_QUOTE=y
CONFIG_NSH_ROMFSETC=y
CONFIG_NSH_ROMFSSECTSIZE=128
CONFIG_NSH_STRERROR=y
CONFIG_NSH_VARS=y
CONFIG_PIPES=y
CONFIG_PREALLOC_TIMERS=50
CONFIG_PRIORITY_INHERITANCE=y
CONFIG_PTHREAD_MUTEX_ROBUST=y
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
CONFIG_RAMTRON_SETSPEED=y
CONFIG_RAM_SIZE=262144
CONFIG_RAM_START=0x20000000
CONFIG_RAW_BINARY=y
CONFIG_RTC_DATETIME=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_HPWORKPRIORITY=249
CONFIG_SCHED_HPWORKSTACKSIZE=1280
CONFIG_SCHED_INSTRUMENTATION=y
CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKSTACKSIZE=1632
CONFIG_SCHED_WAITPID=y
CONFIG_SEM_PREALLOCHOLDERS=32
CONFIG_SERIAL_TERMIOS=y
CONFIG_SIG_DEFAULT=y
CONFIG_SIG_SIGALRM_ACTION=y
CONFIG_SIG_SIGUSR1_ACTION=y
CONFIG_SIG_SIGUSR2_ACTION=y
CONFIG_SIG_SIGWORK=4
CONFIG_STACK_COLORATION=y
CONFIG_START_DAY=30
CONFIG_START_MONTH=11
@ -155,11 +125,6 @@ CONFIG_STM32_I2C1=y
CONFIG_STM32_I2C2=y
CONFIG_STM32_JTAG_SW_ENABLE=y
CONFIG_STM32_PWR=y
CONFIG_STM32_RTC=y
CONFIG_STM32_RTC_HSECLOCK=y
CONFIG_STM32_RTC_MAGIC=0xfacefeee
CONFIG_STM32_RTC_MAGIC_REG=1
CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32_SPI1=y

View File

@ -162,8 +162,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
#endif // FLASH_BASED_PARAMS
/* Configure the HW based on the manifest */
px4_platform_configure();
//px4_platform_configure();
return OK;
}

View File

@ -1167,8 +1167,9 @@ GPS::print_status()
PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
#ifndef CONSTRAINED_FLASH
print_message(ORB_ID(sensor_gps), _report_gps_pos);
#endif // !CONSTRAINED_FLASH
}
if (_instance == Instance::Main && _secondary_instance.load()) {

View File

@ -124,7 +124,6 @@ list(APPEND EKF_SRCS
EKF/estimator_interface.cpp
EKF/fake_height_control.cpp
EKF/fake_pos_control.cpp
EKF/gravity_fusion.cpp
EKF/height_control.cpp
EKF/imu_down_sampler.cpp
EKF/output_predictor.cpp
@ -175,6 +174,10 @@ if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
endif()
if(CONFIG_EKF2_GRAVITY_FUSION)
list(APPEND EKF_SRCS EKF/gravity_fusion.cpp)
endif()
if(CONFIG_EKF2_MAGNETOMETER)
list(APPEND EKF_SRCS
EKF/mag_3d_control.cpp

View File

@ -38,11 +38,9 @@ list(APPEND EKF_SRCS
covariance.cpp
ekf.cpp
ekf_helper.cpp
EKFGSF_yaw.cpp
estimator_interface.cpp
fake_height_control.cpp
fake_pos_control.cpp
gravity_fusion.cpp
height_control.cpp
imu_down_sampler.cpp
output_predictor.cpp
@ -86,6 +84,7 @@ if(CONFIG_EKF2_GNSS)
gnss_height_control.cpp
gps_checks.cpp
gps_control.cpp
EKFGSF_yaw.cpp
)
endif()
@ -93,6 +92,10 @@ if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
endif()
if(CONFIG_EKF2_GRAVITY_FUSION)
list(APPEND EKF_SRCS gravity_fusion.cpp)
endif()
if(CONFIG_EKF2_MAGNETOMETER)
list(APPEND EKF_SRCS
mag_3d_control.cpp

View File

@ -62,6 +62,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_control_status.flags.wind = false;
}
#if defined(CONFIG_EKF2_GNSS)
// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
if (_control_status.flags.fixed_wing) {
if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
@ -73,6 +74,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_yawEstimator.setTrueAirspeed(0.f);
}
}
#endif // CONFIG_EKF2_GNSS
if (_params.arsp_thr <= 0.f) {
stopAirspeedFusion();
@ -99,7 +101,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
fuseAirspeed(airspeed_sample, _aid_src_airspeed);
}
#if defined(CONFIG_EKF2_GNSS)
_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
#endif // CONFIG_EKF2_GNSS
const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
@ -219,8 +223,11 @@ void Ekf::stopAirspeedFusion()
if (_control_status.flags.fuse_aspd) {
ECL_INFO("stopping airspeed fusion");
resetEstimatorAidStatus(_aid_src_airspeed);
_yawEstimator.setTrueAirspeed(NAN);
_control_status.flags.fuse_aspd = false;
#if defined(CONFIG_EKF2_GNSS)
_yawEstimator.setTrueAirspeed(NAN);
#endif // CONFIG_EKF2_GNSS
}
}

View File

@ -454,8 +454,10 @@ struct parameters {
Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
// gravity fusion
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
int32_t flow_ctrl{0};

View File

@ -128,7 +128,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_DRAG_FUSION
controlHeightFusion(imu_delayed);
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
controlGravityFusion(imu_delayed);
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// Additional data odometry data from an external estimator can be fused.

View File

@ -65,7 +65,12 @@ void Ekf::initialiseCovariance()
P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
// position
#if defined(CONFIG_EKF2_BAROMETER)
float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
#else
float z_pos_var = sq(1.f);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));

View File

@ -45,7 +45,10 @@
#include "estimator_interface.h"
#include "EKFGSF_yaw.h"
#if defined(CONFIG_EKF2_GNSS)
# include "EKFGSF_yaw.h"
#endif // CONFIG_EKF2_GNSS
#include "bias_estimator.hpp"
#include "height_bias_estimator.hpp"
#include "position_bias_estimator.hpp"
@ -310,9 +313,13 @@ public:
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
const auto &aid_src_gravity() const { return _aid_src_gravity; }
void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
#endif // CONFIG_EKF2_GRAVITY_FUSION
// get the state vector at the delayed time horizon
const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
@ -488,9 +495,6 @@ public:
Vector3f calcRotVecVariances() const;
float getYawVar() const;
// Returns true if the output of the yaw emergency estimator can be used for a reset
bool isYawEmergencyEstimateAvailable() const;
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
@ -533,11 +537,6 @@ public:
bool gps_checks_passed() const { return _gps_checks_passed; };
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
@ -547,6 +546,15 @@ public:
# if defined(CONFIG_EKF2_GNSS_YAW)
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
# endif // CONFIG_EKF2_GNSS_YAW
// Returns true if the output of the yaw emergency estimator can be used for a reset
bool isYawEmergencyEstimateAvailable() const;
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
@ -554,8 +562,6 @@ public:
const auto &aid_src_mag() const { return _aid_src_mag; }
#endif // CONFIG_EKF2_MAGNETOMETER
const auto &aid_src_gravity() const { return _aid_src_gravity; }
#if defined(CONFIG_EKF2_AUXVEL)
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
#endif // CONFIG_EKF2_AUXVEL
@ -731,7 +737,9 @@ private:
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
estimator_aid_source3d_s _aid_src_gravity{};
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_AUXVEL)
estimator_aid_source2d_s _aid_src_aux_vel{};
@ -1086,7 +1094,6 @@ private:
void stopGpsFusion();
bool shouldResetGpsFusion() const;
bool isYawFailure() const;
// return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(const gpsMessage &gps);
@ -1094,11 +1101,6 @@ private:
void controlGnssHeightFusion(const gpsSample &gps_sample);
void stopGpsHgtFusion();
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool resetYawToEKFGSF();
void resetGpsDriftCheckFilters();
# if defined(CONFIG_EKF2_GNSS_YAW)
@ -1115,6 +1117,17 @@ private:
void updateGpsYaw(const gpsSample &gps_sample);
# endif // CONFIG_EKF2_GNSS_YAW
// Declarations used to control use of the EKF-GSF yaw estimator
bool isYawFailure() const;
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Returns true if the reset was successful
bool resetYawToEKFGSF();
// yaw estimator instance
EKFGSF_yaw _yawEstimator{};
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
@ -1184,8 +1197,10 @@ private:
void updateGroundEffect();
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
// gravity fusion: heuristically enable / disable gravity fusion
void controlGravityFusion(const imuSample &imu_delayed);
#endif // CONFIG_EKF2_GRAVITY_FUSION
void resetQuatCov(const float yaw_noise = NAN);
void resetQuatCov(const Vector3f &euler_noise_ned);
@ -1231,11 +1246,6 @@ private:
// yaw_variance : yaw error variance (rad^2)
void resetQuatStateYaw(float yaw, float yaw_variance);
// Declarations used to control use of the EKF-GSF yaw estimator
// yaw estimator instance
EKFGSF_yaw _yawEstimator{};
uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};

View File

@ -1028,56 +1028,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_time_last_heading_fuse = _time_delayed_us;
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
// don't allow reset if there's just been a yaw reset
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
if (yaw_alignment_changed || quat_reset) {
return false;
}
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;
return true;
}
#endif // CONFIG_EKF2_GNSS
bool Ekf::isYawEmergencyEstimateAvailable() const
{
#if defined(CONFIG_EKF2_GNSS)
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
return false;
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
#else
return false;
#endif
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()
{

View File

@ -563,10 +563,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms);
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER)
// using baro
if (_params.baro_ctrl > 0) {
max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
// using airspeed

View File

@ -302,18 +302,6 @@ bool Ekf::shouldResetGpsFusion() const
return (is_reset_required || is_inflight_nav_failure);
}
bool Ekf::isYawFailure() const
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
const float euler_yaw = getEulerYaw(_R_to_earth);
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
return fabsf(yaw_error) > math::radians(25.f);
}
#if defined(CONFIG_EKF2_GNSS_YAW)
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{
@ -436,3 +424,57 @@ void Ekf::stopGpsFusion()
stopGpsYawFusion();
#endif // CONFIG_EKF2_GNSS_YAW
}
bool Ekf::isYawEmergencyEstimateAvailable() const
{
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
return false;
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
}
bool Ekf::isYawFailure() const
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
const float euler_yaw = getEulerYaw(_R_to_earth);
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
return fabsf(yaw_error) > math::radians(25.f);
}
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
return false;
}
// don't allow reset if there's just been a yaw reset
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
if (yaw_alignment_changed || quat_reset) {
return false;
}
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;
return true;
}
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}

View File

@ -155,7 +155,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
* Vector3f(_mag_strength_gps, 0, 0);
// mag_B: reset
#if defined(CONFIG_EKF2_GNSS)
if (isYawEmergencyEstimateAvailable()) {
const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
const Dcmf R_to_body = R_to_earth.transpose();
@ -163,8 +165,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
_state.mag_B = mag - (R_to_body * mag_earth_pred);
ECL_INFO("resetMagStates using yaw estimator");
} else if (!reset_heading && _control_status.flags.yaw_align) {
} else
#endif // CONFIG_EKF2_GNSS
if (!reset_heading && _control_status.flags.yaw_align) {
// mag_B: reset using WMM
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = mag - (R_to_body * mag_earth_pred);

View File

@ -189,6 +189,16 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_DRAG_FUSION)
_param_ekf2_drag_ctrl(_params->drag_ctrl),
_param_ekf2_drag_noise(_params->drag_noise),
_param_ekf2_bcoef_x(_params->bcoef_x),
_param_ekf2_bcoef_y(_params->bcoef_y),
_param_ekf2_mcoef(_params->mcoef),
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
_param_ekf2_grav_noise(_params->gravity_noise),
#endif // CONFIG_EKF2_GRAVITY_FUSION
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
@ -199,16 +209,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
_param_ekf2_gyr_b_lim(_params->gyro_bias_lim),
#if defined(CONFIG_EKF2_DRAG_FUSION)
_param_ekf2_drag_ctrl(_params->drag_ctrl),
_param_ekf2_drag_noise(_params->drag_noise),
_param_ekf2_bcoef_x(_params->bcoef_x),
_param_ekf2_bcoef_y(_params->bcoef_y),
_param_ekf2_mcoef(_params->mcoef),
#endif // CONFIG_EKF2_DRAG_FUSION
_param_ekf2_grav_noise(_params->gravity_noise)
_param_ekf2_gyr_b_lim(_params->gyro_bias_lim)
{
// advertise expected minimal topic set immediately to ensure logging
_attitude_pub.advertise();
@ -229,7 +230,9 @@ EKF2::~EKF2()
perf_free(_ecl_ekf_update_perf);
perf_free(_ecl_ekf_update_full_perf);
perf_free(_msg_missed_imu_perf);
#if defined(CONFIG_EKF2_BAROMETER)
perf_free(_msg_missed_air_data_perf);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
perf_free(_msg_missed_airspeed_perf);
#endif // CONFIG_EKF2_AIRSPEED
@ -398,7 +401,9 @@ int EKF2::print_status()
perf_print_counter(_ecl_ekf_update_perf);
perf_print_counter(_ecl_ekf_update_full_perf);
perf_print_counter(_msg_missed_imu_perf);
#if defined(CONFIG_EKF2_BAROMETER)
perf_print_counter(_msg_missed_air_data_perf);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
perf_print_counter(_msg_missed_airspeed_perf);
#endif // CONFIG_EKF2_AIRSPEED
@ -1082,8 +1087,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
// gravity
PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_AUXVEL)
// aux velocity
@ -1431,7 +1438,9 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_ekf.getBetaInnov(innovations.beta);
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
_ekf.getGravityInnov(innovations.gravity);
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_TERRAIN)
# if defined(CONFIG_EKF2_RANGE_FINDER)
@ -1525,7 +1534,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
_ekf.getBetaInnovRatio(test_ratios.beta);
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_TERRAIN)
# if defined(CONFIG_EKF2_RANGE_FINDER)
@ -1590,7 +1601,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
# endif // CONFIG_EKF2_OPTICAL_FLOW
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
_ekf.getGravityInnovVar(variances.gravity);
#endif // CONFIG_EKF2_GRAVITY_FUSION
// Not yet supported
variances.aux_vvel = NAN;

View File

@ -349,8 +349,6 @@ private:
uORB::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
#endif // CONFIG_EKF2_EXTERNAL_VISION
hrt_abstime _status_gravity_pub_last{0};
#if defined(CONFIG_EKF2_AUXVEL)
uORB::Subscription _landing_target_pose_sub {ORB_ID(landing_target_pose)};
@ -476,8 +474,6 @@ private:
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
@ -511,13 +507,20 @@ private:
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
# if defined(CONFIG_EKF2_GNSS_YAW)
hrt_abstime _status_gnss_yaw_pub_last {0};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
hrt_abstime _status_gravity_pub_last {0};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
#endif // CONFIG_EKF2_GRAVITY_FUSION
PreFlightChecker _preflt_checker;
Ekf _ekf;
@ -722,6 +725,20 @@ private:
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_DRAG_FUSION)
(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl, ///< drag fusion selection
// Multi-rotor drag specific force fusion
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s)
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise,
#endif // CONFIG_EKF2_GRAVITY_FUSION
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
@ -746,21 +763,9 @@ private:
(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s)
#if defined(CONFIG_EKF2_DRAG_FUSION)
(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl, ///< drag fusion selection
// Multi-rotor drag specific force fusion
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s)
#endif // CONFIG_EKF2_DRAG_FUSION
// output predictor filter time constants
(ParamFloat<px4::params::EKF2_TAU_VEL>) _param_ekf2_tau_vel,
(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos,
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise
(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos
)
};
#endif // !EKF2_HPP

View File

@ -75,6 +75,13 @@ depends on MODULES_EKF2
---help---
EKF2 GNSS yaw fusion support.
menuconfig EKF2_GRAVITY_FUSION
depends on MODULES_EKF2
bool "gravity fusion support"
default y
---help---
EKF2 gravity fusion support.
menuconfig EKF2_MAGNETOMETER
depends on MODULES_EKF2
bool "magnetometer support"