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

style and format clean

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 2cc193c173
commit efe312d4a6
5 changed files with 23 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6 -4
View File
@@ -1178,13 +1178,15 @@ void Ekf2::run()
ev_data.posErr = _ev_pos_noise.get();
ev_data.hgtErr = _ev_pos_noise.get();
}
// orientation measurement error from parameters
if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) {
ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmax(ev_odom.pose_covariance[18], ev_odom.pose_covariance[20]))));
ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmax(ev_odom.pose_covariance[18],
ev_odom.pose_covariance[20]))));
} else {
ev_data.angErr = _ev_ang_noise.get();
}
else {
ev_data.angErr = _ev_ang_noise.get();
}
// only set data if all positions are valid
if (sqrtf(ev_odom.pose_covariance[0]) < ep_max_std_dev && sqrtf(ev_odom.pose_covariance[6]) < ep_max_std_dev) {
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+2
View File
@@ -667,10 +667,12 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
_pub_est_status.get().covariances[13] = _P(X_bx, X_bx);
_pub_est_status.get().covariances[14] = _P(X_by, X_by);
_pub_est_status.get().covariances[15] = _P(X_bz, X_bz);
// mag - not determined
for (size_t i = 16; i <= 21; i++) {
_pub_est_status.get().covariances[i] = NAN;
}
// replacing the hor wind cov with terrain altitude covariance
_pub_est_status.get().covariances[22] = _P(X_tz, X_tz);
_pub_est_status.get().covariances[23] = NAN;
src/modules/local_position_estimator/sensors/mocap.cpp
+1
View File
@@ -66,6 +66,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;
} else {
// if we don't have covariances, assume every reading
_mocap_xy_valid = true;
src/modules/local_position_estimator/sensors/vision.cpp
+2 -1
View File
@@ -71,6 +71,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
_vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV;
_vision_z_valid = _vision_epv <= EP_MAX_STD_DEV;
} else {
// if we don't have covariances, assume every reading
_vision_xy_valid = true;
@@ -138,7 +139,7 @@ void BlockLocalPositionEstimator::visionCorrect()
if (vision_delay < 0.0f) { vision_delay = 0.0f; }
// use auto-calculated delay from measurement if parameter is set to zero
if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0){ return; }
if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+12 -6
View File
@@ -792,10 +792,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
static float last_vision_y = 0.0f;
static float last_vision_z = 0.0f;
vision_xy_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? sqrtf(fmaxf(visual_odom.pose_covariance[0], visual_odom.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
vision_z_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev : true) ? false : true;
vision_vxy_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? sqrtf(fmaxf(visual_odom.velocity_covariance[0], visual_odom.velocity_covariance[6])) > ev_max_std_dev : true) ? false : true;
vision_vz_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] > ep_max_std_dev : true) ? false : true;
vision_xy_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? sqrtf(fmaxf(visual_odom.pose_covariance[0],
visual_odom.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
vision_z_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev :
true) ? false : true;
vision_vxy_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? sqrtf(fmaxf(visual_odom.velocity_covariance[0],
visual_odom.velocity_covariance[6])) > ev_max_std_dev : true) ? false : true;
vision_vz_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] >
ep_max_std_dev : true) ? false : true;
/* reset position estimate on first vision update */
if (vision_xy_valid) {
@@ -909,8 +913,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap);
mocap_xy_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? sqrtf(fmaxf(mocap.pose_covariance[0], mocap.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
mocap_z_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? mocap.pose_covariance[11] > ep_max_std_dev : true) ? false : true;
mocap_xy_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? sqrtf(fmaxf(mocap.pose_covariance[0],
mocap.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
mocap_z_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? mocap.pose_covariance[11] > ep_max_std_dev : true) ? false :
true;
if (!params.disable_mocap) {
/* reset position estimate on first mocap update */