From 25c7a03a8bbcd0e2506052423911d50efc95b939 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Nov 2017 12:13:23 -0500 Subject: [PATCH] Navigator remove unnecessary calls to updateParams - the SuperBlock already does this --- src/modules/navigator/datalinkloss.cpp | 8 ----- src/modules/navigator/datalinkloss.h | 19 ++++------ src/modules/navigator/enginefailure.cpp | 8 ----- src/modules/navigator/enginefailure.h | 20 ++++------- src/modules/navigator/follow_target.cpp | 31 ++-------------- src/modules/navigator/follow_target.h | 46 ++++++++++-------------- src/modules/navigator/gpsfailure.cpp | 8 +---- src/modules/navigator/gpsfailure.h | 9 +---- src/modules/navigator/land.cpp | 21 ----------- src/modules/navigator/land.h | 16 +++------ src/modules/navigator/loiter.cpp | 21 +---------- src/modules/navigator/loiter.h | 20 +++++------ src/modules/navigator/mission.h | 4 +-- src/modules/navigator/mission_block.cpp | 22 ++++++------ src/modules/navigator/mission_block.h | 15 ++++---- src/modules/navigator/navigator.h | 16 +++++++++ src/modules/navigator/navigator_main.cpp | 7 +++- src/modules/navigator/navigator_mode.h | 6 ---- src/modules/navigator/rcloss.cpp | 8 ----- src/modules/navigator/rcloss.h | 20 ++++------- src/modules/navigator/rtl.cpp | 6 ++-- src/modules/navigator/rtl.h | 2 +- src/modules/navigator/takeoff.cpp | 35 ++++-------------- src/modules/navigator/takeoff.h | 13 +++---- 24 files changed, 107 insertions(+), 274 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 6312581026..6229fb2f63 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -54,8 +54,6 @@ #include "navigator.h" #include "datalinkloss.h" -#define DELAY_SIGMA 0.01f - DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_commsholdwaittime(this, "CH_T"), @@ -69,12 +67,6 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_numberdatalinklosses(this, "N"), _param_skipcommshold(this, "CHSK"), _dll_state(DLL_STATE_NONE) -{ - /* initial reset */ - on_inactive(); -} - -DataLinkLoss::~DataLinkLoss() { } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 31e0e39078..ef52cfc860 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -40,28 +40,21 @@ #ifndef NAVIGATOR_DATALINKLOSS_H #define NAVIGATOR_DATALINKLOSS_H -#include -#include - -#include - #include "navigator_mode.h" #include "mission_block.h" class Navigator; -class DataLinkLoss : public MissionBlock +class DataLinkLoss final : public MissionBlock { public: DataLinkLoss(Navigator *navigator, const char *name); - ~DataLinkLoss(); - - virtual void on_inactive(); - - virtual void on_activation(); + ~DataLinkLoss() = default; - virtual void on_active(); + void on_inactive() override; + void on_activation() override; + void on_active() override; private: /* Params */ @@ -82,7 +75,7 @@ private: DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, DLL_STATE_TERMINATE = 3, DLL_STATE_END = 4 - } _dll_state; + } _dll_state{DLL_STATE_NONE}; /** * Set the DLL item diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index e8cdfa2a58..c752ab3476 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -52,17 +52,9 @@ #include "navigator.h" #include "enginefailure.h" -#define DELAY_SIGMA 0.01f - EngineFailure::EngineFailure(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _ef_state(EF_STATE_NONE) -{ - /* initial reset */ - on_inactive(); -} - -EngineFailure::~EngineFailure() { } diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h index 2c48c2fce2..2a5c0941f8 100644 --- a/src/modules/navigator/enginefailure.h +++ b/src/modules/navigator/enginefailure.h @@ -40,34 +40,26 @@ #ifndef NAVIGATOR_ENGINEFAILURE_H #define NAVIGATOR_ENGINEFAILURE_H -#include -#include - -#include - #include "navigator_mode.h" #include "mission_block.h" class Navigator; -class EngineFailure : public MissionBlock +class EngineFailure final : public MissionBlock { public: EngineFailure(Navigator *navigator, const char *name); + ~EngineFailure() = default; - ~EngineFailure(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); + void on_inactive() override; + void on_activation() override; + void on_active() override; private: enum EFState { EF_STATE_NONE = 0, EF_STATE_LOITERDOWN = 1, - } _ef_state; + } _ef_state{EF_STATE_NONE}; /** * Set the DLL item diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index b756717473..8bae416912 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -60,28 +60,11 @@ constexpr float FollowTarget::_follow_position_matricies[4][9]; FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _navigator(navigator), _param_min_alt(this, "NAV_MIN_FT_HT", false), _param_tracking_dist(this, "NAV_FT_DST", false), _param_tracking_side(this, "NAV_FT_FS", false), - _param_tracking_resp(this, "NAV_FT_RS", false), - _param_yaw_auto_max(this, "MC_YAWRAUTO_MAX", false), - _follow_target_state(SET_WAIT_FOR_TARGET_POSITION), - _follow_target_position(FOLLOW_FROM_BEHIND), - _follow_target_sub(-1), - _step_time_in_ms(0.0f), - _follow_offset(OFFSET_M), - _target_updates(0), - _last_update_time(0), - _current_target_motion(), - _previous_target_motion(), - _yaw_rate(0.0F), - _responsiveness(0.0F), - _yaw_auto_max(0.0F), - _yaw_angle(0.0F) + _param_tracking_resp(this, "NAV_FT_RS", false) { - _current_target_motion = {}; - _previous_target_motion = {}; _current_vel.zero(); _step_vel.zero(); _est_target_vel.zero(); @@ -90,10 +73,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : _target_position_delta.zero(); } -FollowTarget::~FollowTarget() -{ -} - void FollowTarget::on_inactive() { reset_target_validity(); @@ -105,8 +84,6 @@ void FollowTarget::on_activation() _responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F); - _yaw_auto_max = math::radians(_param_yaw_auto_max.get()); - _follow_target_position = _param_tracking_side.get(); if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { @@ -230,11 +207,7 @@ void FollowTarget::on_active() _current_target_motion.lat, _current_target_motion.lon); - _yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); - - _yaw_rate = _wrap_pi(_yaw_rate); - - _yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max); + _yaw_rate = _wrap_pi((_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0f)); } else { _yaw_angle = _yaw_rate = NAN; diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 74c3d03ba4..853c73eeb7 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -40,24 +40,19 @@ #pragma once -#include -#include -#include -#include +#include + #include "navigator_mode.h" #include "mission_block.h" + #include -class FollowTarget : public MissionBlock +class FollowTarget final : public MissionBlock { public: FollowTarget(Navigator *navigator, const char *name); - - FollowTarget(const FollowTarget &) = delete; - FollowTarget &operator=(const FollowTarget &) = delete; - - ~FollowTarget(); + ~FollowTarget() = default; void on_inactive() override; void on_activation() override; @@ -112,23 +107,20 @@ private: }; // follow left side - Navigator *_navigator; control::BlockParamFloat _param_min_alt; control::BlockParamFloat _param_tracking_dist; control::BlockParamInt _param_tracking_side; control::BlockParamFloat _param_tracking_resp; - control::BlockParamFloat _param_yaw_auto_max; + FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION}; + int _follow_target_position{FOLLOW_FROM_BEHIND}; - FollowTargetState _follow_target_state; - int _follow_target_position; + int _follow_target_sub{-1}; + float _step_time_in_ms{0.0f}; + float _follow_offset{OFFSET_M}; - int _follow_target_sub; - float _step_time_in_ms; - float _follow_offset; - - uint64_t _target_updates; - uint64_t _last_update_time; + uint64_t _target_updates{0}; + uint64_t _last_update_time{0}; math::Vector<3> _current_vel; math::Vector<3> _step_vel; @@ -138,15 +130,14 @@ private: math::Vector<3> _target_position_delta; math::Vector<3> _filtered_target_position_delta; - follow_target_s _current_target_motion; - follow_target_s _previous_target_motion; - float _yaw_rate; - float _responsiveness; - float _yaw_auto_max; - float _yaw_angle; + follow_target_s _current_target_motion{}; + follow_target_s _previous_target_motion{}; - // Mavlink defined motion reporting capabilities + float _yaw_rate{0.0f}; + float _responsiveness{0.0f}; + float _yaw_angle{0.0f}; + // Mavlink defined motion reporting capabilities enum { POS = 0, VEL = 1, @@ -155,6 +146,7 @@ private: }; math::Matrix<3, 3> _rot_matrix; + void track_target_position(); void track_target_velocity(); bool target_velocity_valid(); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 9849fde111..b554b91ef3 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -51,19 +51,13 @@ using matrix::Eulerf; using matrix::Quatf; -static constexpr float DELAY_SIGMA = 0.01f; - GpsFailure::GpsFailure(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_loitertime(this, "LT"), _param_openlooploiter_roll(this, "R"), _param_openlooploiter_pitch(this, "P"), - _param_openlooploiter_thrust(this, "TR"), - _gpsf_state(GPSF_STATE_NONE), - _timestamp_activation(0) + _param_openlooploiter_thrust(this, "TR") { - /* initial reset */ - on_inactive(); } void diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index dcc9dce887..ddf2ce2384 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -40,18 +40,11 @@ #ifndef NAVIGATOR_GPSFAILURE_H #define NAVIGATOR_GPSFAILURE_H -#include -#include - -#include -#include -#include - #include "mission_block.h" class Navigator; -class GpsFailure : public MissionBlock +class GpsFailure final : public MissionBlock { public: GpsFailure(Navigator *navigator, const char *name); diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index ba1f50096a..76bf62c873 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -38,18 +38,6 @@ * @author Andreas Antener */ -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - #include "land.h" #include "navigator.h" @@ -58,15 +46,6 @@ Land::Land(Navigator *navigator, const char *name) : { } -Land::~Land() -{ -} - -void -Land::on_inactive() -{ -} - void Land::on_activation() { diff --git a/src/modules/navigator/land.h b/src/modules/navigator/land.h index ef17b29987..06ddca813b 100644 --- a/src/modules/navigator/land.h +++ b/src/modules/navigator/land.h @@ -41,25 +41,17 @@ #ifndef NAVIGATOR_LAND_H #define NAVIGATOR_LAND_H -#include -#include - #include "navigator_mode.h" #include "mission_block.h" -class Land : public MissionBlock +class Land final : public MissionBlock { public: Land(Navigator *navigator, const char *name); + ~Land() = default; - ~Land(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); - + void on_activation() override; + void on_active() override; }; #endif diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index c08affd272..dde78f6daa 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -39,31 +39,12 @@ * @author Anton Babushkin */ -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - #include "loiter.h" #include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_yawmode(this, "MIS_YAWMODE", false), - _loiter_pos_set(false) -{ -} - -Loiter::~Loiter() + _param_yawmode(this, "MIS_YAWMODE", false) { } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index b347549935..84240bd552 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -41,25 +41,20 @@ #ifndef NAVIGATOR_LOITER_H #define NAVIGATOR_LOITER_H -#include -#include - #include "navigator_mode.h" #include "mission_block.h" -class Loiter : public MissionBlock +class Loiter final : public MissionBlock { public: Loiter(Navigator *navigator, const char *name); + ~Loiter() = default; - ~Loiter(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); + void on_inactive() override; + void on_activation() override; + void on_active() override; + // TODO: share this with mission enum mission_yaw_mode { MISSION_YAWMODE_NONE = 0, MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, @@ -81,7 +76,8 @@ private: void set_loiter_position(); control::BlockParamInt _param_yawmode; - bool _loiter_pos_set; + + bool _loiter_pos_set{false}; }; #endif diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 671c43a72e..620318e430 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -51,8 +51,6 @@ #include -#include -#include #include #include #include @@ -66,7 +64,7 @@ class Navigator; -class Mission : public MissionBlock +class Mission final : public MissionBlock { public: Mission(Navigator *navigator, const char *name); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 722a65f493..4a1b026019 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,11 +55,7 @@ #include MissionBlock::MissionBlock(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - _param_yaw_timeout(this, "MIS_YAW_TMT", false), - _param_yaw_err(this, "MIS_YAW_ERR", false), - _param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false), - _param_reverse_delay(this, "VT_B_REV_DEL", false) + NavigatorMode(navigator, name) { } @@ -293,9 +289,11 @@ MissionBlock::is_mission_item_reached() float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); - if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) { - mission_acceptance_radius = ((velocity / _param_back_trans_dec_mss.get() / 2) * velocity) + _param_reverse_delay.get() * - velocity; + const float back_trans_dec = _navigator->get_vtol_back_trans_deceleration(); + const float reverse_delay = _navigator->get_vtol_reverse_delay(); + + if (back_trans_dec > FLT_EPSILON && velocity > FLT_EPSILON) { + mission_acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity; } @@ -328,16 +326,16 @@ MissionBlock::is_mission_item_reached() float yaw_err = _wrap_pi(_mission_item.yaw - cog); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ - if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) - || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { + if (fabsf(yaw_err) < math::radians(_navigator->get_yaw_threshold()) + || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && - (_param_yaw_timeout.get() >= FLT_EPSILON) && - (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { + (_navigator->get_yaw_timeout() >= FLT_EPSILON) && + (now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ad5023f5c6..480e37e77c 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -42,8 +42,12 @@ #define NAVIGATOR_MISSION_BLOCK_H #include "navigator_mode.h" +#include "navigation.h" -#include +#include +#include +#include +#include #include #include #include @@ -59,7 +63,7 @@ public: * Constructor */ MissionBlock(Navigator *navigator, const char *name); - ~MissionBlock() = default; + virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; MissionBlock &operator=(const MissionBlock &) = delete; @@ -130,13 +134,6 @@ protected: hrt_abstime _time_wp_reached{0}; orb_advert_t _actuator_pub{nullptr}; - - control::BlockParamFloat _param_yaw_timeout; - control::BlockParamFloat _param_yaw_err; - - // VTOL parameters - control::BlockParamFloat _param_back_trans_dec_mss; - control::BlockParamFloat _param_reverse_delay; }; #endif diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ba1efbd1ed..daca10bae6 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -250,6 +251,13 @@ public: // Param access float get_loiter_min_alt() const { return _param_loiter_min_alt.get(); } + float get_takeoff_min_alt() const { return _param_takeoff_min_alt.get(); } + float get_yaw_timeout() const { return _param_yaw_timeout.get(); } + float get_yaw_threshold() const { return _param_yaw_err.get(); } + + float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss.get(); } + float get_vtol_reverse_delay() const { return _param_reverse_delay.get(); } + bool force_vtol() const { return _vstatus.is_vtol && !_vstatus.is_rotary_wing && _param_force_vtol.get(); } private: @@ -330,7 +338,15 @@ private: control::BlockParamInt _param_traffic_avoidance_mode; /**< avoiding other aircraft is enabled */ // non-navigator parameters + // Mission (MIS_*) control::BlockParamFloat _param_loiter_min_alt; + control::BlockParamFloat _param_takeoff_min_alt; + control::BlockParamFloat _param_yaw_timeout; + control::BlockParamFloat _param_yaw_err; + + // VTOL parameters TODO: get these out of navigator + control::BlockParamFloat _param_back_trans_dec_mss; + control::BlockParamFloat _param_reverse_delay; float _mission_cruising_speed_mc{-1.0f}; float _mission_cruising_speed_fw{-1.0f}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index abbafc1b7b..a862dd30eb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -105,7 +105,12 @@ Navigator::Navigator() : _param_force_vtol(this, "FORCE_VT"), _param_traffic_avoidance_mode(this, "TRAFF_AVOID"), // non-navigator params - _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false) + _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), + _param_takeoff_min_alt(this, "MIS_TAKEOFF_ALT", false), + _param_yaw_timeout(this, "MIS_YAW_TMT", false), + _param_yaw_err(this, "MIS_YAW_ERR", false), + _param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false), + _param_reverse_delay(this, "VT_B_REV_DEL", false) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 22a76d093f..3616e8aa04 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -42,15 +42,9 @@ #ifndef NAVIGATOR_MODE_H #define NAVIGATOR_MODE_H -#include - #include #include -#include - -#include - class Navigator; class NavigatorMode : public control::SuperBlock diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 388f2a6246..d18ebf1d28 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -53,18 +53,10 @@ #include "navigator.h" #include "datalinkloss.h" -#define DELAY_SIGMA 0.01f - RCLoss::RCLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_loitertime(this, "LT"), _rcl_state(RCL_STATE_NONE) -{ - /* initial reset */ - on_inactive(); -} - -RCLoss::~RCLoss() { } diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h index bcb74d8778..8f23d5ff88 100644 --- a/src/modules/navigator/rcloss.h +++ b/src/modules/navigator/rcloss.h @@ -40,28 +40,20 @@ #ifndef NAVIGATOR_RCLOSS_H #define NAVIGATOR_RCLOSS_H -#include -#include - -#include - #include "navigator_mode.h" #include "mission_block.h" class Navigator; -class RCLoss : public MissionBlock +class RCLoss final : public MissionBlock { public: RCLoss(Navigator *navigator, const char *name); + ~RCLoss() = default; - ~RCLoss(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); + void on_inactive() override; + void on_activation() override; + void on_active() override; private: /* Params */ @@ -72,7 +64,7 @@ private: RCL_STATE_LOITER = 1, RCL_STATE_TERMINATE = 2, RCL_STATE_END = 3 - } _rcl_state; + } _rcl_state{RCL_STATE_NONE}; /** * Set the RCL item diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index f35f8f3d87..329f9e761a 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -167,7 +167,7 @@ RTL::set_rtl_item() _mission_item.origin = ORIGIN_ONBOARD; mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", - (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt)); + (int)ceilf(climb_alt), (int)ceilf(climb_alt - _navigator->get_home_position()->alt)); break; } @@ -198,7 +198,7 @@ RTL::set_rtl_item() _mission_item.origin = ORIGIN_ONBOARD; mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", - (int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt)); + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - home.alt)); break; } @@ -235,7 +235,7 @@ RTL::set_rtl_item() pos_sp_triplet->previous.valid = false; mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", - (int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt)); + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - home.alt)); break; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index ebac5f54a4..b18d8254eb 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -46,7 +46,7 @@ class Navigator; -class RTL : public MissionBlock +class RTL final : public MissionBlock { public: RTL(Navigator *navigator, const char *name); diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index edd7787ea0..94ce97213c 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -38,33 +38,11 @@ * @author Lorenz Meier -#include -#include -#include -#include - -#include -#include - -#include -#include - #include "takeoff.h" #include "navigator.h" Takeoff::Takeoff(Navigator *navigator, const char *name) : - MissionBlock(navigator, name), - _param_min_alt(this, "MIS_TAKEOFF_ALT", false) -{ -} - -Takeoff::~Takeoff() -{ -} - -void -Takeoff::on_inactive() + MissionBlock(navigator, name) { } @@ -106,10 +84,10 @@ Takeoff::set_takeoff_position() float min_abs_altitude; if (_navigator->home_position_valid()) { //only use home position if it is valid - min_abs_altitude = _navigator->get_global_position()->alt + _param_min_alt.get(); + min_abs_altitude = _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt(); } else { //e.g. flow - min_abs_altitude = _param_min_alt.get(); + min_abs_altitude = _navigator->get_takeoff_min_alt(); } // Use altitude if it has been set. If home position is invalid use min_abs_altitude @@ -120,22 +98,21 @@ Takeoff::set_takeoff_position() if (abs_altitude < min_abs_altitude) { abs_altitude = min_abs_altitude; mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); + "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); } } else { // Use home + minimum clearance but only notify. abs_altitude = min_abs_altitude; mavlink_log_info(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); + "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); } if (abs_altitude < _navigator->get_global_position()->alt) { // If the suggestion is lower than our current alt, let's not go down. abs_altitude = _navigator->get_global_position()->alt; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Already higher than takeoff altitude"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude"); } // set current mission item to takeoff diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h index 810f6efc19..75913f8adc 100644 --- a/src/modules/navigator/takeoff.h +++ b/src/modules/navigator/takeoff.h @@ -47,21 +47,16 @@ #include "navigator_mode.h" #include "mission_block.h" -class Takeoff : public MissionBlock +class Takeoff final : public MissionBlock { public: Takeoff(Navigator *navigator, const char *name); + ~Takeoff() = default; - ~Takeoff(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); + void on_activation() override; + void on_active() override; private: - control::BlockParamFloat _param_min_alt; void set_takeoff_position(); };