Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Fw pos control move to local pos: minimal changes to make it compile

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2020-03-04 21:51:16 +01:00
committed by Roman Bapst
parent 59a7262c31
commit 477092fa24
1 changed files with 4 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+4 -6
View File
@@ -1478,8 +1478,6 @@ FixedwingPositionControl::Run()
perf_begin(_loop_perf); perf_begin(_loop_perf);
/* only run controller if position changed */ /* only run controller if position changed */
float lpos_x_prev = _local_pos.x;
float lpos_y_prev = _local_pos.y;
if (_local_pos_sub.update(&_local_pos)) { if (_local_pos_sub.update(&_local_pos)) {
@@ -1504,7 +1502,7 @@ 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 && _current_altitude_reset_counter != _alt_reset_counter) { if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) {
_hold_alt += -_local_pos.delta_z; _hold_alt += -_local_pos.delta_z;
// make TECS accept step in altitude and demanded altitude // make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude); _tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude);
@@ -1512,7 +1510,7 @@ FixedwingPositionControl::Run()
// 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
&& _current_latitude_lon_reset_counter != _pos_reset_counter) { && _local_pos.vxy_reset_counter != _pos_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;
@@ -1520,8 +1518,8 @@ FixedwingPositionControl::Run()
} }
// update the reset counters in any case // update the reset counters in any case
_alt_reset_counter = _current_altitude_reset_counter; _alt_reset_counter = _local_pos.vz_reset_counter;
_pos_reset_counter = _current_latitude_lon_reset_counter; _pos_reset_counter = _local_pos.vxy_reset_counter;
airspeed_poll(); airspeed_poll();
_manual_control_sub.update(&_manual); _manual_control_sub.update(&_manual);