From 5865a40bf32a48a493edc9c7dce3ba031a8056d7 Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Thu, 12 Jul 2018 22:13:50 +0100
Subject: [PATCH] attitude_estimator_q: add attitude data validation check
---
.../attitude_estimator_q_main.cpp | 93 +++++++++++--------
1 file changed, 53 insertions(+), 40 deletions(-)
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 08dc7a216d..c476b4f90e 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -100,17 +100,18 @@ public:
void task_main();
private:
+ const float _eo_max_std_dev = 100.0f; /**< Maximum permissible standard deviation for estimated orientation */
const float _dt_min = 0.00001f;
const float _dt_max = 0.02f;
- bool _task_should_exit = false; /**< if true, task should exit */
- int _control_task = -1; /**< task handle for task */
+ bool _task_should_exit = false; /**< if true, task should exit */
+ int _control_task = -1; /**< task handle for task */
int _params_sub = -1;
int _sensors_sub = -1;
int _global_pos_sub = -1;
- int _vision_sub = -1;
- int _mocap_sub = -1;
+ int _vision_odom_sub = -1;
+ int _mocap_odom_sub = -1;
int _magnetometer_sub = -1;
orb_advert_t _att_pub = nullptr;
@@ -262,8 +263,8 @@ void AttitudeEstimatorQ::task_main()
#endif
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
- _vision_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry));
- _mocap_sub = orb_subscribe(ORB_ID(vehicle_mocap_odometry));
+ _vision_odom_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry));
+ _mocap_odom_sub = orb_subscribe(ORB_ID(vehicle_mocap_odometry));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer));
@@ -341,49 +342,61 @@ void AttitudeEstimatorQ::task_main()
// Update vision and motion capture heading
bool vision_updated = false;
- orb_check(_vision_sub, &vision_updated);
+ orb_check(_vision_odom_sub, &vision_updated);
if (vision_updated) {
- vehicle_attitude_s vision;
-
- if (orb_copy(ORB_ID(vehicle_visual_odometry), _vision_sub, &vision) == PX4_OK) {
- Quatf q(vision.q);
-
- Dcmf Rvis = Quatf(vision.q);
- Vector3f v(1.0f, 0.0f, 0.4f);
-
- // Rvis is Rwr (robot respect to world) while v is respect to world.
- // Hence Rvis must be transposed having (Rwr)' * Vw
- // Rrw * Vw = vn. This way we have consistency
- _vision_hdg = Rvis.transpose() * v;
-
- // vision external heading usage (ATT_EXT_HDG_M 1)
- if (_ext_hdg_mode == 1) {
- // Check for timeouts on data
- _ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
+ vehicle_odometry_s vision;
+
+ 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;
+
+ if (vision_att_valid) {
+ Dcmf Rvis = Quatf(vision.q);
+ Vector3f v(1.0f, 0.0f, 0.4f);
+
+ // Rvis is Rwr (robot respect to world) while v is respect to world.
+ // Hence Rvis must be transposed having (Rwr)' * Vw
+ // Rrw * Vw = vn. This way we have consistency
+ _vision_hdg = Rvis.transpose() * v;
+
+ // vision external heading usage (ATT_EXT_HDG_M 1)
+ if (_ext_hdg_mode == 1) {
+ // Check for timeouts on data
+ _ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
+ }
}
}
}
bool mocap_updated = false;
- orb_check(_mocap_sub, &mocap_updated);
+ orb_check(_mocap_odom_sub, &mocap_updated);
if (mocap_updated) {
vehicle_odometry_s mocap;
- if (orb_copy(ORB_ID(vehicle_mocap_odometry), _mocap_sub, &mocap) == PX4_OK) {
- Dcmf Rmoc = Quatf(mocap.q);
- Vector3f v(1.0f, 0.0f, 0.4f);
-
- // Rmoc is Rwr (robot respect to world) while v is respect to world.
- // Hence Rmoc must be transposed having (Rwr)' * Vw
- // Rrw * Vw = vn. This way we have consistency
- _mocap_hdg = Rmoc.transpose() * v;
-
- // Motion Capture external heading usage (ATT_EXT_HDG_M 2)
- if (_ext_hdg_mode == 2) {
- // Check for timeouts on data
- _ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
+ 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;
+
+ if (mocap_att_valid) {
+ Dcmf Rmoc = Quatf(mocap.q);
+ Vector3f v(1.0f, 0.0f, 0.4f);
+
+ // Rmoc is Rwr (robot respect to world) while v is respect to world.
+ // Hence Rmoc must be transposed having (Rwr)' * Vw
+ // Rrw * Vw = vn. This way we have consistency
+ _mocap_hdg = Rmoc.transpose() * v;
+
+ // Motion Capture external heading usage (ATT_EXT_HDG_M 2)
+ if (_ext_hdg_mode == 2) {
+ // Check for timeouts on data
+ _ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
+ }
}
}
}
@@ -451,8 +464,8 @@ void AttitudeEstimatorQ::task_main()
orb_unsubscribe(_params_sub);
orb_unsubscribe(_sensors_sub);
orb_unsubscribe(_global_pos_sub);
- orb_unsubscribe(_vision_sub);
- orb_unsubscribe(_mocap_sub);
+ orb_unsubscribe(_vision_odom_sub);
+ orb_unsubscribe(_mocap_odom_sub);
orb_unsubscribe(_magnetometer_sub);
}
--
GitLab