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

ekf2: verify parameter configuration for height sources

Browse Source
This commit is contained in:
bresch 2022-08-12 16:17:18 +02:00 committed by Daniel Agar
parent 8dd5d0d1a3
commit a2a29ba0dd
3 changed files with 70 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/common.h
View File

@ -128,14 +128,14 @@ enum RngCtrl : uint8_t {
enum SensorFusionMask : uint16_t {
// Bit locations for fusion_mode
/* USE_GPS = (1<<0), ///< set to true to use GPS data */
DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
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 */
DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
};

63
src/modules/ekf2/EKF2.cpp
View File

@ -31,6 +31,7 @@
*
****************************************************************************/
#include <px4_platform_common/events.h>
#include "EKF2.hpp"
using namespace time_literals;
@ -284,6 +285,8 @@ void EKF2::Run()
// update parameters from storage
updateParams();
VerifyParams();
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
@ -643,6 +646,66 @@ void EKF2::Run()
ScheduleDelayed(100_ms);
}
void EKF2::VerifyParams()
{
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS)
|| (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) {
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS |
SensorFusionMask::DEPRECATED_USE_GPS_YAW));
_param_ekf2_aid_mask.commit();
mavlink_log_critical(&_mavlink_log_pub, "Use EKF2_GPS_CTRL instead\n");
/* EVENT
* @description <param>EKF2_AID_MASK</param> is set to {1:.0}.
*/
events::send<float>(events::ID("ekf2_aid_mask_gps"), events::Log::Warning,
"Use EKF2_GPS_CTRL instead", _param_ekf2_aid_mask.get());
}
if ((_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::HPOS)) {
_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() & ~GnssCtrl::VPOS);
_param_ekf2_gps_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "GPS lon/lat is required for altitude fusion\n");
/* EVENT
* @description <param>EKF2_GPS_CTRL</param> is set to {1:.0}.
*/
events::send<float>(events::ID("ekf2_gps_ctrl_alt"), events::Log::Warning,
"GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get());
}
if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) {
_param_ekf2_baro_ctrl.set(1);
_param_ekf2_baro_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "Baro enabled by EKF2_HGT_REF\n");
/* EVENT
* @description <param>EKF2_BARO_CTRL</param> is set to {1:.0}.
*/
events::send<float>(events::ID("ekf2_hgt_ref_baro"), events::Log::Warning,
"Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get());
}
if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) {
_param_ekf2_rng_ctrl.set(1);
_param_ekf2_rng_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "Range enabled by EKF2_HGT_REF\n");
/* EVENT
* @description <param>EKF2_RNG_CTRL</param> is set to {1:.0}.
*/
events::send<float>(events::ID("ekf2_hgt_ref_rng"), events::Log::Warning,
"Range enabled by EKF2_HGT_REF", _param_ekf2_rng_ctrl.get());
}
if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) {
_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL));
_param_ekf2_gps_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "GPS enabled by EKF2_HGT_REF\n");
/* EVENT
* @description <param>EKF2_GPS_CTRL</param> is set to {1:.0}.
*/
events::send<float>(events::ID("ekf2_hgt_ref_gps"), events::Log::Warning,
"GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
}
}
void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
{
// airspeed

5
src/modules/ekf2/EKF2.hpp
View File

@ -52,6 +52,7 @@
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
@ -134,6 +135,8 @@ private:
void Run() override;
void VerifyParams();
void PublishAidSourceStatus(const hrt_abstime &timestamp);
void PublishAttitude(const hrt_abstime &timestamp);
void PublishBaroBias(const hrt_abstime &timestamp);
@ -281,6 +284,8 @@ private:
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
hrt_abstime _airspeed_validated_timestamp_last{0};
orb_advert_t _mavlink_log_pub{nullptr};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};