fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-10-30 21:19:57 +01:00
parent 9f57ee8564
commit a61b1e089c
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -2293,7 +2293,6 @@ FixedwingPositionControl::task_main()
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt;
_alt_reset_counter = _global_pos.alt_reset_counter;
// make TECS accept step in altitude and demanded altitude
_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
_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
_global_pos_valid = true;

10
src/modules/mc_pos_control/mc_pos_control_main.cpp
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;
}
}