diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-10-18 14:15:35 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-10-18 14:15:35 +0200 |
commit | 7d4981d4b4389d28fa05d661ed52be41564b953b (patch) | |
tree | 43fc62003b5e2c948407425d45b47c9c385842be | |
parent | 3a571ea18f9b54ac07dd5ec5d54655aaf9603dd5 (diff) | |
download | px4-firmware-7d4981d4b4389d28fa05d661ed52be41564b953b.tar.gz px4-firmware-7d4981d4b4389d28fa05d661ed52be41564b953b.tar.bz2 px4-firmware-7d4981d4b4389d28fa05d661ed52be41564b953b.zip |
multirotor_pos_control: bug fixed
-rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 7 |
1 files changed, 6 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 6bc9912b0..b53bceb76 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -389,7 +389,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) vz = -local_pos.dist_bottom_rate; /* move altitude setpoint according to ground distance */ local_pos_sp.z = surface_offset - sp_z_dist; - sp_z = -sp_z_dist; } /* move altitude setpoint with throttle stick */ @@ -408,6 +407,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = local_pos.z - z_sp_offs_max; } } + + if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) { + sp_z = -sp_z_dist; + } else { + sp_z = local_pos_sp.z; + } } if (control_mode.flag_control_position_enabled) { |