From 8e732fc52763a05739e4f13caf0dff84817839cd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 12:58:12 +0400 Subject: position_estimator_inav default parameters changed, some fixes --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 10 ++++++++++ .../position_estimator_inav/position_estimator_inav_main.c | 1 + .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 3 files changed, 12 insertions(+), 1 deletion(-) (limited to 'src/modules') 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) -- cgit v1.2.3