fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 07:20:34 +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;
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+1 -1
View File
@@ -298,7 +298,7 @@ private:
bool _hdg_hold_enabled{false}; // heading hold enabled
bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
position_setpoint_s _hdg_hold_position{}; // position where heading hold started
Vector2f _hdg_hold_position{}; // position where heading hold started
// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
float _manual_control_setpoint_for_height_rate{0.0f};