fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 06:18:52 +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-26 18:01:44 +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/ekf2/EKF/ekf.cpp
+1
View File
@@ -330,6 +330,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
&& fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
) {
ECL_INFO("fused external observation as position measurement");
_state_reset_status.reset_count.posNE++;
_time_last_hor_pos_fuse = _time_delayed_us;
_last_known_pos.xy() = _state.pos.xy();
return true;
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);
}
}
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+4 -6
View File
@@ -406,12 +406,10 @@ private:
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
// ESTIMATOR RESET COUNTERS
// captures the number of times the estimator has reset the horizontal position
uint8_t _pos_reset_counter{0};
// captures the number of times the estimator has reset the altitude state
uint8_t _alt_reset_counter{0};
uint8_t _vxy_reset_counter{0};
uint8_t _vz_reset_counter{0};
uint8_t _xy_reset_counter{0};
uint64_t _time_last_xy_reset{0};
// LATERAL-DIRECTIONAL GUIDANCE