Sonar sensor usage in PX4

metusa · December 25, 2016, 12:44pm

Hi
I will use maxbotic I2C sonar to estimate the altitude precisely. I use the driver in /driver/mb12xx.cpp file and publish the distance_sensor topic. I see position_estimator_inav module uses it and publish the distance from ground field in the vehicle_local_position topic. In PX4, what should I do in order to use sonar sensor value together with barometer? I think it uses baro and gps value to estimate altitude, am I right?
Thanks.

Nicolas · January 3, 2017, 8:50am

I am not familiar with INAV. LPE is currently the default estimator and EKF2 is the most recent development. Both of the latter use range measurements to aid with height as well as terrain estimation. I.e. these estimators fuse range measurements with GPS and baro. For LPE, this happens here:

github.com

PX4/Firmware/blob/6ff85fb927bf65b614e4d8abb6ec3624df411710/src/modules/local_position_estimator/sensors/lidar.cpp#L78


	// account for leaning
	y(0) = y(0) *
	       cosf(_eul(0)) *
	       cosf(_eul(1));


	// measurement matrix
	Matrix<float, n_y_lidar, n_x> C;
	C.setZero();
	// y = -(z - tz)
	// TODO could add trig to make this an EKF correction
	C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir.
	C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.


	// use parameter covariance unless sensor provides reasonable value
	SquareMatrix<float, n_y_lidar> R;
	R.setZero();
	float cov = _sub_lidar->get().covariance;


	if (cov < 1.0e-3f) {
		R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get();

A quick glance at INAV leads me to believe that it uses range sensors (referred to as lidar measurements in INAV) if available, otherwise baro. I.e. it does not use the two as LPE or EKF2 do:

github.com

PX4/Firmware/blob/4453e4201b7a245cff52beeb38a293161aea4c48/src/modules/position_estimator_inav/position_estimator_inav_main.cpp#L1163




		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
					acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
					corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
			memcpy(z_est, z_est_prev, sizeof(z_est));
		}


		/* inertial filter correction for altitude */
		if (use_lidar) {
			inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);


		} else {
			inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
		}


		if (use_gps_z) {
			epv = fminf(epv, gps.epv);


			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);

I would suggest you upgrade to a more modern estimator.