fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 19:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

reset position-mode line following after position reset

Browse Source
This commit is contained in:
Marco Hauswirth
2024-07-24 14:09:12 +02:00
committed by Silvan Fuhrer
parent 97561d7802
commit 5808dac4bc
3 changed files with 23 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+18 -7
View File
@@ -2134,6 +2134,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
throttle_max = 0.0f;
}
if (_local_pos.xy_reset_counter != _xy_reset_counter) {
_time_last_xy_reset = _local_pos.timestamp;
}
/* heading control */
// TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here)
if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
@@ -2164,6 +2168,13 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
_hdg_hold_position.lon = _current_longitude;
}
// if there's a reset-by-fusion, the ekf needs some time to converge,
// therefore we go into track holiding for 2 seconds
if (_local_pos.timestamp - _time_last_xy_reset < 2e6) {
_hdg_hold_position.lat = _current_latitude;
_hdg_hold_position.lon = _current_longitude;
}
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon);
@@ -2310,28 +2321,24 @@ FixedwingPositionControl::Run()
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) {
if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _vz_reset_counter) {
// make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(_current_altitude, -_local_pos.vz);
}
// adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _local_pos.vxy_reset_counter != _pos_reset_counter) {
&& _local_pos.vxy_reset_counter != _vxy_reset_counter) {
// reset heading hold flag, which will re-initialise position control
_hdg_hold_enabled = false;
}
}
// update the reset counters in any case
_alt_reset_counter = _local_pos.vz_reset_counter;
_pos_reset_counter = _local_pos.vxy_reset_counter;
// Convert Local setpoints to global setpoints
if (!_global_local_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
|| (_local_pos.vxy_reset_counter != _pos_reset_counter)) {
|| (_local_pos.vxy_reset_counter != _vxy_reset_counter)) {
double reference_latitude = 0.;
double reference_longitude = 0.;
@@ -2616,6 +2623,10 @@ FixedwingPositionControl::Run()
_spoilers_setpoint_pub.publish(spoilers_setpoint);
}
_vz_reset_counter = _local_pos.vz_reset_counter;
_vxy_reset_counter = _local_pos.vxy_reset_counter;
_xy_reset_counter = _local_pos.xy_reset_counter;
perf_end(_loop_perf);
}
}