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

Always check reset flags in auto. Only set reset flags to true when required. Do not limit hor/vert setpoint acceleration when setpoints are reset.

Browse Source
This commit is contained in:
devbharat
2016-11-21 09:05:22 +01:00
committed by Lorenz Meier
parent d720cbe189
commit 59c1dd7183
2 changed files with 120 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -24
View File
@@ -1076,11 +1076,12 @@ void MulticopterPositionControl::control_auto(float dt)
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_pos_sp();
reset_alt_sp();
}
// Always check reset state of altitude and position control flags in auto
reset_pos_sp();
reset_alt_sp();
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
@@ -1239,25 +1240,6 @@ void MulticopterPositionControl::control_auto(float dt)
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
/*
* if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
* this makes the takeoff finish smoothly.
*/
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
|| _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
&& _pos_sp_triplet.current.acceptance_radius > 0.0f
/* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
_reset_pos_sp = false;
_reset_alt_sp = false;
/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
} else {
_reset_pos_sp = true;
_reset_alt_sp = true;
}
// During a mission or in loiter it's safe to retract the landing gear.
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
@@ -1660,7 +1642,7 @@ MulticopterPositionControl::task_main()
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if (acc_hor.length() > _params.acc_hor_max) {
if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
@@ -1670,9 +1652,10 @@ MulticopterPositionControl::task_main()
}
// limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
}