From c314f31068ebeba858d98f5411bbdfc2812a8f6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 11 Oct 2013 11:58:21 +0200 Subject: [PATCH] multirotor_pos_control: update altitude setpoint if reference altitude changed --- .../multirotor_pos_control.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index f283a1eb45..8d6898cf2e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -70,11 +70,13 @@ #include "multirotor_pos_control_params.h" #include "thrust_pid.h" - static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ +static const float alt_ctl_dz = 0.2f; +static const float pos_ctl_dz = 0.05f; + __EXPORT int multirotor_pos_control_main(int argc, char *argv[]); /** @@ -233,9 +235,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) float surface_offset = 0.0f; // Z of ground level float z_sp_dist = 0.0f; // distance to ground setpoint (positive) + float ref_alt = 0.0f; + hrt_abstime ref_timestamp = 0; + hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; uint64_t local_ref_timestamp = 0; uint64_t surface_bottom_timestamp = 0; @@ -346,11 +349,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - //if (local_pos.ref_timestamp != ref_prev_t) { + if (local_pos.ref_timestamp != ref_timestamp) { /* reference alt changed, don't follow large ground level changes in manual flight */ - //local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; + local_pos_sp.z += local_pos.ref_alt - ref_alt; // TODO also correct XY setpoint - //} + } if (control_mode.flag_control_altitude_enabled) { /* reset setpoints to current position if needed */ @@ -704,6 +707,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) surface_bottom_timestamp = local_pos.surface_bottom_timestamp; + ref_alt = local_pos.ref_alt; + ref_timestamp = local_pos.ref_timestamp; + /* run at approximately 100 Hz */ usleep(10000); }