From 83257240794a68042ab7a80a0c83f5555f9ee8b7 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 26 Jul 2018 15:53:46 +0100 Subject: [PATCH] general fixes on VIO data access --- .../init.d-posix/1013_iris_vision | 3 + msg/vehicle_odometry.msg | 2 +- posix-configs/SITL/init/lpe/iris_vision | 94 ------------------- .../attitude_estimator_q_main.cpp | 14 +-- src/modules/ekf2/ekf2_main.cpp | 50 +++++----- .../BlockLocalPositionEstimator.cpp | 7 +- .../BlockLocalPositionEstimator.hpp | 1 - .../sensors/mocap.cpp | 4 +- .../sensors/vision.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/simulator/simulator.h | 3 +- src/modules/simulator/simulator_mavlink.cpp | 13 ++- 12 files changed, 56 insertions(+), 141 deletions(-) delete mode 100644 posix-configs/SITL/init/lpe/iris_vision diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision index 99da9fc2d8..27bd0371cc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision @@ -11,4 +11,7 @@ if [ $AUTOCNF = yes ] then param set EKF2_AID_MASK 8 param set EKF2_EV_DELAY 5 + + # LPE: Vision + baro + param set LPE_FUSION 132 fi diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 2b28cb22a9..69febfafb2 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -5,7 +5,7 @@ float32 x # North position float32 y # East position float32 z # Down position -# Orientation quaternion. NaN if invalid/unknown +# Orientation quaternion. First value NaN if invalid/unknown float32[4] q # Quaternion rotation from NED earth-fixed frame to XYZ body frame # Row-major representation of 6x6 pose cross-covariance matrix URT. diff --git a/posix-configs/SITL/init/lpe/iris_vision b/posix-configs/SITL/init/lpe/iris_vision deleted file mode 100644 index 6437fef2c6..0000000000 --- a/posix-configs/SITL/init/lpe/iris_vision +++ /dev/null @@ -1,94 +0,0 @@ -uorb start -param load -param set MAV_TYPE 2 -param set SYS_AUTOSTART 4010 -param set SYS_RESTART_TYPE 2 -param set SYS_MC_EST_GROUP 1 -dataman start -param set BAT_N_CELLS 3 -param set CAL_GYRO0_ID 2293768 -param set CAL_ACC0_ID 1376264 -param set CAL_ACC1_ID 1310728 -param set CAL_MAG0_ID 196616 -param set CAL_GYRO0_XOFF 0.01 -param set CAL_ACC0_XOFF 0.01 -param set CAL_ACC0_YOFF -0.01 -param set CAL_ACC0_ZOFF 0.01 -param set CAL_ACC0_XSCALE 1.01 -param set CAL_ACC0_YSCALE 1.01 -param set CAL_ACC0_ZSCALE 1.01 -param set CAL_ACC1_XOFF 0.01 -param set CAL_MAG0_XOFF 0.01 -param set SENS_BOARD_ROT 0 -param set SENS_BOARD_X_OFF 0.000001 -param set COM_RC_IN_MODE 1 -param set NAV_DLL_ACT 2 -param set COM_DISARM_LAND 3 -param set NAV_ACC_RAD 2.0 -param set RTL_RETURN_ALT 30.0 -param set RTL_DESCEND_ALT 10.0 -param set RTL_LAND_DELAY 0 -param set MIS_TAKEOFF_ALT 2.5 -param set MC_ROLLRATE_P 0.3 -param set MC_PITCHRATE_P 0.3 -param set MC_PITCH_P 5.5 -param set MC_ROLL_P 5.5 -param set MC_ROLLRATE_I 0.1 -param set MC_PITCHRATE_I 0.1 -param set MPC_HOLD_MAX_Z 2.0 -param set MPC_Z_VEL_P 0.8 -param set MPC_Z_VEL_I 0.15 -param set MPC_XY_VEL_P 0.15 -param set MPC_XY_VEL_I 0.2 -param set SDLOG_DIRS_MAX 7 -param set MC_PITCH_P 5.0 -param set MC_ROLL_P 5.0 -param set MC_ROLLRATE_P 0.2 -param set MC_PITCHRATE_P 0.2 -param set MC_ROLLRATE_I 0.05 -param set MC_PITCHRATE_I 0.05 -param set MPC_ALT_MODE 1 -param set LPE_T_MAX_GRADE 0 -param set MPC_XY_VEL_MAX 2 -param set MPC_Z_VEL_MAX_DN 2 -param set MPC_TILTMAX_AIR 10 -param set MPC_TILTMAX_LND 10 -param set MPC_XY_P 0.5 -param set MIS_TAKEOFF_ALT 2 -param set NAV_ACC_RAD 1.0 -param set CBRK_GPSFAIL 240024 -# Vision + baro -param set LPE_FUSION 132 - -replay tryapplyparams -simulator start -s -tone_alarm start -gyrosim start -accelsim start -barosim start -gpssim start -pwm_out_sim start -sensors start -commander start -land_detector start multicopter -navigator start -attitude_estimator_q start -local_position_estimator start -mc_pos_control start -mc_att_control start -mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix -mavlink start -x -u 14556 -r 2000000 -mavlink start -x -u 14557 -r 2000000 -m onboard -o 14540 -mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 80 -s ATTITUDE -u 14556 -mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556 -mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 -mavlink stream -r 20 -s RC_CHANNELS -u 14556 -mavlink stream -r 250 -s HIGHRES_IMU -u 14556 -mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink stream -r 80 -s VISION_POSITION_ESTIMATE -u 14556 -logger start -e -t -mavlink boot_complete -replay trystart diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index c476b4f90e..aa806698a2 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -349,9 +349,10 @@ void AttitudeEstimatorQ::task_main() if (orb_copy(ORB_ID(vehicle_visual_odometry), _vision_odom_sub, &vision) == PX4_OK) { // validation check for vision attitude data - bool vision_att_valid = !PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15], - fmaxf(vision.pose_covariance[18], - vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true; + bool vision_att_valid = PX4_ISFINITE(vision.q[0]) + && (PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15], + fmaxf(vision.pose_covariance[18], + vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true); if (vision_att_valid) { Dcmf Rvis = Quatf(vision.q); @@ -379,9 +380,10 @@ void AttitudeEstimatorQ::task_main() if (orb_copy(ORB_ID(vehicle_mocap_odometry), _mocap_odom_sub, &mocap) == PX4_OK) { // validation check for mocap attitude data - bool mocap_att_valid = !PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15], - fmaxf(mocap.pose_covariance[18], - mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true; + bool mocap_att_valid = PX4_ISFINITE(mocap.q[0]) + && (PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15], + fmaxf(mocap.pose_covariance[18], + mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true); if (mocap_att_valid) { Dcmf Rmoc = Quatf(mocap.q); diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 30b9ee1248..a8ee9a2e36 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -221,7 +221,7 @@ private: // set pose/velocity as invalid if standard deviation is bigger than max_std_dev // TODO: the user should be allowed to set these values by a parameter static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position - //static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation + static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation //static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity // GPS blending and switching @@ -1160,35 +1160,43 @@ void Ekf2::run() if (visual_odometry_updated) { // copy both attitude & position, we need both to fill a single ext_vision_message - vehicle_odometry_s ev_odom = {}; + vehicle_odometry_s ev_odom; orb_copy(ORB_ID(vehicle_visual_odometry), _ev_odom_sub, &ev_odom); ext_vision_message ev_data; - ev_data.posNED(0) = ev_odom.x; - ev_data.posNED(1) = ev_odom.y; - ev_data.posNED(2) = ev_odom.z; - ev_data.quat = matrix::Quatf(ev_odom.q); - // position measurement error from parameters - if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) { - ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6]))); - ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11])); - } else { - ev_data.posErr = _ev_pos_noise.get(); - ev_data.hgtErr = _ev_pos_noise.get(); + // check for valid position data + if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) { + ev_data.posNED(0) = ev_odom.x; + ev_data.posNED(1) = ev_odom.y; + ev_data.posNED(2) = ev_odom.z; + + // position measurement error from parameters + if (PX4_ISFINITE(ev_odom.pose_covariance[0])) { + ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6]))); + ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11])); + } else { + 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], fmaxf(ev_odom.pose_covariance[18], - ev_odom.pose_covariance[20])))); + // check for valid orientation data + if (PX4_ISFINITE(ev_odom.q[0])) { + ev_data.quat = matrix::Quatf(ev_odom.q); - } else { - ev_data.angErr = _ev_ang_noise.get(); + // orientation measurement error from parameters + if (PX4_ISFINITE(ev_odom.pose_covariance[15])) { + ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmaxf(ev_odom.pose_covariance[18], + ev_odom.pose_covariance[20])))); + + } 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) { + // only set data if all positions and orientation are valid + if (ev_data.posErr < ep_max_std_dev && ev_data.angErr < eo_max_std_dev) { // use timestamp from external computer, clocks are synchronized when using MAVROS _ekf.setExtVisionData(ev_odom.timestamp, &ev_data); } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 4990200c07..9e946c6f47 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -599,7 +599,7 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().z = xLP(X_z); // down } - _pub_gpos.get().yaw = matrix::Eulerf(_q).psi(); + _pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); _pub_lpos.get().vx = xLP(X_vx); // north _pub_lpos.get().vy = xLP(X_vy); // east @@ -723,7 +723,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().vel_n = xLP(X_vx); _pub_gpos.get().vel_e = xLP(X_vy); _pub_gpos.get().vel_d = xLP(X_vz); - _pub_gpos.get().yaw = matrix::Eulerf(_q).psi(); + _pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); @@ -827,8 +827,7 @@ void BlockLocalPositionEstimator::updateSSParams() void BlockLocalPositionEstimator::predict() { // get acceleration - _q = matrix::Quatf(&_sub_att.get().q[0]); - _R_att = matrix::Dcm(_q); + _R_att = matrix::Dcm(matrix::Quatf(_sub_att.get().q)); Vector3f a(_sub_sensor.get().accelerometer_m_s2); // note, bias is removed in dynamics function _u = _R_att * a; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 7cdd7ed298..002147d7a9 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -436,7 +436,6 @@ private: Vector _u; // input vector Matrix _P; // state covariance matrix - matrix::Quatf _q; matrix::Dcm _R_att; Matrix _A; // dynamics matrix diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index 9991fbbe25..ee0c028784 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -60,7 +60,7 @@ void BlockLocalPositionEstimator::mocapInit() int BlockLocalPositionEstimator::mocapMeasure(Vector &y) { - if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) { + if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) { // check if the mocap data is valid based on the covariances _mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6])); _mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]); @@ -75,7 +75,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector &y) if (!_mocap_xy_valid || !_mocap_z_valid) { _time_last_mocap = _sub_mocap_odom.get().timestamp; - return !OK; + return -1; } else { y.setZero(); diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 9f5c4f4f21..ad20d3d3b9 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -65,7 +65,7 @@ void BlockLocalPositionEstimator::visionInit() int BlockLocalPositionEstimator::visionMeasure(Vector &y) { - if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) { + if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) { // check if the vision data is valid based on the covariances _vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6])); _vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]); @@ -80,7 +80,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector &y) if (!_vision_xy_valid || !_vision_z_valid) { _time_last_vision_p = _sub_visual_odom.get().timestamp; - return !OK; + return -1; } else { y.setZero(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0d09a294fb..ec6e121102 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2109,7 +2109,7 @@ protected: bool send(const hrt_abstime t) { - vehicle_odometry_s vodom = {}; + vehicle_odometry_s vodom; if (_odom_sub->update(&_odom_time, &vodom)) { mavlink_vision_position_estimate_t vmsg = {}; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 9ac696e167..cdd7886036 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -344,8 +344,7 @@ private: // class methods int publish_sensor_topics(mavlink_hil_sensor_t *imu); int publish_flow_topic(mavlink_hil_optical_flow_t *flow); - template - int publish_odometry_topic(T *odom_mavlink); + int publish_odometry_topic(mavlink_message_t *odom_mavlink); int publish_distance_topic(mavlink_distance_sensor_t *dist); #ifndef __PX4_QURT diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 29757a4d21..16f69e6a45 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1123,18 +1123,17 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) return OK; } -template -int Simulator::publish_odometry_topic(T *msg) +int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink) { uint64_t timestamp = hrt_absolute_time(); - struct vehicle_odometry_s odom = {}; + struct vehicle_odometry_s odom; odom.timestamp = timestamp; - if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) { + if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) { mavlink_odometry_t odom_msg; - mavlink_msg_odometry_decode(msg, &odom_msg); + mavlink_msg_odometry_decode(odom_mavlink, &odom_msg); /* Dcm rotation matrix from body frame to local NED frame */ matrix::Dcm Rbl; @@ -1175,9 +1174,9 @@ int Simulator::publish_odometry_topic(T *msg) odom.velocity_covariance[i] = odom_msg.twist_covariance[i]; } - } else if (msg->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { + } else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { mavlink_vision_position_estimate_t ev; - mavlink_msg_vision_position_estimate_decode(msg, &ev); + mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev); /* The position in the local NED frame */ odom.x = ev.x; odom.y = ev.y;