fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 05:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc pos control: Support do change speed

Browse Source
This commit is contained in:
Lorenz Meier
2016-01-22 13:31:18 +01:00
parent 7871ab0fae
commit f73148e98d
1 changed files with 14 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
+14 -1
View File
@@ -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);