From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:34:19 +0200 Subject: [PATCH] navigator: make MissionBlock subclass of NavigatorMode --- src/modules/navigator/loiter.cpp | 3 +- src/modules/navigator/loiter.h | 2 +- src/modules/navigator/mission.cpp | 3 +- src/modules/navigator/mission.h | 2 +- src/modules/navigator/mission_block.cpp | 44 ++++++++++++------------- src/modules/navigator/mission_block.h | 11 +++---- src/modules/navigator/rtl.cpp | 3 +- src/modules/navigator/rtl.h | 2 +- 8 files changed, 32 insertions(+), 38 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f5eb1e7a5a..542483fb15 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -53,8 +53,7 @@ #include "loiter.h" Loiter::Loiter(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - MissionBlock(navigator) + MissionBlock(navigator, name) { /* load initial params */ updateParams(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 685ae61410..65ff5c31ee 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -47,7 +47,7 @@ #include "navigator_mode.h" #include "mission_block.h" -class Loiter : public NavigatorMode, MissionBlock +class Loiter : public MissionBlock { public: /** diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c09f617446..72255103bd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -58,8 +58,7 @@ #include "mission.h" Mission::Mission(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - MissionBlock(navigator), + MissionBlock(navigator, name), _param_onboard_enabled(this, "ONBOARD_EN"), _onboard_mission({0}), _offboard_mission({0}), diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 842c8c9ee8..6e47619467 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -62,7 +62,7 @@ class Navigator; -class Mission : public NavigatorMode, MissionBlock +class Mission : public MissionBlock { public: /** diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4f177d76ea..9b8d3d9c70 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -52,13 +52,13 @@ #include "mission_block.h" -MissionBlock::MissionBlock(Navigator *navigator) : +MissionBlock::MissionBlock(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _mission_item({0}), - _mission_item_valid(false), - _navigator_priv(navigator) + _mission_item_valid(false) { } @@ -70,7 +70,7 @@ bool MissionBlock::is_mission_item_reached() { if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _navigator_priv->get_vstatus()->condition_landed; + return _navigator->get_vstatus()->condition_landed; } /* TODO: count turns */ @@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached() float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative - ? _mission_item.altitude + _navigator_priv->get_home_position()->alt + ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, - _navigator_priv->get_global_position()->lat, - _navigator_priv->get_global_position()->lon, - _navigator_priv->get_global_position()->alt, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, &dist_xy, &dist_z); - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) { + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ - if (_navigator_priv->get_global_position()->alt > - altitude_amsl - _navigator_priv->get_acceptance_radius()) { + if (_navigator->get_global_position()->alt > + altitude_amsl - _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ - if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else { @@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ - if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; @@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->valid = true; sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; sp->loiter_direction = item->loiter_direction; @@ -211,25 +211,25 @@ bool MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { + if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { /* use current position */ - pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt; + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ - _navigator_priv->set_can_loiter_at_sp(true); + _navigator->set_can_loiter_at_sp(true); } if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius() + || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() || pos_sp_triplet->current.loiter_direction != 1 || pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || pos_sp_triplet->next.valid) { /* position setpoint triplet should be updated */ pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius(); + pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); pos_sp_triplet->current.loiter_direction = 1; pos_sp_triplet->previous.valid = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8726964fa3..f990027522 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -47,17 +47,17 @@ #include #include +#include "navigator_mode.h" + class Navigator; -class MissionBlock +class MissionBlock : public NavigatorMode { public: /** * Constructor - * - * @param pointer to parent class */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, const char *name); /** * Destructor @@ -101,9 +101,6 @@ public: mission_item_s _mission_item; bool _mission_item_valid; - -private: - Navigator *_navigator_priv; }; #endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index af2ff308ce..043f773a43 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -56,8 +56,7 @@ #define DELAY_SIGMA 0.01f RTL::RTL(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - MissionBlock(navigator), + MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 90299c3fad..b4b729e898 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -54,7 +54,7 @@ class Navigator; -class RTL : public NavigatorMode, MissionBlock +class RTL : public MissionBlock { public: /**