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

make sure to update the reset counters every time the topic updates

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-11-21 07:29:21 +01:00
parent 9f57ee8564
commit a61b1e089c
2 changed files with 10 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -4
View File
@@ -724,19 +724,16 @@ MulticopterPositionControl::poll_subscriptions()
if (_control_mode.flag_control_manual_enabled) {
if (_z_reset_counter != _local_pos.z_reset_counter) {
_pos_sp(2) += _local_pos.delta_z;
_z_reset_counter = _local_pos.z_reset_counter;
}
if (_xy_reset_counter != _local_pos.xy_reset_counter) {
_pos_sp(0) += _local_pos.delta_xy[0];
_pos_sp(1) += _local_pos.delta_xy[1];
_xy_reset_counter = _local_pos.xy_reset_counter;
}
if (_vz_reset_counter != _local_pos.vz_reset_counter) {
_vel_sp(2) += _local_pos.delta_vz;
_vel_sp_prev(2) += _local_pos.delta_vz;
_vz_reset_counter = _local_pos.vz_reset_counter;
}
if (_vxy_reset_counter != _local_pos.vxy_reset_counter) {
@@ -744,9 +741,14 @@ MulticopterPositionControl::poll_subscriptions()
_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];
_vxy_reset_counter = _local_pos.vxy_reset_counter;
}
}
// update the reset counters in any case
_z_reset_counter = _local_pos.z_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;
}
}