From 38b42789984db8bacbd9c4f7db5967873da4af77 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Tue, 12 Apr 2016 13:25:19 -0700 Subject: [PATCH] minimum follow target alt of 8 m added, protecting against nan values in pos controller, fixing ci build error --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++++-- src/modules/navigator/follow_target.cpp | 5 +---- src/modules/navigator/follow_target.h | 4 ++-- src/modules/navigator/mission_block.cpp | 2 +- 4 files changed, 12 insertions(+), 9 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 8dd6672071..d2431989fd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1417,10 +1417,16 @@ MulticopterPositionControl::task_main() _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); } + // guard against any bad velocity values + + bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && + PX4_ISFINITE(_pos_sp_triplet.current.vy) && + _pos_sp_triplet.current.velocity_valid; + // do not go slower than the follow target velocity when position tracking is active (set to valid) if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - _pos_sp_triplet.current.velocity_valid && + velocity_valid && _pos_sp_triplet.current.position_valid) { math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0); @@ -1444,7 +1450,7 @@ MulticopterPositionControl::task_main() // track target using velocity only } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - _pos_sp_triplet.current.velocity_valid) { + velocity_valid) { _vel_sp(0) = _pos_sp_triplet.current.vx; _vel_sp(1) = _pos_sp_triplet.current.vy; diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 060149cb43..7a3955bd4d 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include @@ -78,8 +77,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : _target_vel.zero(); _target_distance.zero(); _target_position_offset.zero(); - - _rot_matrix = (_follow_position_matricies[_follow_target_position]); } FollowTarget::~FollowTarget() @@ -97,7 +94,7 @@ void FollowTarget::on_activation() _follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get(); - _follow_target_position = (FollowTargetPosition) _param_tracking_side.get(); + _follow_target_position = _param_tracking_side.get(); if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { _follow_target_position = FOLLOW_FROM_BEHIND; diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index b00136b5ce..7395accd58 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -72,7 +72,7 @@ private: WAIT_FOR_TARGET_POSITION }; - enum FollowTargetPosition { + enum { FOLLOW_FROM_RIGHT, FOLLOW_FROM_BEHIND, FOLLOW_FROM_FRONT, @@ -103,7 +103,7 @@ private: control::BlockParamInt _param_tracking_side; FollowTargetState _follow_target_state; - FollowTargetPosition _follow_target_position; + int _follow_target_position; int _follow_target_sub; float _step_time_in_ms; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 74dbd59f74..8c2c460c63 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -469,7 +469,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea item->lon = target.lon; item->altitude = _navigator->get_home_position()->alt; - if (min_clearance > 0.0f) { + if (min_clearance > 8.0f) { item->altitude += min_clearance; } else { item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)