From 118b9aad194231950c639f3bb1e6c8bf5c659239 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 May 2017 15:17:09 +0200 Subject: [PATCH] MC pos control: Document the reference update handling function better --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b81a27ac57..2d1c1c90e0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -797,17 +797,22 @@ MulticopterPositionControl::update_ref() float alt_sp = 0.0f; if (_ref_timestamp != 0) { - /* calculate current position setpoint in global frame */ + // 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); } - /* update local projection reference */ + // update local projection reference including altitude map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { - /* reproject position setpoint to new reference */ + // reproject position setpoint to new reference + // this effectively adjusts the position setpoint to keep the vehicle + // in its current local position. It would only change its + // global position on the next setpoint update. map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); }