diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 393d5960a2..8a899f6d21 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -68,6 +68,7 @@ #include #include #include +#include using control::BlockParamFloat; using control::BlockParamExtFloat; @@ -190,6 +191,7 @@ private: BlockParamExtFloat _rng_delay_ms; ///< range finder measurement delay relative to the IMU (mSec) BlockParamExtFloat _airspeed_delay_ms; ///< airspeed measurement delay relative to the IMU (mSec) BlockParamExtFloat _ev_delay_ms; ///< off-board vision measurement delay relative to the IMU (mSec) + BlockParamExtFloat _auxvel_delay_ms; ///< auxillary velocity measurement delay relative to the IMU (mSec) BlockParamExtFloat _gyro_noise; ///< IMU angular rate noise used for covariance prediction (rad/sec) BlockParamExtFloat _accel_noise; ///< IMU acceleration noise use for covariance prediction (m/sec**2) @@ -342,6 +344,7 @@ Ekf2::Ekf2(): _rng_delay_ms(this, "RNG_DELAY", true, _params->range_delay_ms), _airspeed_delay_ms(this, "ASP_DELAY", true, _params->airspeed_delay_ms), _ev_delay_ms(this, "EV_DELAY", true, _params->ev_delay_ms), + _auxvel_delay_ms(this, "AVEL_DELAY", true, _params->auxvel_delay_ms), _gyro_noise(this, "GYR_NOISE", true, _params->gyro_noise), _accel_noise(this, "ACC_NOISE", true, _params->accel_noise), _gyro_bias_p_noise(this, "GYR_B_NOISE", true, _params->gyro_bias_p_noise), @@ -465,6 +468,7 @@ void Ekf2::run() int status_sub = orb_subscribe(ORB_ID(vehicle_status)); int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection)); int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + int landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose)); bool imu_bias_reset_request = false; @@ -498,6 +502,7 @@ void Ekf2::run() sensor_selection_s sensor_selection = {}; sensor_baro_s sensor_baro = {}; sensor_baro.pressure = 1013.5f; // initialise pressure to sea level + landing_target_pose_s landing_target_pose = {}; while (!should_exit()) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); @@ -537,6 +542,7 @@ void Ekf2::run() bool vision_position_updated = false; bool vision_attitude_updated = false; bool vehicle_status_updated = false; + bool landing_target_pose_updated = false; orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors); // update all other topics if they have new data @@ -872,6 +878,25 @@ void Ekf2::run() _ekf.set_in_air_status(!vehicle_land_detected.landed); } + // use the landing target pose estimate as another source of velocity data + orb_check(landing_target_pose_sub, &landing_target_pose_updated); + + if (landing_target_pose_updated) { + orb_copy(ORB_ID(landing_target_pose), landing_target_pose_sub, &landing_target_pose); + + // we can only use the landing target if it has a fixed position and a valid velocity estimate + if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) { + // velocity of vehicle relative to target has opposite sign to target relative to vehicle + float velocity[2]; + velocity[0] = -landing_target_pose.vx_rel; + velocity[1] = -landing_target_pose.vy_rel; + float variance[2]; + variance[0] = landing_target_pose.cov_vx_rel; + variance[1] = landing_target_pose.cov_vy_rel; + _ekf.setAuxVelData(landing_target_pose.timestamp, velocity, variance); + } + } + // run the EKF update and output const bool updated = _ekf.update(); @@ -1238,6 +1263,7 @@ void Ekf2::run() ekf2_innovations_s innovations; innovations.timestamp = now; _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); + _ekf.get_aux_vel_innov(&innovations.aux_vel_innov[0]); _ekf.get_mag_innov(&innovations.mag_innov[0]); _ekf.get_heading_innov(&innovations.heading_innov); _ekf.get_airspeed_innov(&innovations.airspeed_innov); @@ -1414,6 +1440,7 @@ void Ekf2::run() orb_unsubscribe(status_sub); orb_unsubscribe(sensor_selection_sub); orb_unsubscribe(sensor_baro_sub); + orb_unsubscribe(landing_target_pose_sub); for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { orb_unsubscribe(range_finder_subs[i]); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index f10538f501..6babe44f0a 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -136,6 +136,18 @@ PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 100); */ PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175); +/** + * Auxillary Velocity Estimate (e.g from a landing target) delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5); + /** * Integer bitmask controlling GPS checks. *