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,8 +545,10 @@ 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
-			    && (PX4_R(att.R, 2, 2) > 0.7f)) { //check if altitude estimation for lidar is enabled and new sensor data
+			if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
+				
+				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
+			    		&& (PX4_R(att.R, 2, 2) > 0.7f)) {
 
 					if (!use_lidar_prev && use_lidar) {
 						lidar_first = true;
@@ -582,6 +584,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						lidar_offset_count = 0;
 						lidar_valid_time = t;
 					}
+				} else {
+					lidar_valid = false;
+				}
 			}
 
 			/* optical flow */
@@ -798,6 +803,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
 
+				if (!params.disable_mocap) {
 					/* reset position estimate on first mocap update */
 					if (!mocap_valid) {
 						x_est[0] = mocap.x;
@@ -817,6 +823,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					mocap_updates++;
 				}
+			}
 
 			/* vehicle GPS position */
 			orb_check(vehicle_gps_position_sub, &updated);
@@ -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