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

bugfix: wrong reset_counter used

Browse Source
This commit is contained in:
Marco Hauswirth
2024-07-26 18:01:44 +02:00
committed by Silvan Fuhrer
parent 5808dac4bc
commit 9d9d8aeb4c
2 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+4 -5
View File
@@ -2321,14 +2321,14 @@ FixedwingPositionControl::Run()
// handle estimator reset events. we only adjust setpoins for manual modes // handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) { if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _vz_reset_counter) { if (_control_mode.flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
// make TECS accept step in altitude and demanded altitude // make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(_current_altitude, -_local_pos.vz); _tecs.handle_alt_step(_current_altitude, -_local_pos.vz);
} }
// adjust navigation waypoints in position control mode // adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _local_pos.vxy_reset_counter != _vxy_reset_counter) { && _local_pos.xy_reset_counter != _xy_reset_counter) {
// 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;
@@ -2338,7 +2338,7 @@ FixedwingPositionControl::Run()
// Convert Local setpoints to global setpoints // Convert Local setpoints to global setpoints
if (!_global_local_proj_ref.isInitialized() if (!_global_local_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp) || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
|| (_local_pos.vxy_reset_counter != _vxy_reset_counter)) { || (_local_pos.xy_reset_counter != _xy_reset_counter)) {
double reference_latitude = 0.; double reference_latitude = 0.;
double reference_longitude = 0.; double reference_longitude = 0.;
@@ -2623,8 +2623,7 @@ FixedwingPositionControl::Run()
_spoilers_setpoint_pub.publish(spoilers_setpoint); _spoilers_setpoint_pub.publish(spoilers_setpoint);
} }
_vz_reset_counter = _local_pos.vz_reset_counter; _z_reset_counter = _local_pos.z_reset_counter;
_vxy_reset_counter = _local_pos.vxy_reset_counter;
_xy_reset_counter = _local_pos.xy_reset_counter; _xy_reset_counter = _local_pos.xy_reset_counter;
perf_end(_loop_perf); perf_end(_loop_perf);
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+1 -2
View File
@@ -406,9 +406,8 @@ private:
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
// ESTIMATOR RESET COUNTERS // ESTIMATOR RESET COUNTERS
uint8_t _vxy_reset_counter{0};
uint8_t _vz_reset_counter{0};
uint8_t _xy_reset_counter{0}; uint8_t _xy_reset_counter{0};
uint8_t _z_reset_counter{0};
uint64_t _time_last_xy_reset{0}; uint64_t _time_last_xy_reset{0};
// LATERAL-DIRECTIONAL GUIDANCE // LATERAL-DIRECTIONAL GUIDANCE