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

Replacing SETPOINT_TYPE_VELOCITY use with velocity and position valid

Browse Source 
booleans in setpoint struct
This commit is contained in:
Jimmy Johnson
2016-03-27 12:10:57 +02:00
committed by Lorenz Meier
parent 9993b24bf9
commit 6ff1b2d896
3 changed files with 21 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+12 -10
View File
@@ -1375,10 +1375,11 @@ MulticopterPositionControl::task_main()
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
// do not go slower than the follow target velocity when position tracking is active
// 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) {
_pos_sp_triplet.current.velocity_valid &&
_pos_sp_triplet.current.position_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
@@ -1397,16 +1398,20 @@ MulticopterPositionControl::task_main()
_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
}
// Ignore position control calculated vel set points
// 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) {
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_VELOCITY &&
_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
}
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
@@ -1417,11 +1422,8 @@ MulticopterPositionControl::task_main()
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
src/modules/navigator/follow_target.cpp
+8 -5
View File
@@ -135,7 +135,7 @@ void FollowTarget::on_active()
_follow_target_state = TRACK_VELOCITY;
} else {
track_target_position();
}
}
break;
}
case TRACK_VELOCITY: {
@@ -175,7 +175,7 @@ void FollowTarget::track_target_position()
// keep the current velocity updated
_current_vel = _target_vel;
_current_vel = _target_vel;
}
void FollowTarget::track_target_velocity()
@@ -271,16 +271,20 @@ void FollowTarget::update_position_sp(math::Vector<3> & vel)
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// activate line following in pos control
pos_sp_triplet->previous.valid = true;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
switch(_follow_target_state) {
case TRACK_POSITION:
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.position_valid = true;
pos_sp_triplet->current.velocity_valid = true;
break;
case TRACK_VELOCITY:
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
pos_sp_triplet->current.position_valid = false;
pos_sp_triplet->current.velocity_valid = true;
break;
default:
break;
@@ -288,7 +292,6 @@ void FollowTarget::update_position_sp(math::Vector<3> & vel)
pos_sp_triplet->current.vx = vel(0);
pos_sp_triplet->current.vy = vel(1);
pos_sp_triplet->current.velocity_valid = true;
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
src/modules/navigator/mission_block.cpp
+1 -4
View File
@@ -374,10 +374,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
sp->disable_mc_yaw_control = true;
}
break;// get rid of this
case NAV_CMD_FOLLOW_TARGET:
sp->type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
break;
break;
default:
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;