fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 18:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW pos control: do not requre global pos in manual position control

Browse Source
This commit is contained in:
bresch
2024-07-31 15:14:16 +02:00
committed by Mathieu Bresciani
parent 6cf0bf5e19
commit f04aa2494b
2 changed files with 7 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+6 -9
View File
@@ -2158,28 +2158,25 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
if (!_hdg_hold_enabled) {
// just switched back from non heading-hold to heading hold
_hdg_hold_enabled = true;
_hdg_hold_yaw = _yaw;
_hdg_hold_position.lat = _current_latitude;
_hdg_hold_position.lon = _current_longitude;
_hdg_hold_position = curr_pos_local;
}
// 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 < 2_s) {
_hdg_hold_position.lat = _current_latitude;
_hdg_hold_position.lon = _current_longitude;
_hdg_hold_position = curr_pos_local;
}
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);
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;