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

Drone yaw set to destination waypoint yaw in LOITER mode if MIS_YAWMODE=0 (#7269)

Browse Source 
* Drone does not change orientation while in LOITER mode if MIS_YAWMODE=0

* Fix code style

* Some more code style fixes

* Format checks passed
This commit is contained in:
Nacho Carnicero
2017-06-04 14:01:40 +02:00
committed by Lorenz Meier
parent c80fd2c317
commit e6670d7926
2 changed files with 25 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/loiter.cpp
+16 -12
View File
@@ -59,6 +59,7 @@
Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_min_alt(this, "MIS_LTRMIN_ALT", false),
_param_yawmode(this, "MIS_YAWMODE", false),
_loiter_pos_set(false)
{
// load initial params
@@ -159,19 +160,22 @@ Loiter::reposition()
memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
pos_sp_triplet->next.valid = false;
// set yaw
// set yaw (depends on the value of parameter MIS_YAWMODE):
// MISSION_YAWMODE_NONE: do not change yaw setpoint
// MISSION_YAWMODE_FRONT_TO_WAYPOINT: point to next waypoint
if (_param_yawmode.get() != MISSION_YAWMODE_NONE) {
float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
if (travel_dist > 1.0f) {
// calculate direction the vehicle should point to.
pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
if (travel_dist > 1.0f) {
// calculate direction the vehicle should point to.
pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
}
}
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);