Changed min/max distance with distance topic params. Added a check for mocap: mocap estimation requires heading from mocap

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -37,6 +37,7 @@
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Nuno Marques <n.marques21@hotmail.com>
+ * @author Christoph Tobler <toblech@student.ethz.ch>
  */
 #include <px4_posix.h>
 #include <unistd.h>
@@ -315,12 +316,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		{ 0.0f },		// E (pos)
 		{ 0.0f },		// D (pos)
 	};
+	const int mocap_heading = 2;
 
 	float dist_ground = 0.0f;		//variables for lidar altitude estimation
 	float corr_lidar = 0.0f;
 	float lidar_offset = 0.0f;
-	float lidar_max_dist = 39.0f;
-	float lidar_min_dist = 0.1f;
 	int lidar_offset_count = 0;
 	bool lidar_first = true;
 	bool use_lidar = false;
@@ -545,42 +545,47 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
 			}
 			
-			if (updated && lidar.current_distance > lidar_min_dist && lidar.current_distance < lidar_max_dist
+			if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
-			    && (PX4_R(att.R, 2, 2) > 0.7f)) { //check if altitude estimation for lidar is enabled and new sensor data
 				
-				if (!use_lidar_prev && use_lidar) {
+				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
-					lidar_first = true;
+			    		&& (PX4_R(att.R, 2, 2) > 0.7f)) {
-				}
 
-				use_lidar_prev = use_lidar;
+					if (!use_lidar_prev && use_lidar) {
+						lidar_first = true;
+					}
 
-				lidar_time = t;
+					use_lidar_prev = use_lidar;
-				dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
 
-				if (lidar_first) {
+					lidar_time = t;
-					lidar_first = false;
+					dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
-					lidar_offset = dist_ground + z_est[0];
+
-					mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset");
+					if (lidar_first) {
-					warnx("[inav] LIDAR: new ground offset");
+						lidar_first = false;
-				}
+						lidar_offset = dist_ground + z_est[0];
+						mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset");
+						warnx("[inav] LIDAR: new ground offset");
+					}
 
-				corr_lidar = lidar_offset - dist_ground - z_est[0];
+					corr_lidar = lidar_offset - dist_ground - z_est[0];
 
-				if (fabsf(corr_lidar) > params.lidar_err) { //check for spike
+					if (fabsf(corr_lidar) > params.lidar_err) { //check for spike
-					corr_lidar = 0;
+						corr_lidar = 0;
-					lidar_valid = false;
+						lidar_valid = false;
-					lidar_offset_count++;
+						lidar_offset_count++;
 
-					if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
+						if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
-						lidar_first = true;
+							lidar_first = true;
+							lidar_offset_count = 0;
+						}
+
+					} else {
+						corr_lidar = lidar_offset - dist_ground - z_est[0];
+						lidar_valid = true;
 						lidar_offset_count = 0;
+						lidar_valid_time = t;
 					}
-
 				} else {
-					corr_lidar = lidar_offset - dist_ground - z_est[0];
+					lidar_valid = false;
-					lidar_valid = true;
-					lidar_offset_count = 0;
-					lidar_valid_time = t;
 				}
 			}
 
@@ -798,24 +803,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
 
-				/* reset position estimate on first mocap update */
+				if (!params.disable_mocap) {
-				if (!mocap_valid) {
+					/* reset position estimate on first mocap update */
-					x_est[0] = mocap.x;
+					if (!mocap_valid) {
-					y_est[0] = mocap.y;
+						x_est[0] = mocap.x;
-					z_est[0] = mocap.z;
+						y_est[0] = mocap.y;
+						z_est[0] = mocap.z;
 
-					mocap_valid = true;
+						mocap_valid = true;
 
-					warnx("MOCAP data valid");
+						warnx("MOCAP data valid");
-					mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
+						mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
-				}
+					}
 
-				/* calculate correction for position */
+					/* calculate correction for position */
-				corr_mocap[0][0] = mocap.x - x_est[0];
+					corr_mocap[0][0] = mocap.x - x_est[0];
-				corr_mocap[1][0] = mocap.y - y_est[0];
+					corr_mocap[1][0] = mocap.y - y_est[0];
-				corr_mocap[2][0] = mocap.z - z_est[0];
+					corr_mocap[2][0] = mocap.z - z_est[0];
 
-				mocap_updates++;
+					mocap_updates++;
+				}
 			}
 
 			/* vehicle GPS position */
@@ -992,7 +999,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
 		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
 		/* use MOCAP if it's valid and has a valid weight parameter */
-		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
+		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap
 
 		if (params.disable_mocap) { //disable mocap if fake gps is used
 			use_mocap = false;
@@ -1112,7 +1119,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (use_lidar) {
 			accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
-
 		} else {
 			accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
 		}

src/modules/position_estimator_inav/position_estimator_inav_params.cpp

@@ -375,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
 	h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
 	h->disable_mocap = param_find("INAV_DISAB_MOCAP");
 	h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST");
+	h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M");
 
 	return 0;
 }
@@ -408,6 +409,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
 	param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
 	param_get(h->disable_mocap, &(p->disable_mocap));
 	param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est));
+	param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m));
 
 	return 0;
 }

src/modules/position_estimator_inav/position_estimator_inav_params.h

@@ -68,6 +68,7 @@ struct position_estimator_inav_params {
 	float flow_module_offset_y;
 	int32_t disable_mocap;
 	int32_t enable_lidar_alt_est;
+	int32_t att_ext_hdg_m;
 };
 
 struct position_estimator_inav_param_handles {
@@ -97,6 +98,7 @@ struct position_estimator_inav_param_handles {
 	param_t flow_module_offset_y;
 	param_t disable_mocap;
 	param_t enable_lidar_alt_est;
+	param_t att_ext_hdg_m;
 };
 
 #define CBRK_NO_VISION_KEY	328754