fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 23:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

adding user parameters, simplifying dynamic gps filter, adding yaw smoothing

Browse Source
This commit is contained in:
Jimmy Johnson
2016-05-15 16:14:22 -07:00
parent bb79d14cb1
commit 59296e0a49
4 changed files with 88 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -1
View File
@@ -1149,7 +1149,10 @@ void MulticopterPositionControl::control_auto(float dt)
_pos_sp = pos_sp_s.edivide(scale);
/* update yaw setpoint if needed */
if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
if (_pos_sp_triplet.current.yawspeed_valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
src/modules/navigator/follow_target.cpp
+60 -45
View File
@@ -52,6 +52,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/math/Limits.hpp>
#include "navigator.h"
@@ -61,6 +62,8 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_param_min_alt(this, "NAV_MIN_FT_HT", false),
_param_tracking_dist(this,"NAV_FT_DST", false),
_param_tracking_side(this,"NAV_FT_FS", false),
_param_tracking_resp(this,"NAV_FT_RS", false),
_param_yaw_auto_max(this,"MC_YAWRAUTO_MAX", false),
_follow_target_state(SET_WAIT_FOR_TARGET_POSITION),
_follow_target_position(FOLLOW_FROM_BEHIND),
_follow_target_sub(-1),
@@ -70,10 +73,11 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_last_update_time(0),
_current_target_motion({}),
_previous_target_motion({}),
_confidence(0.0F),
_confidence_ratio(0.0F),
_yaw_rate(0.0F)
_avg_cos_ratio(0.0F),
_yaw_rate(0.0F),
_responsiveness(0.0F)
{
_avg_cos_ratio = 0.0F;
_filtered_target_lat = _filtered_target_lon = 0.0F;
updateParams();
_current_vel.zero();
@@ -99,6 +103,10 @@ void FollowTarget::on_activation()
_follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get();
_responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F);
_yaw_auto_max = _param_yaw_auto_max.get();
_follow_target_position = _param_tracking_side.get();
if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
@@ -187,52 +195,33 @@ void FollowTarget::on_active()
float cos_ratio = (_target_position_delta * prev_position_delta)/(_target_position_delta.length() * prev_position_delta.length());
if(_confidence >= RESPONSIVENESS) {
_confidence = 0.0F; // reset confidence level to 50/50
_avg_cos_ratio = _responsiveness*_avg_cos_ratio + (1 - _responsiveness) * cos_ratio;
if(_avg_cos_ratio < 0) {
_avg_cos_ratio = 0.0F;
}
_confidence += cos_ratio;
if (_confidence < -1.0F * RESPONSIVENESS) {
_confidence = -1.0F * RESPONSIVENESS;
if (_avg_cos_ratio > 0.0F) {
_filtered_target_position_delta = _target_position_delta*_avg_cos_ratio + _filtered_target_position_delta*(1.0F - _avg_cos_ratio);
} else {
_filtered_target_position_delta.zero();
}
if (_confidence > RESPONSIVENESS) {
_confidence = RESPONSIVENESS;
}
_confidence_ratio = (_confidence + RESPONSIVENESS) / (RESPONSIVENESS * 2.0F);
// track to the left, right, behind, or front
_filtered_target_position_delta = (_target_position_delta*_confidence_ratio) + _filtered_target_position_delta*(1.0F - _confidence_ratio);
// only track from a set side if we are 100% sure
// UAV is moving in a straight line
if(_confidence_ratio >= 1.0F) {
if(_avg_cos_ratio >= .50F) {
_target_position_offset = _rot_matrix * (_filtered_target_position_delta.normalized() * _follow_offset);
}
} else {
_filtered_target_position_delta.zero();
_confidence_ratio = _confidence = 0.0F;
_avg_cos_ratio = 0.0F;
}
yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = (yaw_angle - _navigator->get_global_position()->yaw)/(dt_ms/1000.0F);
_yaw_rate = _wrap_pi(_yaw_rate);
// update the average velocity of the target based on the position
_est_target_vel = _filtered_target_position_delta / (dt_ms / 1000.0f);
_filtered_target_lat = (_current_target_motion.lat*(double)_confidence_ratio) + _filtered_target_lat*(double)(1 - _confidence_ratio);
_filtered_target_lon = (_current_target_motion.lon*(double)_confidence_ratio) + _filtered_target_lon*(double)(1 - _confidence_ratio);
_filtered_target_lat = (_current_target_motion.lat*(double)_avg_cos_ratio) + _filtered_target_lat*(double)(1 - _avg_cos_ratio);
_filtered_target_lon = (_current_target_motion.lon*(double)_avg_cos_ratio) + _filtered_target_lon*(double)(1 - _avg_cos_ratio);
// are we within the target acceptance radius?
// give a buffer to exit/enter the radius to give the velocity controller
@@ -252,9 +241,35 @@ void FollowTarget::on_active()
_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
// if we are less than 3 meters from the target don't worry about trying to yaw
// just lock the yaw until we are a distance that makes sense
if(_target_distance.length() > 3.0F) {
// smooth yaw
// this really needs to control the yaw rate directly in the attitude pid controller
// but seems to work ok for now since that cannot be controlled directly in auto mode
// right now
yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = (yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
_yaw_rate = _wrap_pi(_yaw_rate);
_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max)*.50F;
} else {
yaw_angle = _yaw_rate = NAN;
}
}
warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f con = %3.6f",
warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f yaw rate = %3.6f",
(double) _step_vel(0),
(double) _step_vel(1),
(double) _current_vel(0),
@@ -263,7 +278,7 @@ void FollowTarget::on_active()
(double) _est_target_vel(1),
(double) _target_distance.length(),
_follow_target_state,
(double)_confidence_ratio, (double) _confidence);
(double)_avg_cos_ratio, (double) _yaw_rate);
}
// update state machine
@@ -280,8 +295,7 @@ void FollowTarget::on_active()
_current_vel = _est_target_vel;
_filtered_target_lat = _current_target_motion.lat;
_filtered_target_lon = _current_target_motion.lon;
// TODO: make max cruise speed less if very close to target
update_position_sp(true, true, _yaw_rate);
update_position_sp(true, true, NAN);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
@@ -294,13 +308,14 @@ void FollowTarget::on_active()
if (_radius_exited == true) {
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
_current_vel += _step_vel;
_last_update_time = current_time;
}
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
update_position_sp(true, false, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
@@ -361,15 +376,15 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
pos_sp_triplet->current.vx = _current_vel(0);
pos_sp_triplet->current.vy = _current_vel(1);
pos_sp_triplet->next.valid = false;
pos_sp_triplet->current.yawspeed_valid = true;
pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
pos_sp_triplet->current.yawspeed = yaw_rate;
_navigator->set_position_setpoint_triplet_updated();
}
void FollowTarget::reset_target_validity()
{
_confidence = -1*RESPONSIVENESS;
_confidence_ratio = 0.0F;
_yaw_rate = NAN;
_avg_cos_ratio = 0.0F;
_previous_target_motion = {};
_current_target_motion = {};
_target_updates = 0;
@@ -384,12 +399,12 @@ void FollowTarget::reset_target_validity()
bool FollowTarget::target_velocity_valid()
{
// need at least 5 continuous data points for velocity estimate
// need at least 2 continuous data points for velocity estimate
return (_target_updates >= 2);
}
bool FollowTarget::target_position_valid()
{
// need at least 2 continuous data points for position estimate
// need at least 1 continuous data points for position estimate
return (_target_updates >= 1);
}
src/modules/navigator/follow_target.h
+11 -11
View File
@@ -60,16 +60,12 @@ public:
private:
static constexpr int TARGET_TIMEOUT_MS = 5000;
static constexpr int TARGET_TIMEOUT_MS = 2500;
static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
static constexpr int INTERPOLATION_PNTS = 20;
static constexpr float FF_K = .1F;
static constexpr float FF_K = .25F;
static constexpr float OFFSET_M = 8;
// higher numbers slow down the time it takes to decide whether a target is moving
static constexpr float RESPONSIVENESS = 8.0F;
enum FollowTargetState {
TRACK_POSITION,
TRACK_VELOCITY,
@@ -103,9 +99,12 @@ private:
Navigator *_navigator;
control::BlockParamFloat _param_min_alt;
control::BlockParamFloat _param_tracking_dist;
control::BlockParamInt _param_tracking_side;
control::BlockParamFloat _param_min_alt;
control::BlockParamFloat _param_tracking_dist;
control::BlockParamInt _param_tracking_side;
control::BlockParamFloat _param_tracking_resp;
control::BlockParamFloat _param_yaw_auto_max;
FollowTargetState _follow_target_state;
int _follow_target_position;
@@ -127,11 +126,12 @@ private:
follow_target_s _current_target_motion;
follow_target_s _previous_target_motion;
float _confidence;
float _confidence_ratio;
float _avg_cos_ratio;
double _filtered_target_lat;
double _filtered_target_lon;
float _yaw_rate;
float _responsiveness;
float _yaw_auto_max;
// Mavlink defined motion reporting capabilities
src/modules/navigator/follow_target_params.c
+13
View File
@@ -77,3 +77,16 @@ PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f);
*/
PARAM_DEFINE_INT32(NAV_FT_FS, 1);
/**
* Dynamic filtering algorithm responsiveness to target movement
* lower numbers increase the responsiveness to changing long lat
* but also ignore less noise
*
* @unit n/a
* @min 0.0
* @max 1.0
* @decimal 2
* @group Follow target
*/
PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.8f);