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

Allow for immediate cruise speed changes at any time during a mission.

Browse Source
This commit is contained in:
Simon Wilks
2016-12-28 11:53:19 +01:00
committed by Lorenz Meier
parent e0fc0a847c
commit 647fafe9bc
3 changed files with 31 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+24
View File
@@ -230,6 +230,12 @@ Mission::on_active()
}
}
/* check if a cruise speed change has been commanded */
if (_mission_type != MISSION_TYPE_NONE) {
cruising_speed_sp_update();
}
/* see if we need to update the current yaw heading */
if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
&& _param_yawmode.get() < MISSION_YAWMODE_MAX
@@ -1106,6 +1112,24 @@ Mission::altitude_sp_foh_reset()
_min_current_sp_distance_xy = FLT_MAX;
}
void
Mission::cruising_speed_sp_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
const float cruising_speed = _navigator->get_cruising_speed();
/* Don't change setpoint if the current waypoint is not valid */
if (!pos_sp_triplet->current.valid ||
fabsf(pos_sp_triplet->current.cruising_speed - cruising_speed) < FLT_EPSILON) {
return;
}
pos_sp_triplet->current.cruising_speed = cruising_speed;
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::do_abort_landing()
{