Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 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-10-30 21:19:57 +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/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+4 -4
View File
@@ -2293,7 +2293,6 @@ FixedwingPositionControl::task_main()
if (_control_mode.flag_control_manual_enabled) { if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) { if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt; _hold_alt += _global_pos.delta_alt;
_alt_reset_counter = _global_pos.alt_reset_counter;
// make TECS accept step in altitude and demanded altitude // make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt); _tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt);
} }
@@ -2304,12 +2303,13 @@ FixedwingPositionControl::task_main()
// reset heading hold flag, which will re-initialise position control // reset heading hold flag, which will re-initialise position control
_hdg_hold_enabled = false; _hdg_hold_enabled = false;
// update reset counter
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
} }
} }
// update the reset counters in any case
_alt_reset_counter = _global_pos.alt_reset_counter;
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
// XXX add timestamp check // XXX add timestamp check
_global_pos_valid = true; _global_pos_valid = true;
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 (_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.delta_z;
_z_reset_counter = _local_pos.z_reset_counter;
} }
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.delta_xy[0];
_pos_sp(1) += _local_pos.delta_xy[1]; _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) { if (_vz_reset_counter != _local_pos.vz_reset_counter) {
_vel_sp(2) += _local_pos.delta_vz; _vel_sp(2) += _local_pos.delta_vz;
_vel_sp_prev(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) { 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(1) += _local_pos.delta_vxy[1];
_vel_sp_prev(0) += _local_pos.delta_vxy[0]; _vel_sp_prev(0) += _local_pos.delta_vxy[0];
_vel_sp_prev(1) += _local_pos.delta_vxy[1]; _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;
} }
} }