diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 16:29:14 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 16:29:14 +0100 |
commit | 2728889f7886e3ab2fea16941d29e60ece0d4449 (patch) | |
tree | ca9994d71205731ee4bb404175c2cf8f13fcc539 /src/modules/mc_pos_control | |
parent | f23e603d02ba416ae250770cdaad6a859d6bae69 (diff) | |
parent | 1dcc5c49cc75778bcdde770f2d6c2700dd2bec2e (diff) | |
download | px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.tar.gz px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.tar.bz2 px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.zip |
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge_attctlposctl
Diffstat (limited to 'src/modules/mc_pos_control')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 15 |
1 files changed, 14 insertions, 1 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index bf65d2805..60682fb8e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -35,6 +35,12 @@ * @file mc_pos_control_main.cpp * Multicopter position controller. * + * Original publication for the desired attitude generation: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011 + * + * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1 + * * The controller has two loops: P loop for position error and PID loop for velocity error. * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). @@ -727,11 +733,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { |