From 831a3d4ed14bc068ee086c5190af96ad809f67d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 26 Jun 2014 07:47:46 +0200 Subject: [PATCH 1/3] mtecs: improve logic readability --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 03353cbc1a..48c9079a48 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -199,8 +199,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ - BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits - BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits + BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); + BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == TECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; @@ -218,12 +218,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ? - _controlTotalEnergy.getOutputLimiter() : - *outputLimiterThrottle, - outputLimiterPitch == NULL ? - _controlEnergyDistribution.getOutputLimiter() : - *outputLimiterPitch); + bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.airspeedDerivativeSp = airspeedDerivativeSp; From 52eb49ba0bd1ea5a05845350f1b3c46f0b059a39 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:09:49 +0200 Subject: [PATCH 2/3] navigator: use common "acceptance radius" parameter for all modes --- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/navigator.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_params.c | 6 +++--- src/modules/navigator/rtl.cpp | 15 +++++++-------- src/modules/navigator/rtl.h | 1 - src/modules/navigator/rtl_params.c | 12 ------------ 7 files changed, 15 insertions(+), 29 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8ef7be4496..4f177d76ea 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ if (_navigator_priv->get_global_position()->alt > - altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) { + altitude_amsl - _navigator_priv->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_takeoff_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) { _waypoint_position_reached = true; } } else { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 4709f71965..184ecd365f 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -115,7 +115,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } + float get_acceptance_radius() { return _param_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } private: @@ -165,7 +165,7 @@ private: bool _update_triplet; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ - control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */ + control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e22c4d72df..546602741f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,7 +123,7 @@ Navigator::Navigator() : _rtl(this, "RTL"), _update_triplet(false), _param_loiter_radius(this, "LOITER_RAD"), - _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") + _param_acceptance_radius(this, "ACC_RAD") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ca31adecc6..084afe3401 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,12 +55,12 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** - * Takeoff Acceptance Radius (FW only) + * Acceptance Radius * - * Acceptance radius for fixedwing. + * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters * @min 1.0 * @group Mission */ -PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b7961a260c..af2ff308ce 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *name) : _rtl_state(RTL_STATE_NONE), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY"), - _param_acceptance_radius(this, "ACCEPT_RAD") + _param_land_delay(this, "LAND_DELAY") { /* load initial params */ updateParams(); @@ -147,7 +146,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -178,7 +177,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -198,7 +197,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = false; @@ -220,7 +219,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = autoland; @@ -246,7 +245,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -265,7 +264,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_IDLE; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c4286b391f..90299c3fad 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -105,7 +105,6 @@ private: control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; - control::BlockParamFloat _param_acceptance_radius; }; #endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 63724f4612..bfe6ce7e18 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); - -/** - * RTL acceptance radius - * - * Acceptance radius for waypoints set for RTL - * - * @unit meters - * @min 1 - * @max - * @group RTL - */ -PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f); From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:34:19 +0200 Subject: [PATCH 3/3] 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: /**