Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 19:57:12 +08:00
Code Issues Actions 14 Packages Projects Releases Wiki Activity

reset setpoint to current position

Browse Source 
avoid abrupt position changes as the delta can be quite big depending on deadreckoning/imu
This commit is contained in:
ChristophTobler
2017-10-25 11:05:47 +02:00
committed by ChristophTobler
parent bd84061ea5
commit 8e457b6037
1 changed files with 3 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -21
View File
@@ -297,8 +297,6 @@ private:
// they are used to identify a reset event // they are used to identify a reset event
uint8_t _z_reset_counter; uint8_t _z_reset_counter;
uint8_t _xy_reset_counter; uint8_t _xy_reset_counter;
uint8_t _vz_reset_counter;
uint8_t _vxy_reset_counter;
uint8_t _heading_reset_counter; uint8_t _heading_reset_counter;
matrix::Dcmf _R_setpoint; matrix::Dcmf _R_setpoint;
@@ -471,8 +469,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_takeoff_vel_limit(0.0f), _takeoff_vel_limit(0.0f),
_z_reset_counter(0), _z_reset_counter(0),
_xy_reset_counter(0), _xy_reset_counter(0),
_vz_reset_counter(0),
_vxy_reset_counter(0),
_heading_reset_counter(0) _heading_reset_counter(0)
{ {
/* Make the attitude quaternion valid */ /* Make the attitude quaternion valid */
@@ -778,32 +774,18 @@ MulticopterPositionControl::poll_subscriptions()
// since we want the vehicle to track the original state. // since we want the vehicle to track the original state.
if (_control_mode.flag_control_manual_enabled) { if (_control_mode.flag_control_manual_enabled) {
if (_z_reset_counter != _local_pos.z_reset_counter) { if (_z_reset_counter != _local_pos.z_reset_counter) {
_pos_sp(2) += _local_pos.delta_z; _pos_sp(2) = _local_pos.z;
} }
if (_xy_reset_counter != _local_pos.xy_reset_counter) { if (_xy_reset_counter != _local_pos.xy_reset_counter) {
_pos_sp(0) += _local_pos.delta_xy[0]; _pos_sp(0) = _local_pos.x;
_pos_sp(1) += _local_pos.delta_xy[1]; _pos_sp(1) = _local_pos.y;
}
if (_vz_reset_counter != _local_pos.vz_reset_counter) {
_vel_sp(2) += _local_pos.delta_vz;
_vel_sp_prev(2) += _local_pos.delta_vz;
}
if (_vxy_reset_counter != _local_pos.vxy_reset_counter) {
_vel_sp(0) += _local_pos.delta_vxy[0];
_vel_sp(1) += _local_pos.delta_vxy[1];
_vel_sp_prev(0) += _local_pos.delta_vxy[0];
_vel_sp_prev(1) += _local_pos.delta_vxy[1];
} }
} }
// update the reset counters in any case // update the reset counters in any case
_z_reset_counter = _local_pos.z_reset_counter; _z_reset_counter = _local_pos.z_reset_counter;
_xy_reset_counter = _local_pos.xy_reset_counter; _xy_reset_counter = _local_pos.xy_reset_counter;
_vz_reset_counter = _local_pos.vz_reset_counter;
_vxy_reset_counter = _local_pos.vxy_reset_counter;
} }
orb_check(_pos_sp_triplet_sub, &updated); orb_check(_pos_sp_triplet_sub, &updated);