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

mc_pos_control: reset height reference when global position available

Browse Source 
This enables the reference height to be reset when global vertical position becomes available in-flight.
This commit is contained in:
Paul Riseborough
2017-12-07 08:20:16 +00:00
committed by Lorenz Meier
parent db8900fffc
commit a81c49014f
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+9 -1
View File
@@ -268,6 +268,7 @@ private:
struct map_projection_reference_s _ref_pos;
float _ref_alt;
bool _ref_alt_is_global; /** true when the reference altitude is defined in a global reference frame */
hrt_abstime _ref_timestamp;
hrt_abstime _last_warn;
@@ -458,6 +459,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_user_intention_xy(brake),
_user_intention_z(brake),
_ref_alt(0.0f),
_ref_alt_is_global(false),
_ref_timestamp(0),
_last_warn(0),
_yaw(0.0f),
@@ -849,13 +851,15 @@ MulticopterPositionControl::update_ref()
// 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)) {
&& ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
|| (!_ref_alt_is_global && _local_pos.z_global))) {
double lat_sp, lon_sp;
float alt_sp = 0.0f;
if (_ref_timestamp != 0) {
// calculate current position setpoint in global frame
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
// the altitude setpoint is the reference altitude (Z up) plus the (Z down)
// NED setpoint, multiplied out to minus
alt_sp = _ref_alt - _pos_sp(2);
@@ -865,6 +869,10 @@ MulticopterPositionControl::update_ref()
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_local_pos.z_global) {
_ref_alt_is_global = true;
}
if (_ref_timestamp != 0) {
// reproject position setpoint to new reference
// this effectively adjusts the position setpoint to keep the vehicle