fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-06-06 13:33:56 +02:00 committed by Roman
parent 937c5adfc0
commit b2410460a5
2 changed files with 79 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
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;

58
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -269,6 +269,14 @@ private:
float _takeoff_thrust_sp;
bool control_vel_enabled_prev; /**< previous loop was in velocity controlled mode (control_state.flag_control_velocity_enabled) */
// counters for reset events on position and velocity states
// they are used to identify a reset event
uint8_t _z_reset_counter;
uint8_t _xy_reset_counter;
uint8_t _vz_reset_counter;
uint8_t _vxy_reset_counter;
uint8_t _heading_reset_counter;
/**
* Update our local parameter cache.
*/
@ -407,7 +415,12 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_z_lp(0),
_acc_z_lp(0),
_takeoff_thrust_sp(0.0f),
control_vel_enabled_prev(false)
control_vel_enabled_prev(false),
_z_reset_counter(0),
_xy_reset_counter(0),
_vz_reset_counter(0),
_vxy_reset_counter(0),
_heading_reset_counter(0)
{
// Make the quaternion valid for control state
_ctrl_state.q[0] = 1.0f;
@ -660,6 +673,18 @@ MulticopterPositionControl::poll_subscriptions()
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
if (_control_mode.flag_control_manual_enabled) {
if (_heading_reset_counter != _ctrl_state.quat_reset_counter) {
_heading_reset_counter = _ctrl_state.quat_reset_counter;
math::Quaternion delta_q(_ctrl_state.delta_q_reset[0], _ctrl_state.delta_q_reset[1], _ctrl_state.delta_q_reset[2], _ctrl_state.delta_q_reset[3]);
// we only extract the heading change from the delta quaternion
math::Vector<3> delta_euler = delta_q.to_euler();
_att_sp.yaw_body += delta_euler(2);
}
}
}
orb_check(_att_sp_sub, &updated);
@ -690,6 +715,37 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
// check if a reset event has happened
// if the vehicle is in manual mode we will shift the setpoints of the
// states which were reset. In auto mode we do not shift the setpoints
// since we want the vehicle to track the original state.
if (_control_mode.flag_control_manual_enabled) {
if (_z_reset_counter != _local_pos.z_reset_counter) {
_pos_sp(2) += _local_pos.delta_z;
_z_reset_counter = _local_pos.z_reset_counter;
}
if (_xy_reset_counter != _local_pos.xy_reset_counter) {
_pos_sp(0) += _local_pos.delta_xy[0];
_pos_sp(1) += _local_pos.delta_xy[1];
_xy_reset_counter = _local_pos.xy_reset_counter;
}
if (_vz_reset_counter != _local_pos.vz_reset_counter) {
_vel_sp(2) += _local_pos.delta_vz;
_vel_sp_prev(2) += _local_pos.delta_vz;
_vz_reset_counter = _local_pos.vz_reset_counter;
}
if (_vxy_reset_counter != _local_pos.vxy_reset_counter) {
_vel_sp(0) += _local_pos.delta_vxy[0];
_vel_sp(1) += _local_pos.delta_vxy[1];
_vel_sp_prev(0) += _local_pos.delta_vxy[0];
_vel_sp_prev(1) += _local_pos.delta_vxy[1];
_vxy_reset_counter = _local_pos.vxy_reset_counter;
}
}
}
}