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

ekf2: move local position publication to method

Browse Source
This commit is contained in:
Daniel Agar
2020-11-11 20:17:52 -05:00
parent c09a5e0d9f
commit 98334d1325
2 changed files with 122 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+120 -132
View File
@@ -681,137 +681,9 @@ void EKF2::Run()
if (ekf_updated) { if (ekf_updated) {
vehicle_local_position_s lpos{};
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
filter_control_status_u control_status; filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value); _ekf.get_control_mode(&control_status.value);
// only publish position after successful alignment
if (control_status.flags.tilt_align) {
// generate vehicle local position data
lpos.timestamp_sample = imu_sample_new.time_us;
// Position of body origin in local NED frame
const Vector3f position = _ekf.getPosition();
lpos.x = position(0);
lpos.y = position(1);
lpos.z = position(2);
// Velocity of body origin in local NED frame (m/s)
const Vector3f velocity = _ekf.getVelocity();
lpos.vx = velocity(0);
lpos.vy = velocity(1);
lpos.vz = velocity(2);
// vertical position time derivative (m/s)
lpos.z_deriv = _ekf.getVerticalPositionDerivative();
// Acceleration of body origin in local frame
Vector3f vel_deriv = _ekf.getVelocityDerivative();
lpos.ax = vel_deriv(0);
lpos.ay = vel_deriv(1);
lpos.az = vel_deriv(2);
// TODO: better status reporting
lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
lpos.z_valid = !_preflt_checker.hasVertFailed();
lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
lpos.v_z_valid = !_preflt_checker.hasVertFailed();
// Position of local NED origin in GPS / WGS84 frame
map_projection_reference_s ekf_origin;
uint64_t origin_time;
// true if position (x,y,z) has a valid WGS-84 global reference (ref_lat, ref_lon, alt)
const bool ekf_origin_valid = _ekf.get_ekf_origin(&origin_time, &ekf_origin, &lpos.ref_alt);
lpos.xy_global = ekf_origin_valid;
lpos.z_global = ekf_origin_valid;
if (ekf_origin_valid && (origin_time > lpos.ref_timestamp)) {
lpos.ref_timestamp = origin_time;
lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
}
// The rotation of the tangent plane vs. geographical north
const Quatf q = _ekf.getQuaternion();
Quatf delta_q_reset;
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
lpos.heading = Eulerf(q).psi();
lpos.delta_heading = Eulerf(delta_q_reset).psi();
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
float terrain_vpos = _ekf.getTerrainVertPos();
lpos.dist_bottom = terrain_vpos - lpos.z; // Distance to bottom surface (ground) in meters
// constrain the distance to ground to _rng_gnd_clearance
if (lpos.dist_bottom < _param_ekf2_min_rng.get()) {
lpos.dist_bottom = _param_ekf2_min_rng.get();
}
if (!_had_valid_terrain) {
_had_valid_terrain = lpos.dist_bottom_valid;
}
// only consider ground effect if compensation is configured and the vehicle is armed (props spinning)
if ((_param_ekf2_gnd_eff_dz.get() > 0.0f) && _armed) {
// set ground effect flag if vehicle is closer than a specified distance to the ground
if (lpos.dist_bottom_valid) {
_ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get());
// if we have no valid terrain estimate and never had one then use ground effect flag from land detector
// _had_valid_terrain is used to make sure that we don't fall back to using this option
// if we temporarily lose terrain data due to the distance sensor getting out of range
} else if (!_had_valid_terrain) {
// update ground effect flag based on land detector state
_ekf.set_gnd_effect_flag(_in_ground_effect);
}
} else {
_ekf.set_gnd_effect_flag(false);
}
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
// get state reset information of position and velocity
_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
// get control limit information
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
// convert NaN to INFINITY
if (!PX4_ISFINITE(lpos.vxy_max)) {
lpos.vxy_max = INFINITY;
}
if (!PX4_ISFINITE(lpos.vz_max)) {
lpos.vz_max = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_min)) {
lpos.hagl_min = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_max)) {
lpos.hagl_max = INFINITY;
}
// publish vehicle local position data
lpos.timestamp = _replay_mode ? now : hrt_absolute_time();
_local_position_pub.publish(lpos);
// publish vehicle_odometry
PublishOdometry(now, imu_sample_new);
}
// publish estimator states // publish estimator states
estimator_states_s states; estimator_states_s states;
states.timestamp_sample = imu_sample_new.time_us; states.timestamp_sample = imu_sample_new.time_us;
@@ -836,8 +708,7 @@ void EKF2::Run()
status.hgt_test_ratio, status.tas_test_ratio, status.hgt_test_ratio, status.tas_test_ratio,
status.hagl_test_ratio, status.beta_test_ratio); status.hagl_test_ratio, status.beta_test_ratio);
status.pos_horiz_accuracy = lpos.eph; _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
status.pos_vert_accuracy = lpos.epv;
_ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.get_ekf_soln_status(&status.solution_status_flags);
_ekf.getImuVibrationMetrics().copyTo(status.vibe); _ekf.getImuVibrationMetrics().copyTo(status.vibe);
status.time_slip = _last_time_slip_us * 1e-6f; status.time_slip = _last_time_slip_us * 1e-6f;
@@ -922,6 +793,8 @@ void EKF2::Run()
} }
} }
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishGlobalPosition(now); PublishGlobalPosition(now);
PublishSensorBias(now); PublishSensorBias(now);
PublishWindEstimate(now); PublishWindEstimate(now);
@@ -1233,6 +1106,121 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
} }
} }
void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
{
vehicle_local_position_s lpos;
// generate vehicle local position data
lpos.timestamp_sample = timestamp;
// Position of body origin in local NED frame
const Vector3f position = _ekf.getPosition();
lpos.x = position(0);
lpos.y = position(1);
lpos.z = position(2);
// Velocity of body origin in local NED frame (m/s)
const Vector3f velocity = _ekf.getVelocity();
lpos.vx = velocity(0);
lpos.vy = velocity(1);
lpos.vz = velocity(2);
// vertical position time derivative (m/s)
lpos.z_deriv = _ekf.getVerticalPositionDerivative();
// Acceleration of body origin in local frame
Vector3f vel_deriv = _ekf.getVelocityDerivative();
lpos.ax = vel_deriv(0);
lpos.ay = vel_deriv(1);
lpos.az = vel_deriv(2);
// TODO: better status reporting
lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
lpos.z_valid = !_preflt_checker.hasVertFailed();
lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
lpos.v_z_valid = !_preflt_checker.hasVertFailed();
// Position of local NED origin in GPS / WGS84 frame
uint64_t origin_time;
map_projection_reference_s ekf_origin_pos;
const bool ekf_origin_valid = _ekf.get_ekf_origin(&origin_time, &ekf_origin_pos, &lpos.ref_alt);
lpos.ref_timestamp = origin_time;
lpos.ref_lat = math::degrees(ekf_origin_pos.lat_rad); // Reference point latitude in degrees
lpos.ref_lon = math::degrees(ekf_origin_pos.lon_rad); // Reference point longitude in degrees
lpos.xy_global = ekf_origin_valid;
lpos.z_global = ekf_origin_valid;
Quatf delta_q_reset;
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
lpos.delta_heading = Eulerf(delta_q_reset).psi();
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
float terrain_vpos = _ekf.getTerrainVertPos();
// Distance to bottom surface (ground) in meters
// constrain the distance to ground to _rng_gnd_clearance
lpos.dist_bottom = math::min(terrain_vpos - lpos.z, _param_ekf2_min_rng.get());
if (!_had_valid_terrain) {
_had_valid_terrain = lpos.dist_bottom_valid;
}
// only consider ground effect if compensation is configured and the vehicle is armed (props spinning)
if ((_param_ekf2_gnd_eff_dz.get() > 0.0f) && _armed) {
// set ground effect flag if vehicle is closer than a specified distance to the ground
if (lpos.dist_bottom_valid) {
_ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get());
// if we have no valid terrain estimate and never had one then use ground effect flag from land detector
// _had_valid_terrain is used to make sure that we don't fall back to using this option
// if we temporarily lose terrain data due to the distance sensor getting out of range
} else if (!_had_valid_terrain) {
// update ground effect flag based on land detector state
_ekf.set_gnd_effect_flag(_in_ground_effect);
}
} else {
_ekf.set_gnd_effect_flag(false);
}
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
// get state reset information of position and velocity
_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
// get control limit information
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
// convert NaN to INFINITY
if (!PX4_ISFINITE(lpos.vxy_max)) {
lpos.vxy_max = INFINITY;
}
if (!PX4_ISFINITE(lpos.vz_max)) {
lpos.vz_max = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_min)) {
lpos.hagl_min = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_max)) {
lpos.hagl_max = INFINITY;
}
// publish vehicle local position data
lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_local_position_pub.publish(lpos);
}
void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu) void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
{ {
// generate vehicle odometry data // generate vehicle odometry data
@@ -1265,8 +1253,8 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
odom.yawspeed = rates(2) - gyro_bias(2); odom.yawspeed = rates(2) - gyro_bias(2);
// get the covariance matrix size // get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
// Get covariances to vehicle odometry // Get covariances to vehicle odometry
float covariances[24]; float covariances[24];
src/modules/ekf2/EKF2.hpp
+2 -1
View File
@@ -137,8 +137,9 @@ private:
void PublishAttitude(const hrt_abstime &timestamp); void PublishAttitude(const hrt_abstime &timestamp);
void PublishEkfDriftMetrics(const hrt_abstime &timestamp); void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
void PublishGlobalPosition(const hrt_abstime &timestamp); void PublishGlobalPosition(const hrt_abstime &timestamp);
void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow); void PublishLocalPosition(const hrt_abstime &timestamp);
void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu); void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow);
void PublishSensorBias(const hrt_abstime &timestamp); void PublishSensorBias(const hrt_abstime &timestamp);
void PublishWindEstimate(const hrt_abstime &timestamp); void PublishWindEstimate(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp); void PublishYawEstimatorStatus(const hrt_abstime &timestamp);