position_estimator_inav default parameters changed, some fixes

3 files changed, 12 insertions, 1 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 508879ae2..3a14d516a 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -209,6 +209,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	float pos_y_integral = 0.0f;
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
+	float home_alt = 0.0f;
+	hrt_abstime home_alt_t = 0;
 
 	thread_running = true;
 
@@ -288,6 +290,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			float pos_sp_speed_z = 0.0f;
 
 			if (status.flag_control_manual_enabled) {
+				if (local_pos.home_timestamp != home_alt_t) {
+					if (home_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						local_pos_sp.z -= local_pos.home_alt - home_alt;
+					}
+					home_alt_t = local_pos.home_timestamp;
+					home_alt = local_pos.home_alt;
+				}
 				/* move altitude setpoint with manual controls */
 				float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
 				if (alt_sp_ctl != 0.0f) {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index fb09948ec..428269e4a 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -391,6 +391,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
 								local_pos.home_alt = baro_alt0;
 								local_pos.home_timestamp = hrt_absolute_time();
+								z_est[0] += sonar_corr;
 								sonar_corr = 0.0f;
 								sonar_corr_filtered = 0.0;
 							}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index eac2fc1ce..c90c611a7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
 PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
-PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)