From e6670d792650c960ad8a50a6a56dc9d9b2a58e83 Mon Sep 17 00:00:00 2001 From: Nacho Carnicero Date: Sun, 4 Jun 2017 14:01:40 +0200 Subject: [PATCH] Drone yaw set to destination waypoint yaw in LOITER mode if MIS_YAWMODE=0 (#7269) * Drone does not change orientation while in LOITER mode if MIS_YAWMODE=0 * Fix code style * Some more code style fixes * Format checks passed --- src/modules/navigator/loiter.cpp | 28 ++++++++++++++++------------ src/modules/navigator/loiter.h | 9 +++++++++ 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index d8ff78afa2..2844ce61c3 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -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); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 802e1f6151..824f294280 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -60,6 +60,14 @@ public: virtual void on_active(); + enum mission_yaw_mode { + MISSION_YAWMODE_NONE = 0, + MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, + MISSION_YAWMODE_FRONT_TO_HOME = 2, + MISSION_YAWMODE_BACK_TO_HOME = 3, + MISSION_YAWMODE_MAX = 4 + }; + private: /** * Use the stored reposition location of the navigator @@ -73,6 +81,7 @@ private: void set_loiter_position(); control::BlockParamFloat _param_min_alt; + control::BlockParamInt _param_yawmode; bool _loiter_pos_set; };