From f73148e98daa508932497f0c2f800aeb2d7020e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 13:31:18 +0100 Subject: [PATCH] mc pos control: Support do change speed --- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) 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 388713d24c..13155f1aae 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -996,7 +996,20 @@ void MulticopterPositionControl::control_auto(float dt) if (current_setpoint_valid) { /* scaled space: 1 == position error resulting max allowed speed */ - math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + math::Vector<3> cruising_speed = _params.vel_max; + + if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && + _pos_sp_triplet.current.cruising_speed > 0.1f) { + cruising_speed(0) = _pos_sp_triplet.current.cruising_speed; + cruising_speed(1) = _pos_sp_triplet.current.cruising_speed; + } else { + // XXX need to introduce cruising speed param + cruising_speed(0) = _params.vel_max(0) * 0.6f; + cruising_speed(1) = _params.vel_max(0) * 0.6f; + } + + math::Vector<3> scale = _params.pos_p.edivide(cruising_speed); /* convert current setpoint to scaled space */ math::Vector<3> curr_sp_s = curr_sp.emult(scale);