dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: Use arming state to prevent unsafe reference point changes

This commit is contained in:
Paul Riseborough 2017-09-22 08:38:52 +10:00 committed by Lorenz Meier
parent ef0e47ee63
commit f0e8abe783
1 changed files with 5 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -266,7 +266,6 @@ private:
struct map_projection_reference_s _ref_pos; struct map_projection_reference_s _ref_pos;
float _ref_alt; float _ref_alt;
hrt_abstime _ref_timestamp; hrt_abstime _ref_timestamp;
bool _first_origin_set;
hrt_abstime _last_warn; hrt_abstime _last_warn;
math::Vector<3> _thrust_int; math::Vector<3> _thrust_int;
@ -456,7 +455,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_user_intention_z(brake), _user_intention_z(brake),
_ref_alt(0.0f), _ref_alt(0.0f),
_ref_timestamp(0), _ref_timestamp(0),
_first_origin_set{false},
_last_warn(0), _last_warn(0),
_yaw(0.0f), _yaw(0.0f),
_yaw_takeoff(0.0f), _yaw_takeoff(0.0f),
@ -860,7 +858,11 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
void void
MulticopterPositionControl::update_ref() MulticopterPositionControl::update_ref()
{ {
if (_local_pos.ref_timestamp != _ref_timestamp && _first_origin_set) { // The reference point is only allowed to change when the vehicle is in standby state which is the
// normal state when the estimator origin is set. Changing reference point in flight causes large controller
// setpoint changes. Changing reference point in other arming states is untested and shoud not be performed.
if ((_local_pos.ref_timestamp != _ref_timestamp)
&& (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
double lat_sp, lon_sp; double lat_sp, lon_sp;
float alt_sp = 0.0f; float alt_sp = 0.0f;
@ -887,12 +889,6 @@ MulticopterPositionControl::update_ref()
_ref_timestamp = _local_pos.ref_timestamp; _ref_timestamp = _local_pos.ref_timestamp;
} else if (_local_pos.xy_global && _local_pos.z_global) {
// Ignore the origin change for the first time the origin is set.
// This allows for GPS use to commence after takeoff
_first_origin_set = true;
_ref_timestamp = _local_pos.ref_timestamp;
} }
} }