fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 14:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

made position controllers handle estimator state resets

Browse Source
This commit is contained in:
tumbili
2016-11-21 07:29:21 +01:00
committed by Roman
parent 937c5adfc0
commit b2410460a5
2 changed files with 79 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+22
View File
@@ -248,6 +248,10 @@ private:
float _asp_after_transition;
bool _was_in_transition;
// estimator reset counters
uint8_t _pos_reset_counter; // captures the number of times the estimator has reset the horizontal position
uint8_t _alt_reset_counter; // captures the number of times the estimator has reset the altitude state
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
enum FW_POSCTRL_MODE {
@@ -613,6 +617,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_last_tecs_update(0.0f),
_asp_after_transition(0.0f),
_was_in_transition(false),
_pos_reset_counter(0),
_alt_reset_counter(0),
_l1_control(),
_tecs(),
_control_mode_current(FW_POSCTRL_MODE_OTHER),
@@ -2283,6 +2289,22 @@ FixedwingPositionControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
// 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 && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt;
}
// adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled && _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
_hdg_hold_prev_wp.lat += _global_pos.delta_lat_lon[0];
_hdg_hold_prev_wp.lon += _global_pos.delta_lat_lon[1];
_hdg_hold_curr_wp.lat += _global_pos.delta_lat_lon[0];
_hdg_hold_curr_wp.lon += _global_pos.delta_lat_lon[1];
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
}
}
// XXX add timestamp check
_global_pos_valid = true;