diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 4a237ccc5b..613e0212b2 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_module( enginefailure.cpp gpsfailure.cpp follow_target.cpp + NavigatorCore.cpp DEPENDS git_ecl ecl_geo @@ -59,4 +60,5 @@ px4_add_module( motion_planning ) -px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman) \ No newline at end of file +px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman) +px4_add_functional_gtest(SRC NavigatorModeTakeoffTest.cpp LINKLIBS modules__navigator modules__dataman) diff --git a/src/modules/navigator/FakeNavigator.h b/src/modules/navigator/FakeNavigator.h new file mode 100644 index 0000000000..3157456ec7 --- /dev/null +++ b/src/modules/navigator/FakeNavigator.h @@ -0,0 +1,17 @@ +#pragma once + +#define MODULE_NAME "navigator" + +#include + + +class FakeNavigator : public Navigator +{ +public: + FakeNavigator() : + Navigator() {}; + + virtual ~FakeNavigator() {}; + +private: +}; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp index afce886b9d..178988a202 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -34,7 +34,7 @@ #include #include "geofence_breach_avoidance.h" #include "fake_geofence.hpp" -#include "dataman_mocks.hpp" +#include #include using namespace matrix; diff --git a/src/modules/navigator/NavigatorCore.cpp b/src/modules/navigator/NavigatorCore.cpp new file mode 100644 index 0000000000..82c79a8fd5 --- /dev/null +++ b/src/modules/navigator/NavigatorCore.cpp @@ -0,0 +1,131 @@ +/*************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file NavigatorCore.cpp + * + */ + +#include "NavigatorCore.h" +#include + + +NavigatorCore::NavigatorCore() : + ModuleParams(nullptr) +{ + +} + +NavigatorCore::~NavigatorCore() +{ + +} + +bool NavigatorCore::forceVTOL() +{ + + return _status.is_vtol && (isRotaryWing() || _status.in_transition_to_fw) && _param_nav_force_vt.get(); +} + +float NavigatorCore::getHorAcceptanceRadiusMeter() +{ + if (isRotaryWing()) { + return getDefaultHorAcceptanceRadiusMeter(); + + } else { + return math::max(_pos_ctrl_status.acceptance_radius, getDefaultHorAcceptanceRadiusMeter()); + } +} + +float NavigatorCore::getAltAcceptanceRadMeter() +{ + if (isFixedWing()) { + const position_setpoint_s &next_sp = getCurrentTriplet().next; + + if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { + // Use separate (tighter) altitude acceptance for clean altitude starting point before landing + return getFixedWingLandingAltAcceptanceRadius(); + } + } + + return getDefaultAltAcceptanceRadiusMeter(); +} + +float NavigatorCore::getDefaultAltAcceptanceRadiusMeter() +{ + + if (isFixedWing()) { + return getFixedWingAltAcceptanceRadiusMeter(); + + } else if (isRover()) { + return INFINITY; + + } else { + float alt_acceptance_radius = getMulticopterAltAcceptanceRadiusMeter(); + + if ((_pos_ctrl_status.timestamp > getCurrentTriplet().timestamp) + && _pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { + alt_acceptance_radius = _pos_ctrl_status.altitude_acceptance; + } + + return alt_acceptance_radius; + } +} + +float NavigatorCore::getAcceptanceRadiusMeter() +{ + + float acceptance_radius = _param_nav_acc_rad.get(); + + // for fixed-wing and rover, return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance) + if (!isRotaryWing() && PX4_ISFINITE(_pos_ctrl_status.acceptance_radius) && _pos_ctrl_status.timestamp != 0) { + + acceptance_radius = math::max(acceptance_radius, _pos_ctrl_status.acceptance_radius); + } + + return acceptance_radius; +} + +float NavigatorCore::getAltAcceptanceRadiusMeter() +{ + + if (isFixedWing()) { + const position_setpoint_s &next_sp = _triplet.next; + + if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { + // Use separate (tighter) altitude acceptance for clean altitude starting point before landing + return _param_nav_fw_altl_rad.get(); + } + } + + return getDefaultAltAcceptanceRadiusMeter(); +} diff --git a/src/modules/navigator/NavigatorCore.h b/src/modules/navigator/NavigatorCore.h new file mode 100644 index 0000000000..1137a74a21 --- /dev/null +++ b/src/modules/navigator/NavigatorCore.h @@ -0,0 +1,157 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +class NavigatorCore : public ModuleParams +{ + +public: + NavigatorCore(); + + ~NavigatorCore(); + + typedef matrix::Vector3 Vector3f; + typedef matrix::Vector2 Vector2d; + + bool getLanded() { return _landed_state.landed; } + bool getMaybeLanded() { return _landed_state.maybe_landed; } + float getLandDetectedAltMaxMeter() { return _landed_state.alt_max; } + double getLatRad() { return _global_pos.lat; } + double getLonRad() { return _global_pos.lon;} + float getAltitudeAMSLMeters() { return _global_pos.alt; } + float getTrueHeadingRad() { return _local_pos.heading; } + double getHomeLatRad() { return _home.lat; } + double getHomeLonRad() { return _home.lon; } + float getHomeAltAMSLMeter() { return _home.alt; } + float getHomeTrueHeadingRad() { return _home.yaw; } + home_position_s &getHomePosition() { return _home; } + + bool isNotArmed() { return _status.arming_state != vehicle_status_s::ARMING_STATE_ARMED; } + uint8_t getArmingState() { return _status.arming_state; } + + + float getPosNorthMeter() { return _local_pos.x; } + float getPosEastMeter() { return _local_pos.y; } + float getPosDownMeter() { return _local_pos.z; } + + float getVelNorthMPS() { return _local_pos.vx; } + float getVelEastMPS() { return _local_pos.vy; } + float getVelDownMPS() { return _local_pos.vz; } + + float getLoiterRadiusMeter() { return _param_nav_loiter_rad.get(); } + + bool getIsInTransitionMode() { return _status.in_transition_mode; } + + position_setpoint_triplet_s getCurrentTriplet() { return _triplet; } + + bool isHomeValid() { return (_home.timestamp > 0 && _home.valid_alt && _home.valid_hpos && _home.valid_lpos); } + bool isHomeAltValid() { return (_home.timestamp > 0 && _home.valid_alt); } + + bool isRotaryWing() { return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; } + bool isFixedWing() { return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; } + bool isRover() { return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER; } + bool isVTOL() { return _status.is_vtol; } + uint8_t getVehicleType() { return _status.vehicle_type; } + + float getDefaultAltAcceptanceRadiusMeter(); + float getAltAcceptanceRadMeter(); + float getFixedWingLandingAltAcceptanceRadius() { return _param_nav_fw_altl_rad.get(); } + float getDefaultHorAcceptanceRadiusMeter() { return _param_nav_acc_rad.get(); } + float getHorAcceptanceRadiusMeter(); + float getMulticopterAltAcceptanceRadiusMeter() { return _param_nav_mc_alt_rad.get(); } + float getFixedWingAltAcceptanceRadiusMeter() { return _param_nav_fw_alt_rad.get(); } + + float getAcceptanceRadiusMeter(); + float getAltAcceptanceRadiusMeter(); + + float getRelativeTakeoffMinAltitudeMeter() { return _param_mis_takeoff_alt.get(); } + float getRelativeLoiterMinAltitudeMeter() { return _param_mis_ltrmin_alt.get(); } + bool isTakeoffRequired() { return _param_mis_takeoff_req.get(); } + float getWaypointHeadingTimeoutSeconds() { return _param_mis_yaw_tmt.get(); } + float getWaypointHeadingAcceptanceRad() { return _param_mis_yaw_err.get(); } + + + bool forceVTOL(); + + void updateLocalPosition(const vehicle_local_position_s &local_pos) { _local_pos = local_pos; }; + void updateVehicleStatus(const vehicle_status_s &status) { _status = status; } + void updateGlobalPosition(const vehicle_global_position_s &global_pos) { _global_pos = global_pos; } + void updateLandedState(const vehicle_land_detected_s &landed_state) { _landed_state = landed_state; } + void updateHomePosition(const home_position_s &home) { _home = home; } + void updatePositionControllerStatus(const position_controller_status_s &pos_ctrl_status) { _pos_ctrl_status = pos_ctrl_status; } + +private: + + vehicle_local_position_s _local_pos{}; + vehicle_status_s _status{}; + vehicle_global_position_s _global_pos{}; + vehicle_land_detected_s _landed_state{}; + home_position_s _home{}; + position_controller_status_s _pos_ctrl_status{}; + + position_setpoint_triplet_s _triplet{}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_loiter_rad, /**< loiter radius for fixedwing */ + (ParamFloat) _param_nav_acc_rad, /**< acceptance for takeoff */ + (ParamFloat) + _param_nav_fw_alt_rad, /**< acceptance radius for fixedwing altitude */ + (ParamFloat) + _param_nav_fw_altl_rad, /**< acceptance radius for fixedwing altitude before landing*/ + (ParamFloat) + _param_nav_mc_alt_rad, /**< acceptance radius for multicopter altitude */ + (ParamInt) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */ + (ParamInt) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */ + (ParamFloat) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/ + (ParamFloat) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/ + + // non-navigator parameters + // Mission (MIS_*) + (ParamFloat) _param_mis_ltrmin_alt, + (ParamFloat) _param_mis_takeoff_alt, + (ParamBool) _param_mis_takeoff_req, + (ParamFloat) _param_mis_yaw_tmt, + (ParamFloat) _param_mis_yaw_err + ) +}; diff --git a/src/modules/navigator/NavigatorModeTakeoffTest.cpp b/src/modules/navigator/NavigatorModeTakeoffTest.cpp new file mode 100644 index 0000000000..7c1d94027c --- /dev/null +++ b/src/modules/navigator/NavigatorModeTakeoffTest.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include +#include "FakeNavigator.h" +#include "takeoff.h" +#include + + +TEST(NavigatorModeTakeoffTest, TestTakeoff) +{ + FakeNavigator fake_navigator; + + NavigatorCore &navigator_core = fake_navigator.getCore(); + Takeoff takeoff(&fake_navigator, navigator_core); + fake_navigator.params_update(); + + vehicle_status_s status = {}; + status.timestamp = 1e6; + status.is_vtol = true; + status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + vehicle_global_position_s global_pos = {}; + global_pos.timestamp = 1e6; + global_pos.lat = 40; + global_pos.lon = 3; + global_pos.alt = 300; + + vehicle_land_detected_s landed = {}; + landed.landed = true; + landed.timestamp = 1e6; + + home_position_s home = {}; + home.timestamp = 1e6; + home.lat = global_pos.lat; + home.lon = global_pos.lon; + home.alt = global_pos.alt; + home.valid_alt = true; + home.valid_hpos = true; + home.valid_lpos = true; + + vehicle_local_position_s local_pos = {}; + local_pos.timestamp = 1e6; + local_pos.x = -0.2f; + local_pos.y = 0.1f; + local_pos.z = 0.1f; + + local_pos.vx = local_pos.vy = local_pos.vz = 0; + local_pos.heading = -0.54f; + + navigator_core.updateGlobalPosition(global_pos); + navigator_core.updateHomePosition(home); + navigator_core.updateLandedState(landed); + navigator_core.updateLocalPosition(local_pos); + navigator_core.updateVehicleStatus(status); + + takeoff.on_activation(); + + position_setpoint_s current = fake_navigator.get_position_setpoint_triplet()->current; + + ASSERT_EQ(current.lat, navigator_core.getLatRad()); + ASSERT_EQ(current.lon, navigator_core.getLonRad()); + ASSERT_EQ(current.type, 3); // position_setpoint_s::TYPE_TAKEOFF + ASSERT_EQ(current.valid, true); + ASSERT_EQ(current.alt_valid, true); + ASSERT_EQ(current.alt, global_pos.alt + navigator_core.getRelativeTakeoffMinAltitudeMeter()); + ASSERT_EQ(current.yaw, navigator_core.getTrueHeadingRad()); + ASSERT_EQ(current.yaw_valid, true); + ASSERT_EQ(current.yawspeed_valid, false); + ASSERT_EQ(current.loiter_radius, navigator_core.getLoiterRadiusMeter()); + ASSERT_EQ(current.loiter_direction, 1); + ASSERT_EQ(current.acceptance_radius, navigator_core.getHorAcceptanceRadiusMeter()); + ASSERT_EQ(current.cruising_speed, -1.0f); + + uint64_t t_first = current.timestamp; + + landed.landed = false; + navigator_core.updateLandedState(landed); + global_pos.alt += navigator_core.getRelativeTakeoffMinAltitudeMeter(); + navigator_core.updateGlobalPosition(global_pos); + + takeoff.on_active(); + current = fake_navigator.get_position_setpoint_triplet()->current; + + ASSERT_NE(t_first, current.timestamp); + ASSERT_EQ(current.type, 2); + + + + +} diff --git a/src/modules/navigator/RangeRTLTest.cpp b/src/modules/navigator/RangeRTLTest.cpp index 13efe03de4..52b26ee063 100644 --- a/src/modules/navigator/RangeRTLTest.cpp +++ b/src/modules/navigator/RangeRTLTest.cpp @@ -11,7 +11,8 @@ TEST(Navigator_and_RTL, interact_correctly) { Navigator n; - RTL rtl(&n); + NavigatorCore navigator_core; + RTL rtl(&n, navigator_core); home_position_s home_pos{}; diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index dcec92cac7..55d7ae6f85 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -52,8 +52,8 @@ #include "navigator.h" #include "enginefailure.h" -EngineFailure::EngineFailure(Navigator *navigator) : - MissionBlock(navigator), +EngineFailure::EngineFailure(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core), _ef_state(EF_STATE_NONE) { } @@ -93,15 +93,15 @@ EngineFailure::set_ef_item() case EF_STATE_LOITERDOWN: { //XXX create mission item at ground (below?) here - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.lat = _navigator_core.getLatRad(); + _mission_item.lon = _navigator_core.getLonRad(); _mission_item.altitude_is_relative = false; //XXX setting altitude to a very low value, evaluate other options - _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.altitude = _navigator_core.getHomeAltAMSLMeter() - 1000.0f; _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter(); _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h index 8ba17d8246..a023fcbef4 100644 --- a/src/modules/navigator/enginefailure.h +++ b/src/modules/navigator/enginefailure.h @@ -47,7 +47,7 @@ class Navigator; class EngineFailure : public MissionBlock { public: - EngineFailure(Navigator *navigator); + EngineFailure(Navigator *navigator, NavigatorCore &navigator_core); ~EngineFailure() = default; void on_inactive() override; diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 8e94ab81b4..23011b1419 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -60,8 +60,8 @@ using namespace matrix; constexpr float FollowTarget::_follow_position_matricies[4][9]; -FollowTarget::FollowTarget(Navigator *navigator) : - MissionBlock(navigator), +FollowTarget::FollowTarget(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core), ModuleParams(navigator) { _current_vel.zero(); @@ -134,7 +134,7 @@ void FollowTarget::on_active() // get distance to target - map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + map_projection_init(&target_ref, _navigator_core.getLatRad(), _navigator_core.getLonRad()); map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); @@ -200,12 +200,12 @@ void FollowTarget::on_active() // this really needs to control the yaw rate directly in the attitude pid controller // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode - _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, + _yaw_angle = get_bearing_to_next_waypoint(_navigator_core.getLatRad(), + _navigator_core.getLonRad(), _current_target_motion.lat, _current_target_motion.lon); - _yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f)); + _yaw_rate = wrap_pi((_yaw_angle - _navigator_core.getTrueHeadingRad()) / (dt_ms / 1000.0f)); } else { _yaw_angle = _yaw_rate = NAN; @@ -238,7 +238,7 @@ void FollowTarget::on_active() // 3 degrees of facing target if (PX4_ISFINITE(_yaw_rate)) { - if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) { + if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator_core.getTrueHeadingRad())) < math::radians(3.0F)) { _yaw_rate = NAN; } } @@ -298,8 +298,8 @@ void FollowTarget::on_active() // for now set the target at the minimum height above the uav - target.lat = _navigator->get_global_position()->lat; - target.lon = _navigator->get_global_position()->lon; + target.lat = _navigator_core.getLatRad(); + target.lon = _navigator_core.getLonRad(); target.alt = 0.0F; set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle); @@ -376,7 +376,7 @@ void FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw) { - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { /* landed, don't takeoff, but switch to IDLE mode */ item->nav_cmd = NAV_CMD_IDLE; @@ -387,7 +387,8 @@ FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clea /* use current target position */ item->lat = target.lat; item->lon = target.lon; - item->altitude = _navigator->get_home_position()->alt; + item->altitude = _navigator_core.getHomeAltAMSLMeter(); + if (min_clearance > 8.0f) { item->altitude += min_clearance; @@ -399,8 +400,8 @@ FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clea item->altitude_is_relative = false; item->yaw = yaw; - item->loiter_radius = _navigator->get_loiter_radius(); - item->acceptance_radius = _navigator->get_acceptance_radius(); + item->loiter_radius = _navigator_core.getLoiterRadiusMeter(); + item->acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); item->time_inside = 0.0f; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 25086dca8e..fdf8c5234f 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -54,7 +54,7 @@ class FollowTarget : public MissionBlock, public ModuleParams { public: - FollowTarget(Navigator *navigator); + FollowTarget(Navigator *navigator, NavigatorCore &navigator_core); ~FollowTarget() = default; void on_inactive() override; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 6f4d608cb7..9c9e453f6f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -224,13 +224,13 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) { bool inside_fence = true; - if (isHomeRequired() && _navigator->home_position_valid()) { + if (isHomeRequired() && _navigator->getCore().isHomeValid()) { const float max_horizontal_distance = _param_gf_max_hor_dist.get(); - const double home_lat = _navigator->get_home_position()->lat; - const double home_lon = _navigator->get_home_position()->lon; - const float home_alt = _navigator->get_home_position()->alt; + const double home_lat = _navigator->getCore().getHomeLatRad(); + const double home_lon = _navigator->getCore().getHomeLonRad(); + const float home_alt = _navigator->getCore().getHomeAltAMSLMeter(); float dist_xy = -1.0f; float dist_z = -1.0f; @@ -255,10 +255,10 @@ bool Geofence::isBelowMaxAltitude(float altitude) { bool inside_fence = true; - if (isHomeRequired() && _navigator->home_position_valid()) { + if (isHomeRequired() && _navigator->getCore().isHomeValid()) { const float max_vertical_distance = _param_gf_max_ver_dist.get(); - const float home_alt = _navigator->get_home_position()->alt; + const float home_alt = _navigator->getCore().getHomeAltAMSLMeter(); float dist_z = altitude - home_alt; diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index a475ae5dfd..5a4aa0a102 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -52,8 +52,8 @@ using matrix::Eulerf; using matrix::Quatf; -GpsFailure::GpsFailure(Navigator *navigator) : - MissionBlock(navigator), +GpsFailure::GpsFailure(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core), ModuleParams(navigator) { } @@ -92,7 +92,7 @@ GpsFailure::on_active() Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); q.copyTo(att_sp.q_d); - if (_navigator->get_vstatus()->is_vtol) { + if (_navigator_core.isVTOL()) { _fw_virtual_att_sp_pub.publish(att_sp); } else { diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index bfe56a71b9..e639e7f6da 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -51,7 +51,7 @@ class Navigator; class GpsFailure : public MissionBlock, public ModuleParams { public: - GpsFailure(Navigator *navigator); + GpsFailure(Navigator *navigator, NavigatorCore &navigator_core); ~GpsFailure() = default; void on_inactive() override; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 7a17c8cd6b..dbf9385d50 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -41,8 +41,8 @@ #include "land.h" #include "navigator.h" -Land::Land(Navigator *navigator) : - MissionBlock(navigator) +Land::Land(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core) { } @@ -71,16 +71,16 @@ void Land::on_active() { /* for VTOL update landing location during back transition */ - if (_navigator->get_vstatus()->is_vtol && - _navigator->get_vstatus()->in_transition_mode) { + if (_navigator_core.isVTOL() && + _navigator_core.getIsInTransitionMode()) { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.lat = _navigator_core.getLatRad(); + pos_sp_triplet->current.lon = _navigator_core.getLonRad(); _navigator->set_position_setpoint_triplet_updated(); } - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); set_idle_item(&_mission_item); diff --git a/src/modules/navigator/land.h b/src/modules/navigator/land.h index fdbc2bfec5..bbb8db69e6 100644 --- a/src/modules/navigator/land.h +++ b/src/modules/navigator/land.h @@ -46,7 +46,7 @@ class Land : public MissionBlock { public: - Land(Navigator *navigator); + Land(Navigator *navigator, NavigatorCore &navigator_core); ~Land() = default; void on_activation() override; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index fdfb0b08a9..70813db02d 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -42,8 +42,8 @@ #include "loiter.h" #include "navigator.h" -Loiter::Loiter(Navigator *navigator) : - MissionBlock(navigator), +Loiter::Loiter(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core), ModuleParams(navigator) { } @@ -73,7 +73,7 @@ Loiter::on_active() } // reset the loiter position if we get disarmed - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (_navigator_core.isNotArmed()) { _loiter_pos_set = false; } } @@ -81,8 +81,8 @@ Loiter::on_active() void Loiter::set_loiter_position() { - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && - _navigator->get_land_detected()->landed) { + if (_navigator_core.isNotArmed() && + _navigator_core.getLanded()) { // Not setting loiter position if disarmed and landed, instead mark the current // setpoint as invalid and idle (both, just to be sure). @@ -101,7 +101,8 @@ Loiter::set_loiter_position() _loiter_pos_set = true; // set current mission item to loiter - set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt()); + set_loiter_item(&_mission_item, _navigator_core.getRelativeLoiterMinAltitudeMeter()); + // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -119,7 +120,7 @@ void Loiter::reposition() { // we can't reposition if we are not armed yet - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (_navigator_core.isNotArmed()) { return; } @@ -131,10 +132,10 @@ Loiter::reposition() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->current.velocity_valid = false; - pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading; - pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt; + pos_sp_triplet->previous.yaw = _navigator_core.getTrueHeadingRad(); + pos_sp_triplet->previous.lat = _navigator_core.getLatRad(); + pos_sp_triplet->previous.lon = _navigator_core.getLonRad(); + pos_sp_triplet->previous.alt = _navigator_core.getAltitudeAMSLMeters(); memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 9d2e84cda4..30f52feb70 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -48,7 +48,7 @@ class Loiter : public MissionBlock, public ModuleParams { public: - Loiter(Navigator *navigator); + Loiter(Navigator *navigator, NavigatorCore &navigator_core); ~Loiter() = default; void on_inactive() override; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 5c9cf5fb52..e68f827ef3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -62,8 +62,8 @@ using namespace time_literals; -Mission::Mission(Navigator *navigator) : - MissionBlock(navigator), +Mission::Mission(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core), ModuleParams(navigator) { } @@ -83,7 +83,7 @@ Mission::on_inactive() } /* Without home a mission can't be valid yet anyway, let's wait. */ - if (!_navigator->home_position_valid()) { + if (!_navigator_core.isHomeValid()) { return; } @@ -131,7 +131,7 @@ Mission::on_inactive() } /* require takeoff after non-loiter or landing */ - if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) { + if (!_navigator->get_can_loiter_at_sp() || _navigator_core.getLanded()) { _need_takeoff = true; } @@ -252,7 +252,7 @@ Mission::on_active() /* see if we need to update the current yaw heading */ if (!_param_mis_mnt_yaw_ctl.get() - && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_navigator_core.isRotaryWing()) && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF @@ -330,9 +330,9 @@ Mission::set_execution_mode(const uint8_t mode) case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { // command a transition if in vtol mc mode - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - _navigator->get_vstatus()->is_vtol && - !_navigator->get_land_detected()->landed) { + if (_navigator_core.isRotaryWing() && + _navigator_core.isVTOL() && + !_navigator_core.getLanded()) { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); @@ -418,16 +418,16 @@ Mission::find_mission_land_start() _landing_start_lat = missionitem.lat; _landing_start_lon = missionitem.lon; _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + - _navigator->get_home_position()->alt : missionitem.altitude; + _navigator_core.getHomeAltAMSLMeter() : missionitem.altitude; _land_start_available = true; } - if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || + if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator_core.isVTOL()) || (missionitem.nav_cmd == NAV_CMD_LAND)) { _landing_lat = missionitem.lat; _landing_lon = missionitem.lon; - _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : + _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator_core.getHomeAltAMSLMeter() : missionitem.altitude; // don't have a valid land start yet, use the landing item itself then @@ -529,7 +529,7 @@ Mission::update_mission() * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ if (((_mission.count != old_mission.count) || (_mission.dataman_id != old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { + !_navigator_core.getLanded()) { _mission_waypoints_changed = true; } @@ -640,7 +640,7 @@ Mission::set_mission_items() /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { mavlink_log_info(_navigator->get_mavlink_log_pub(), _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed" : "Mission finished, landed."); @@ -661,7 +661,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_NONE; /* set loiter mission item and ensure that there is a minimum clearance from home */ - set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt()); + set_loiter_item(&_mission_item, _navigator_core.getRelativeTakeoffMinAltitudeMeter()); /* update position setpoint triplet */ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -683,7 +683,7 @@ Mission::set_mission_items() * https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { /* landed, refusing to take off without a mission */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff."); @@ -711,7 +711,7 @@ Mission::set_mission_items() case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { /* force vtol land */ - if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + if (_navigator_core.forceVTOL() && _mission_item.nav_cmd == NAV_CMD_LAND) { _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } @@ -733,13 +733,13 @@ Mission::set_mission_items() float takeoff_alt = calculate_takeoff_altitude(&_mission_item); mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home.", - (double)(takeoff_alt - _navigator->get_home_position()->alt)); + (double)(takeoff_alt - _navigator_core.getHomeAltAMSLMeter())); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.lat = _navigator_core.getLatRad(); + _mission_item.lon = _navigator_core.getLonRad(); /* hold heading for takeoff items */ - _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.yaw = _navigator_core.getTrueHeadingRad(); _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; @@ -748,7 +748,7 @@ Mission::set_mission_items() } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + && _navigator_core.isRotaryWing()) { /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -759,7 +759,7 @@ Mission::set_mission_items() && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_navigator_core.isRotaryWing()) { /* haven't transitioned yet, trigger vtol takeoff logic below */ _work_item_type = WORK_ITEM_TYPE_TAKEOFF; @@ -786,15 +786,15 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_TAKEOFF && new_work_item_type == WORK_ITEM_TYPE_DEFAULT && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { + _navigator_core.isRotaryWing() && + !_navigator_core.getLanded()) { /* disable weathervane before front transition for allowing yaw to align */ pos_sp_triplet->current.disable_weather_vane = true; /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _navigator_core.getLatRad(), _navigator_core.getLonRad(), _mission_item.lat, _mission_item.lon); _mission_item.force_heading = true; @@ -802,15 +802,15 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_ALIGN; /* set position setpoint to current while aligning */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.lat = _navigator_core.getLatRad(); + _mission_item.lon = _navigator_core.getLonRad(); } /* heading is aligned now, prepare transition */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_ALIGN && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { + _navigator_core.isRotaryWing() && + !_navigator_core.getLanded()) { /* re-enable weather vane again after alignment */ pos_sp_triplet->current.disable_weather_vane = false; @@ -821,7 +821,7 @@ Mission::set_mission_items() } set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.yaw = _navigator_core.getTrueHeadingRad(); /* set position setpoint to target during the transition */ generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw); @@ -842,7 +842,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && !_navigator->get_land_detected()->landed) { + && !_navigator_core.getLanded()) { new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; @@ -850,7 +850,7 @@ Mission::set_mission_items() mission_item_next_position = _mission_item; has_next_position_item = true; - float altitude = _navigator->get_global_position()->alt; + float altitude = _navigator_core.getAltitudeAMSLMeters(); if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { altitude = pos_sp_triplet->current.alt; @@ -868,11 +868,11 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_navigator->get_land_detected()->landed) { + && _navigator_core.isFixedWing() + && !_navigator_core.getLanded()) { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude = _navigator_core.getAltitudeAMSLMeters(); _mission_item.altitude_is_relative = false; new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; @@ -901,7 +901,7 @@ Mission::set_mission_items() * XXX: We might want to change that at some point if it is clear to the user * what the altitude means on this waypoint type. */ - float altitude = _navigator->get_global_position()->alt; + float altitude = _navigator_core.getAltitudeAMSLMeters(); if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { @@ -1001,8 +1001,8 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_navigator->get_land_detected()->landed + && _navigator_core.isRotaryWing() + && !_navigator_core.getLanded() && has_next_position_item) { /* disable weathervane before front transition for allowing yaw to align */ @@ -1134,9 +1134,8 @@ Mission::set_mission_items() } } else { - /* allow the vehicle to decelerate before reaching a wp with a hold time */ - const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && get_time_inside(_mission_item) > FLT_EPSILON; + const bool brake_for_hold = _navigator_core.isRotaryWing() + && get_time_inside(_mission_item) > FLT_EPSILON; //allow the vehicle to decelerate before reaching a wp with a hold time if (_mission_item.autocontinue && !brake_for_hold) { /* try to process next mission item */ @@ -1162,19 +1161,19 @@ Mission::set_mission_items() bool Mission::do_need_vertical_takeoff() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_navigator_core.isRotaryWing()) { float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { /* force takeoff if landed (additional protection) */ _need_takeoff = true; - } else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) { + } else if (_navigator_core.getAltitudeAMSLMeters() > takeoff_alt - _navigator_core.getAltAcceptanceRadiusMeter()) { /* if in-air and already above takeoff height, don't do takeoff */ _need_takeoff = false; - } else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius() + } else if (_navigator_core.getAltitudeAMSLMeters() <= takeoff_alt - _navigator_core.getAltAcceptanceRadiusMeter() && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { /* if in-air but below takeoff height and we have a takeoff item */ @@ -1200,13 +1199,13 @@ Mission::do_need_vertical_takeoff() bool Mission::do_need_move_to_land() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_navigator_core.isRotaryWing() && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _navigator_core.getLatRad(), _navigator_core.getLonRad()); - return d_current > _navigator->get_acceptance_radius(); + return d_current > _navigator_core.getAcceptanceRadiusMeter(); } return false; @@ -1215,13 +1214,13 @@ Mission::do_need_move_to_land() bool Mission::do_need_move_to_takeoff() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_navigator_core.isRotaryWing() && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _navigator_core.getLatRad(), _navigator_core.getLonRad()); - return d_current > _navigator->get_acceptance_radius(); + return d_current > _navigator_core.getAcceptanceRadiusMeter(); } return false; @@ -1236,9 +1235,9 @@ Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct posi mission_item->altitude = setpoint->alt; } else { - mission_item->lat = _navigator->get_global_position()->lat; - mission_item->lon = _navigator->get_global_position()->lon; - mission_item->altitude = _navigator->get_global_position()->alt; + mission_item->lat = _navigator_core.getLatRad(); + mission_item->lon = _navigator_core.getLonRad(); + mission_item->altitude = _navigator_core.getAltitudeAMSLMeters(); } mission_item->altitude_is_relative = false; @@ -1253,7 +1252,7 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss mission_item->autocontinue = true; mission_item->time_inside = 0.0f; mission_item->yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _navigator_core.getLatRad(), _navigator_core.getLonRad(), mission_item_next->lat, mission_item_next->lon); mission_item->force_heading = true; } @@ -1265,11 +1264,13 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) float takeoff_alt = get_absolute_altitude_for_item(*mission_item); /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ - if (_navigator->get_land_detected()->landed) { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt()); + if (_navigator_core.getLanded()) { + takeoff_alt = fmaxf(takeoff_alt, _navigator_core.getAltitudeAMSLMeters() + + _navigator_core.getRelativeTakeoffMinAltitudeMeter()); } else { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt()); + takeoff_alt = fmaxf(takeoff_alt, _navigator_core.getHomeAltAMSLMeter() + + _navigator_core.getRelativeTakeoffMinAltitudeMeter()); } return takeoff_alt; @@ -1284,11 +1285,11 @@ Mission::heading_sp_update() // Only update if current triplet is valid if (pos_sp_triplet->current.valid) { - double point_from_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon + double point_from_latlon[2] = { _navigator_core.getLatRad(), + _navigator_core.getLonRad() }; - double point_to_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon + double point_to_latlon[2] = { _navigator_core.getLatRad(), + _navigator_core.getLonRad() }; float yaw_offset = 0.0f; @@ -1327,7 +1328,7 @@ Mission::heading_sp_update() float d_current = get_distance_to_next_waypoint(point_from_latlon[0], point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); - if (d_current > _navigator->get_acceptance_radius()) { + if (d_current > _navigator_core.getAcceptanceRadiusMeter()) { float yaw = matrix::wrap_pi( get_bearing_to_next_waypoint(point_from_latlon[0], point_from_latlon[1], point_to_latlon[0], @@ -1339,7 +1340,7 @@ Mission::heading_sp_update() } else { if (!pos_sp_triplet->current.yaw_valid) { - _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.yaw = _navigator_core.getTrueHeadingRad(); pos_sp_triplet->current.yaw = _mission_item.yaw; pos_sp_triplet->current.yaw_valid = true; } @@ -1383,15 +1384,15 @@ Mission::do_abort_landing() // loiter at the larger of MIS_LTRMIN_ALT above the landing point // or 2 * FW_CLMBOUT_DIFF above the current altitude const float alt_landing = get_absolute_altitude_for_item(_mission_item); - const float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), - _navigator->get_global_position()->alt + 20.0f); + const float alt_sp = math::max(alt_landing + _navigator_core.getRelativeLoiterMinAltitudeMeter(), + _navigator_core.getAltitudeAMSLMeters() + 20.0f); // turn current landing waypoint into an indefinite loiter _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.altitude_is_relative = false; _mission_item.altitude = alt_sp; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; @@ -1659,7 +1660,7 @@ Mission::set_current_mission_item() void Mission::check_mission_valid(bool force) { - if ((!_home_inited && _navigator->home_position_valid()) || force) { + if ((!_home_inited && _navigator_core.isHomeValid()) || force) { MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); @@ -1672,7 +1673,7 @@ Mission::check_mission_valid(bool force) _navigator->get_mission_result()->seq_total = _mission.count; _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); - _home_inited = _navigator->home_position_valid(); + _home_inited = _navigator_core.isHomeValid(); // find and store landing start marker (if available) find_mission_land_start(); @@ -1733,7 +1734,7 @@ bool Mission::need_to_reset_mission() { /* reset mission state when disarmed */ - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) { + if (_navigator_core.isNotArmed() && _need_mission_reset) { _need_mission_reset = false; return true; } @@ -1745,7 +1746,7 @@ void Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw) { waypoint_from_heading_and_distance( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _navigator_core.getLatRad(), _navigator_core.getLonRad(), yaw, 1000000.0f, &(setpoint->lat), &(setpoint->lon)); setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION; @@ -1773,13 +1774,13 @@ Mission::index_closest_mission_item() const if (item_contains_position(missionitem)) { // do not consider land waypoints for a fw if (!((missionitem.nav_cmd == NAV_CMD_LAND) && - (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && - (!_navigator->get_vstatus()->is_vtol))) { + (_navigator_core.isFixedWing()) && + (!_navigator_core.isVTOL()))) { float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon, get_absolute_altitude_for_item(missionitem), - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, + _navigator_core.getLatRad(), + _navigator_core.getLonRad(), + _navigator_core.getAltitudeAMSLMeters(), &dist_xy, &dist_z); if (dist < min_dist) { @@ -1793,12 +1794,12 @@ Mission::index_closest_mission_item() const // for mission reverse also consider the home position if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { float dist = get_distance_to_point_global_wgs84( - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon, - _navigator->get_home_position()->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, + _navigator_core.getHomeLatRad(), + _navigator_core.getHomeLonRad(), + _navigator_core.getHomeAltAMSLMeter(), + _navigator_core.getLatRad(), + _navigator_core.getLonRad(), + _navigator_core.getAltitudeAMSLMeters(), &dist_xy, &dist_z); if (dist < min_dist) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 308b238728..4d3f322642 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -69,7 +69,7 @@ class Navigator; class Mission : public MissionBlock, public ModuleParams { public: - Mission(Navigator *navigator); + Mission(Navigator *navigator, NavigatorCore &navigator_core); ~Mission() override = default; void on_inactive() override; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 01910affba..52135ea408 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -56,14 +56,15 @@ using matrix::wrap_pi; -MissionBlock::MissionBlock(Navigator *navigator) : - NavigatorMode(navigator) +MissionBlock::MissionBlock(Navigator *navigator, NavigatorCore &navigator_core) : + NavigatorMode(navigator), + _navigator_core(navigator_core) { _mission_item.lat = (double)NAN; _mission_item.lon = (double)NAN; _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter(); + //_mission_item.acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -80,7 +81,7 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_LAND: /* fall through */ case NAV_CMD_VTOL_LAND: - return _navigator->get_land_detected()->landed; + return _navigator_core.getLanded(); case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: @@ -114,11 +115,11 @@ MissionBlock::is_mission_item_reached() if (int(_mission_item.params[0]) == 3) { // transition to RW requested, only accept waypoint if vehicle state has changed accordingly - return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + return _navigator_core.isRotaryWing(); } else if (int(_mission_item.params[0]) == 4) { // transition to FW requested, only accept waypoint if vehicle state has changed accordingly - return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + return _navigator_core.isFixedWing(); } else { // invalid vtol transition request @@ -136,8 +137,7 @@ MissionBlock::is_mission_item_reached() hrt_abstime now = hrt_absolute_time(); - if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { - + if (!_navigator_core.getLanded() && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; @@ -145,23 +145,22 @@ MissionBlock::is_mission_item_reached() const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item); dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, + _navigator_core.getLatRad(), + _navigator_core.getLonRad(), + _navigator_core.getAltitudeAMSLMeters(), &dist_xy, &dist_z); if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - + && _navigator_core.isRotaryWing()) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow * take-off procedures like leaving the landing gear down. */ float takeoff_alt = _mission_item.altitude_is_relative ? _mission_item.altitude : - (_mission_item.altitude - _navigator->get_home_position()->alt); + (_mission_item.altitude - _navigator_core.getHomeAltAMSLMeter()); - float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); + float altitude_acceptance_radius = _navigator_core.getAltAcceptanceRadMeter(); /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { @@ -169,26 +168,26 @@ MissionBlock::is_mission_item_reached() } /* require only altitude for takeoff for multicopter */ - if (_navigator->get_global_position()->alt > + if (_navigator_core.getAltitudeAMSLMeters() > mission_item_altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + && _navigator_core.isRover()) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ - if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { + if (dist_xy >= 0.0f && dist_xy <= _navigator_core.getHorAcceptanceRadiusMeter()) { _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->get_acceptance_radius() - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator_core.getHorAcceptanceRadiusMeter() + && dist_z <= _navigator_core.getAltAcceptanceRadMeter()) { _waypoint_position_reached = true; } - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && + } else if (_navigator_core.isFixedWing() && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { @@ -200,8 +199,8 @@ MissionBlock::is_mission_item_reached() */ // check if within loiter radius around wp, if yes then set altitude sp to mission item - if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + if (dist >= 0.0f && dist_xy <= (_navigator_core.getHorAcceptanceRadiusMeter() + fabsf(_mission_item.loiter_radius)) + && dist_z <= _navigator_core.getAltAcceptanceRadMeter()) { _waypoint_position_reached = true; } @@ -216,22 +215,22 @@ MissionBlock::is_mission_item_reached() dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, + _navigator_core.getLatRad(), + _navigator_core.getLonRad(), + _navigator_core.getAltitudeAMSLMeters(), &dist_xy, &dist_z); // check if within loiter radius around wp, if yes then set altitude sp to mission item - if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) - && dist_z <= _navigator->get_default_altitude_acceptance_radius()) { + if (dist >= 0.0f && dist_xy <= (_navigator_core.getHorAcceptanceRadiusMeter() + fabsf(_mission_item.loiter_radius)) + && dist_z <= _navigator_core.getDefaultAltAcceptanceRadiusMeter()) { curr_sp->alt = mission_item_altitude_amsl; curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; _navigator->set_position_setpoint_triplet_updated(); } - } else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + } else if (dist >= 0.f && dist_xy <= (_navigator_core.getAcceptanceRadiusMeter() + fabsf(_mission_item.loiter_radius)) + && dist_z <= _navigator_core.getAltAcceptanceRadiusMeter()) { // loitering, check if new altitude is reached, while still also having check on position _waypoint_position_reached = true; @@ -257,7 +256,7 @@ MissionBlock::is_mission_item_reached() // system position matrix::Vector2f vehicle_pos; - map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + map_projection_project(&ref_pos, _navigator_core.getLatRad(), _navigator_core.getLonRad(), &vehicle_pos(0), &vehicle_pos(1)); const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized()); @@ -279,14 +278,14 @@ MissionBlock::is_mission_item_reached() } else { /*normal mission items */ - float mission_acceptance_radius = _navigator->get_acceptance_radius(); + float mission_acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); /* for vtol back transition calculate acceptance radius based on time and ground speed */ if (_mission_item.vtol_back_transition - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + && _navigator_core.isFixedWing()) { - float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + - _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); + float velocity = sqrtf(_navigator_core.getVelNorthMPS() * _navigator_core.getVelNorthMPS() + + _navigator_core.getVelEastMPS() * _navigator_core.getVelEastMPS()); const float back_trans_dec = _navigator->get_vtol_back_trans_deceleration(); const float reverse_delay = _navigator->get_vtol_reverse_delay(); @@ -299,7 +298,7 @@ MissionBlock::is_mission_item_reached() } if (dist_xy >= 0.0f && dist_xy <= mission_acceptance_radius - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= _navigator_core.getAltAcceptanceRadiusMeter()) { _waypoint_position_reached = true; } } @@ -310,7 +309,7 @@ MissionBlock::is_mission_item_reached() } // consider yaw reached for non-rotary wing vehicles (such as fixed-wing) - if (_navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (!_navigator_core.isRotaryWing()) { _waypoint_yaw_reached = true; } } @@ -319,22 +318,22 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_navigator_core.isRotaryWing() && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) { - const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading); + const float yaw_err = wrap_pi(_mission_item.yaw - _navigator_core.getTrueHeadingRad()); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ - if (fabsf(yaw_err) < _navigator->get_yaw_threshold() - || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) { + if (fabsf(yaw_err) < _navigator_core.getWaypointHeadingAcceptanceRad() + || (_navigator_core.getWaypointHeadingTimeoutSeconds() >= 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 && - (_navigator->get_yaw_timeout() >= FLT_EPSILON) && - (now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) { + (_navigator_core.getWaypointHeadingTimeoutSeconds() >= FLT_EPSILON) && + (now - _time_wp_reached >= (hrt_abstime)_navigator_core.getWaypointHeadingTimeoutSeconds() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } @@ -365,7 +364,7 @@ MissionBlock::is_mission_item_reached() /* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set, or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */ - const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + const bool enforce_exit_heading = !_navigator_core.isRotaryWing() && next_sp.valid && curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER && @@ -379,12 +378,12 @@ MissionBlock::is_mission_item_reached() float yaw_err = 0.0f; - if (dist_current_next > 1.2f * _navigator->get_loiter_radius()) { + if (dist_current_next > 1.2f * _navigator_core.getLoiterRadiusMeter()) { // set required yaw from bearing to the next mission item - _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, + _mission_item.yaw = get_bearing_to_next_waypoint(_navigator_core.getLatRad(), + _navigator_core.getLonRad(), next_sp.lat, next_sp.lon); - const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx); + const float cog = atan2f(_navigator_core.getVelEastMPS(), _navigator_core.getVelNorthMPS()); yaw_err = wrap_pi(_mission_item.yaw - cog); @@ -392,7 +391,7 @@ MissionBlock::is_mission_item_reached() } - if (fabsf(yaw_err) < _navigator->get_yaw_threshold()) { + if (fabsf(yaw_err) < _navigator_core.getWaypointHeadingAcceptanceRad()) { exit_heading_reached = true; } @@ -489,7 +488,7 @@ MissionBlock::issue_command(const mission_item_s &item) if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) { vcmd.param5 = item.lat; vcmd.param6 = item.lon; - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; + vcmd.param7 = item.altitude + _navigator_core.getHomeAltAMSLMeter(); } else { vcmd.param5 = (double)item.params[4]; @@ -505,7 +504,7 @@ float MissionBlock::get_time_inside(const mission_item_s &item) const { if ((item.nav_cmd == NAV_CMD_WAYPOINT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) || + && _navigator_core.isRotaryWing()) || item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || item.nav_cmd == NAV_CMD_DELAY) { @@ -553,10 +552,11 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->lat = item.lat; sp->lon = item.lon; sp->alt = get_absolute_altitude_for_item(item); + sp->alt_valid = true; sp->yaw = item.yaw; sp->yaw_valid = PX4_ISFINITE(item.yaw); sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : - _navigator->get_loiter_radius(); + _navigator_core.getLoiterRadiusMeter(); sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1; if (item.acceptance_radius > 0.0f && PX4_ISFINITE(item.acceptance_radius)) { @@ -564,7 +564,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->acceptance_radius = item.acceptance_radius; } else { - sp->acceptance_radius = _navigator->get_default_acceptance_radius(); + sp->acceptance_radius = _navigator->getCore().getDefaultHorAcceptanceRadiusMeter(); } sp->cruising_speed = _navigator->get_cruising_speed(); @@ -578,9 +578,8 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi case NAV_CMD_TAKEOFF: // if already flying (armed and !landed) treat TAKEOFF like regular POSITION - if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) - && !_navigator->get_land_detected()->landed && !_navigator->get_land_detected()->maybe_landed) { - + if ((_navigator_core.getArmingState() == vehicle_status_s::ARMING_STATE_ARMED) + && !_navigator_core.getLanded() && !_navigator_core.getMaybeLanded()) { sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; } else { @@ -601,12 +600,12 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi case NAV_CMD_LOITER_TO_ALT: // initially use current altitude, and switch to mission item altitude once in loiter position - if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0 - sp->alt = math::max(_navigator->get_global_position()->alt, - _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); + if (_navigator_core.getRelativeLoiterMinAltitudeMeter() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0 + sp->alt = math::max(_navigator_core.getAltitudeAMSLMeters(), + _navigator_core.getHomeAltAMSLMeter() + _navigator_core.getRelativeLoiterMinAltitudeMeter()); } else { - sp->alt = _navigator->get_global_position()->alt; + sp->alt = _navigator_core.getAltitudeAMSLMeters(); } // FALLTHROUGH @@ -630,7 +629,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi void MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) { - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { /* landed, don't takeoff, but switch to IDLE mode */ item->nav_cmd = NAV_CMD_IDLE; @@ -647,19 +646,19 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) } else { /* use current position and use return altitude as clearance */ - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; - item->altitude = _navigator->get_global_position()->alt; + item->lat = _navigator_core.getLatRad(); + item->lon = _navigator_core.getLonRad(); + item->altitude = _navigator_core.getAltitudeAMSLMeters(); - if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { - item->altitude = _navigator->get_home_position()->alt + min_clearance; + if (min_clearance > 0.0f && item->altitude < _navigator_core.getHomeAltAMSLMeter() + min_clearance) { + item->altitude = _navigator_core.getHomeAltAMSLMeter() + min_clearance; } } item->altitude_is_relative = false; item->yaw = NAN; - item->loiter_radius = _navigator->get_loiter_radius(); - item->acceptance_radius = _navigator->get_acceptance_radius(); + item->loiter_radius = _navigator_core.getLoiterRadiusMeter(); + item->acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); item->time_inside = 0.0f; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; @@ -672,14 +671,14 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) item->nav_cmd = NAV_CMD_TAKEOFF; /* use current position */ - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; - item->yaw = _navigator->get_local_position()->heading; + item->lat = _navigator_core.getLatRad(); + item->lon = _navigator_core.getLonRad(); + item->yaw = _navigator_core.getTrueHeadingRad(); item->altitude = abs_altitude; item->altitude_is_relative = false; - item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_radius = _navigator_core.getLoiterRadiusMeter(); item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } @@ -688,7 +687,7 @@ void MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location) { /* VTOL transition to RW before landing */ - if (_navigator->force_vtol()) { + if (_navigator_core.forceVTOL()) { vehicle_command_s vcmd = {}; vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; @@ -703,19 +702,19 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio if (at_current_location) { item->lat = (double)NAN; //descend at current position item->lon = (double)NAN; //descend at current position - item->yaw = _navigator->get_local_position()->heading; + item->yaw = _navigator_core.getTrueHeadingRad(); } else { /* use home position */ - item->lat = _navigator->get_home_position()->lat; - item->lon = _navigator->get_home_position()->lon; - item->yaw = _navigator->get_home_position()->yaw; + item->lat = _navigator_core.getHomeLatRad(); + item->lon = _navigator_core.getHomeLonRad(); + item->yaw = _navigator_core.getHomeTrueHeadingRad(); } item->altitude = 0; item->altitude_is_relative = false; - item->loiter_radius = _navigator->get_loiter_radius(); - item->acceptance_radius = _navigator->get_acceptance_radius(); + item->loiter_radius = _navigator_core.getLoiterRadiusMeter(); + item->acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); item->time_inside = 0.0f; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; @@ -725,13 +724,13 @@ void MissionBlock::set_idle_item(struct mission_item_s *item) { item->nav_cmd = NAV_CMD_IDLE; - item->lat = _navigator->get_home_position()->lat; - item->lon = _navigator->get_home_position()->lon; + item->lat = _navigator_core.getHomeLatRad(); + item->lon = _navigator_core.getHomeLonRad(); item->altitude_is_relative = false; - item->altitude = _navigator->get_home_position()->alt; + item->altitude = _navigator_core.getHomeAltAMSLMeter(); item->yaw = NAN; - item->loiter_radius = _navigator->get_loiter_radius(); - item->acceptance_radius = _navigator->get_acceptance_radius(); + item->loiter_radius = _navigator_core.getLoiterRadiusMeter(); + item->acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); item->time_inside = 0.0f; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; @@ -742,7 +741,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_ { item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; item->params[0] = (float) new_mode; - item->yaw = _navigator->get_local_position()->heading; + item->yaw = _navigator_core.getTrueHeadingRad(); item->autocontinue = true; } @@ -754,18 +753,18 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) */ /* do nothing if altitude max is negative */ - if (_navigator->get_land_detected()->alt_max > 0.0f) { + if (_navigator_core.getLandDetectedAltMaxMeter() > 0.0f) { /* absolute altitude */ float altitude_abs = item.altitude_is_relative - ? item.altitude + _navigator->get_home_position()->alt + ? item.altitude + _navigator_core.getHomeAltAMSLMeter() : item.altitude; /* limit altitude to maximum allowed altitude */ - if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) { + if ((_navigator_core.getLandDetectedAltMaxMeter() + _navigator_core.getHomeAltAMSLMeter()) < altitude_abs) { item.altitude = item.altitude_is_relative ? - _navigator->get_land_detected()->alt_max : - _navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt; + _navigator_core.getLandDetectedAltMaxMeter() : + _navigator_core.getLandDetectedAltMaxMeter() + _navigator_core.getHomeAltAMSLMeter(); } } @@ -779,7 +778,7 @@ float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const { if (mission_item.altitude_is_relative) { - return mission_item.altitude + _navigator->get_home_position()->alt; + return mission_item.altitude + _navigator_core.getHomeAltAMSLMeter(); } else { return mission_item.altitude; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 2de76f1f8f..6007e29a24 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -42,6 +42,7 @@ #include "navigator_mode.h" #include "navigation.h" +#include "NavigatorCore.h" #include #include @@ -61,7 +62,7 @@ public: /** * Constructor */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, NavigatorCore &navigator_core); virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; @@ -151,5 +152,7 @@ protected: hrt_abstime _time_wp_reached{0}; + NavigatorCore &_navigator_core; + uORB::Publication _actuator_pub{ORB_ID(actuator_controls_2)}; }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 335e1ec0f3..2f0bcd6958 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -67,8 +67,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, bool warned = false; // first check if we have a valid position - const bool home_valid = _navigator->home_position_valid(); - const bool home_alt_valid = _navigator->home_alt_valid(); + const bool home_valid = _navigator->getCore().isHomeValid(); + const bool home_alt_valid = _navigator->getCore().isHomeAltValid(); if (!home_alt_valid) { failed = true; @@ -79,7 +79,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint); } - const float home_alt = _navigator->get_home_position()->alt; + const float home_alt = _navigator->getCore().getHomeAltAMSLMeter(); // check if all mission item commands are supported failed = failed || !checkMissionItemValidity(mission); @@ -87,10 +87,10 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, failed = failed || !checkGeofence(mission, home_alt, home_valid); failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned); - if (_navigator->get_vstatus()->is_vtol) { + if (_navigator->getCore().isVTOL()) { failed = failed || !checkVTOL(mission, home_alt, false); - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + } else if (_navigator->getCore().isRotaryWing()) { failed = failed || !checkRotarywing(mission, home_alt); } else { @@ -300,7 +300,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) } // check if the mission starts with a land command while the vehicle is landed - if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) { + if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->getCore().getLanded()) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing"); return false; @@ -336,7 +336,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt : missionitem.altitude - home_alt; // check if we should use default acceptance radius - float acceptance_radius = _navigator->get_default_acceptance_radius(); + float acceptance_radius = _navigator->getCore().getDefaultHorAcceptanceRadiusMeter(); if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { acceptance_radius = missionitem.acceptance_radius; @@ -408,7 +408,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } } - if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) { + if (_navigator->getCore().isTakeoffRequired() && _navigator->getCore().getLanded()) { // check for a takeoff waypoint, after the above conditions have been met // MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission // while the vehicle is flying and it does not require a takeoff waypoint @@ -653,7 +653,7 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission /* check distance from current position to item */ float dist_to_1wp = get_distance_to_next_waypoint( mission_item.lat, mission_item.lon, - _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + _navigator->getCore().getHomeLatRad(), _navigator->getCore().getHomeLonRad()); if (dist_to_1wp < max_distance) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 55f001c0a1..ea0e10791d 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -122,6 +122,8 @@ public: void publish_vehicle_cmd(vehicle_command_s *vcmd); + void params_update(); + /** * Generate an artificial traffic indication * @@ -165,39 +167,39 @@ public: const vehicle_roi_s &get_vroi() { return _vroi; } void reset_vroi() { _vroi = {}; } - bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); } - bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); } + //bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); } + //bool _navigator_core.isHomeValid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); } Geofence &get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } - float get_loiter_radius() { return _param_nav_loiter_rad.get(); } + //float _navigator_core.getLoiterRadiusMeter() { return _param_nav_loiter_rad.get(); } /** * Returns the default acceptance radius defined by the parameter */ - float get_default_acceptance_radius(); + //float get_default_acceptance_radius(); /** * Get the acceptance radius * * @return the distance at which the next waypoint should be used */ - float get_acceptance_radius(); + //float get_acceptance_radius(); /** * Get the default altitude acceptance radius (i.e. from parameters) * * @return the distance from the target altitude before considering the waypoint reached */ - float get_default_altitude_acceptance_radius(); + //float get_default_altitude_acceptance_radius(); /** * Get the altitude acceptance radius * * @return the distance from the target altitude before considering the waypoint reached */ - float get_altitude_acceptance_radius(); + //float get_altitude_acceptance_radius(); /** * Get the cruising speed @@ -290,43 +292,21 @@ public: void geofence_breach_check(bool &have_geofence_position_data); - // Param access - float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } - float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } - bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); } - float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } - float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } - float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } float get_vtol_reverse_delay() const { return _param_reverse_delay; } - bool force_vtol(); + //bool force_vtol(); void acquire_gimbal_control(); void release_gimbal_control(); + NavigatorCore &getCore() { return _navigator_core; } + private: DEFINE_PARAMETERS( - (ParamFloat) _param_nav_loiter_rad, /**< loiter radius for fixedwing */ - (ParamFloat) _param_nav_acc_rad, /**< acceptance for takeoff */ - (ParamFloat) - _param_nav_fw_alt_rad, /**< acceptance radius for fixedwing altitude */ - (ParamFloat) - _param_nav_fw_altl_rad, /**< acceptance radius for fixedwing altitude before landing*/ - (ParamFloat) - _param_nav_mc_alt_rad, /**< acceptance radius for multicopter altitude */ - (ParamInt) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */ (ParamInt) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */ (ParamFloat) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/ - (ParamFloat) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/ - - // non-navigator parameters - // Mission (MIS_*) - (ParamFloat) _param_mis_ltrmin_alt, - (ParamFloat) _param_mis_takeoff_alt, - (ParamBool) _param_mis_takeoff_req, - (ParamFloat) _param_mis_yaw_tmt, - (ParamFloat) _param_mis_yaw_err + (ParamFloat) _param_nav_traff_a_radm /**< avoidance Distance Manned*/ ) int _local_pos_sub{-1}; @@ -385,6 +365,8 @@ private: bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ + NavigatorCore _navigator_core; + NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ @@ -411,7 +393,7 @@ private: // if mission mode is inactive, this flag will be cleared after 2 seconds // update subscriptions - void params_update(); + /** * Publish a new position setpoint triplet for position controllers diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a093147f06..842e404f66 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -76,15 +76,16 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence(this), _gf_breach_avoidance(this), - _mission(this), - _loiter(this), - _takeoff(this), - _land(this), - _precland(this), - _rtl(this), - _engineFailure(this), - _gpsFailure(this), - _follow_target(this) + _navigator_core(), + _mission(this, _navigator_core), + _loiter(this, _navigator_core), + _takeoff(this, _navigator_core), + _land(this, _navigator_core), + _precland(this, _navigator_core), + _rtl(this, _navigator_core), + _engineFailure(this, _navigator_core), + _gpsFailure(this, _navigator_core), + _follow_target(this, _navigator_core) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -177,7 +178,9 @@ Navigator::run() perf_begin(_loop_perf); orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + _navigator_core.updateLocalPosition(_local_pos); orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus); + _navigator_core.updateVehicleStatus(_vstatus); if (fds[2].revents & POLLIN) { // copy mission to clear any update @@ -197,6 +200,7 @@ Navigator::run() /* global position updated */ if (_global_pos_sub.updated()) { _global_pos_sub.copy(&_global_pos); + _navigator_core.updateGlobalPosition(_global_pos); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; @@ -214,8 +218,10 @@ Navigator::run() } _land_detected_sub.update(&_land_detected); + _navigator_core.updateLandedState(_land_detected); _position_controller_status_sub.update(); _home_pos_sub.update(&_home_pos); + _navigator_core.updateHomePosition(_home_pos); while (_vehicle_command_sub.updated()) { const unsigned last_generation = _vehicle_command_sub.get_last_generation(); @@ -254,7 +260,7 @@ Navigator::run() } reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos, - home_position_valid()); + _navigator_core.isHomeValid()); } } @@ -268,7 +274,7 @@ Navigator::run() rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = _navigator_core.getLoiterRadiusMeter(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; @@ -281,7 +287,7 @@ Navigator::run() } rep->current.cruising_throttle = get_cruising_throttle(); - rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { @@ -351,11 +357,11 @@ Navigator::run() rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = _navigator_core.getLoiterRadiusMeter(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - if (home_position_valid()) { + if (_navigator_core.isHomeValid()) { rep->current.yaw = cmd.param4; rep->previous.valid = true; @@ -721,7 +727,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); } else { - test_point_distance = 2.0f * get_loiter_radius(); + test_point_distance = 2.0f * _navigator_core.getLoiterRadiusMeter(); vertical_test_point_distance = 5.0f; if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { @@ -739,7 +745,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) _gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome()); _gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome()); - if (home_position_valid()) { + if (_navigator_core.isHomeValid()) { _gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt); } @@ -802,12 +808,12 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) rep->current.lon = lointer_center_lat_lon(1); rep->current.alt = loiter_altitude_amsl; rep->current.valid = true; - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = _navigator_core.getLoiterRadiusMeter(); rep->current.alt_valid = true; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; rep->current.loiter_direction = 1; rep->current.cruising_throttle = get_cruising_throttle(); - rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter(); rep->current.cruising_speed = get_cruising_speed(); } @@ -872,36 +878,13 @@ Navigator::publish_position_setpoint_triplet() _pos_sp_triplet_updated = false; } -float -Navigator::get_default_acceptance_radius() -{ - return _param_nav_acc_rad.get(); -} +// float +// Navigator::get_default_acceptance_radius() +// { +// return _navigator_core.getDefaultHorAcceptanceRadiusMeter(); +// } -float -Navigator::get_default_altitude_acceptance_radius() -{ - if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - return _param_nav_fw_alt_rad.get(); - - } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { - return INFINITY; - - } else { - float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); - - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { - alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; - } - - return alt_acceptance_radius; - } -} - -float +/* float Navigator::get_altitude_acceptance_radius() { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { @@ -914,7 +897,7 @@ Navigator::get_altitude_acceptance_radius() } return get_default_altitude_acceptance_radius(); -} +} */ float Navigator::get_cruising_speed() @@ -973,8 +956,8 @@ Navigator::reset_position_setpoint(position_setpoint_s &sp) sp.timestamp = hrt_absolute_time(); sp.lat = static_cast(NAN); sp.lon = static_cast(NAN);; - sp.loiter_radius = get_loiter_radius(); - sp.acceptance_radius = get_default_acceptance_radius(); + sp.loiter_radius = _navigator_core.getLoiterRadiusMeter(); + sp.acceptance_radius = _navigator_core.getDefaultHorAcceptanceRadiusMeter(); sp.cruising_speed = get_cruising_speed(); sp.cruising_throttle = get_cruising_throttle(); sp.valid = false; @@ -994,7 +977,7 @@ Navigator::get_cruising_throttle() } } -float +/* float Navigator::get_acceptance_radius() { float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD @@ -1008,7 +991,7 @@ Navigator::get_acceptance_radius() } return acceptance_radius; -} +} */ float Navigator::get_yaw_acceptance(float mission_item_yaw) @@ -1270,13 +1253,13 @@ Navigator::abort_landing() return should_abort; } -bool +/* bool Navigator::force_vtol() { return _vstatus.is_vtol && (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw) && _param_nav_force_vt.get(); -} +} */ int Navigator::custom_command(int argc, char *argv[]) { diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index c0ee352ad9..93ed644194 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -59,8 +59,8 @@ #define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state -PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), +PrecLand::PrecLand(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, _navigator_core), ModuleParams(navigator) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); @@ -90,9 +90,9 @@ PrecLand::on_activation() // Check that the current position setpoint is valid, otherwise land at current position if (!pos_sp_triplet->current.valid) { PX4_WARN("Resetting landing position to current position"); - 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.lat = _navigator_core.getLatRad(); + pos_sp_triplet->current.lon = _navigator_core.getLonRad(); + pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters(); pos_sp_triplet->current.valid = true; pos_sp_triplet->current.timestamp = hrt_absolute_time(); } @@ -121,7 +121,7 @@ PrecLand::on_active() } // stop if we are landed - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { switch_to_state_done(); } @@ -198,10 +198,10 @@ PrecLand::run_state_start() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _navigator_core.getLatRad(), _navigator_core.getLonRad()); // check if we've reached the start point - if (dist < _navigator->get_acceptance_radius()) { + if (dist < _navigator_core.getAcceptanceRadiusMeter()) { if (!_point_reached_time) { _point_reached_time = hrt_absolute_time(); } @@ -235,9 +235,9 @@ PrecLand::run_state_horizontal_approach() PX4_WARN("Lost landing target while landing (horizontal approach)."); // Stay at current position for searching for the landing target - 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.lat = _navigator_core.getLatRad(); + pos_sp_triplet->current.lon = _navigator_core.getLonRad(); + pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters(); if (!switch_to_state_start()) { if (!switch_to_state_fallback()) { @@ -300,9 +300,9 @@ PrecLand::run_state_descend_above_target() PX4_WARN("Lost landing target while landing (descending)."); // Stay at current position for searching for the target - 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.lat = _navigator_core.getLatRad(); + pos_sp_triplet->current.lon = _navigator_core.getLonRad(); + pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters(); if (!switch_to_state_start()) { if (!switch_to_state_fallback()) { @@ -341,7 +341,7 @@ PrecLand::run_state_search() // target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly _target_acquired_time = hrt_absolute_time(); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - float new_alt = _navigator->get_global_position()->alt + 1.0f; + float new_alt = _navigator_core.getAltitudeAMSLMeters() + 1.0f; pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt; _navigator->set_position_setpoint_triplet_updated(); } @@ -395,7 +395,7 @@ bool PrecLand::switch_to_state_horizontal_approach() { if (check_state_conditions(PrecLandState::HorizontalApproach)) { - _approach_alt = _navigator->get_global_position()->alt; + _approach_alt = _navigator_core.getAltitudeAMSLMeters(); _point_reached_time = 0; @@ -454,9 +454,9 @@ PrecLand::switch_to_state_fallback() { PX4_WARN("Falling back to normal land."); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - 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.lat = _navigator_core.getLatRad(); + pos_sp_triplet->current.lon = _navigator_core.getLonRad(); + pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters(); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; _navigator->set_position_setpoint_triplet_updated(); @@ -557,8 +557,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y) // set a best guess for previous setpoints for smooth transition map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat, _navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1)); - _sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt; - _sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt; + _sp_pev_prev(0) = _sp_pev(0) - _navigator_core.getVelNorthMPS() * dt; + _sp_pev_prev(1) = _sp_pev(1) - _navigator_core.getVelEastMPS() * dt; } _last_slewrate_time = now; diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index 1246e3099d..7fa043afb7 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -67,7 +67,7 @@ enum class PrecLandMode { class PrecLand : public MissionBlock, public ModuleParams { public: - PrecLand(Navigator *navigator); + PrecLand(Navigator *navigator, NavigatorCore &navigator_core); ~PrecLand() override = default; void on_activation() override; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index f10dd8f951..945a45ad11 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -52,8 +52,8 @@ static constexpr float DELAY_SIGMA = 0.01f; using namespace time_literals; using namespace math; -RTL::RTL(Navigator *navigator) : - MissionBlock(navigator), +RTL::RTL(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core), ModuleParams(navigator) { } @@ -80,14 +80,14 @@ void RTL::find_RTL_destination() return; } - if (!_navigator->home_position_valid()) { + if (!_navigator_core.isHomeValid()) { return; } _destination_check_time = hrt_absolute_time(); // get home position: - home_position_s &home_landing_position = *_navigator->get_home_position(); + home_position_s &home_landing_position = _navigator_core.getHomePosition(); // get global position const vehicle_global_position_s &global_position = *_navigator->get_global_position(); @@ -110,8 +110,8 @@ void RTL::find_RTL_destination() _destination.type = RTL_DESTINATION_HOME; - const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + const bool vtol_in_rw_mode = _navigator_core.isVTOL() + && _navigator_core.isRotaryWing(); // consider the mission landing if not RTL_HOME type set @@ -237,8 +237,8 @@ void RTL::find_RTL_destination() void RTL::on_activation() { - _deny_mission_landing = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + _deny_mission_landing = _navigator_core.isVTOL() + && _navigator_core.isRotaryWing(); // output the correct message, depending on where the RTL destination is switch (_destination.type) { @@ -259,16 +259,15 @@ void RTL::on_activation() _rtl_loiter_rad = _param_rtl_loiter_rad.get(); - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_navigator_core.isRotaryWing()) { _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); } else { _rtl_alt = max(global_position.alt, max(_destination.alt, - _navigator->get_home_position()->alt + _param_rtl_return_alt.get())); + _navigator_core.getHomeAltAMSLMeter() + _param_rtl_return_alt.get())); } - - if (_navigator->get_land_detected()->landed) { + if (_navigator_core.getLanded()) { // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; @@ -343,7 +342,7 @@ void RTL::set_rtl_item() // do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, // even if current climb altitude is below (e.g. RTL immediately after take off) - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_navigator_core.isRotaryWing()) { _mission_item.nav_cmd = NAV_CMD_WAYPOINT; } else { @@ -354,8 +353,8 @@ void RTL::set_rtl_item() _mission_item.lon = gpos.lon; _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; - _mission_item.yaw = _navigator->get_local_position()->heading; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.yaw = _navigator_core.getTrueHeadingRad(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -383,7 +382,7 @@ void RTL::set_rtl_item() _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); } - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -405,18 +404,18 @@ void RTL::set_rtl_item() // Except for vtol which might be still off here and should point towards this location. const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { + if (_navigator_core.isVTOL() && (d_current > _navigator_core.getAcceptanceRadiusMeter())) { _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); } else { _mission_item.yaw = _destination.yaw; } - if (_navigator->get_vstatus()->is_vtol) { + if (_navigator->getCore().isVTOL()) { _mission_item.loiter_radius = _rtl_loiter_rad; } - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -438,8 +437,8 @@ void RTL::set_rtl_item() _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; _mission_item.yaw = _destination.yaw; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); _mission_item.autocontinue = autoland; _mission_item.origin = ORIGIN_ONBOARD; @@ -467,7 +466,7 @@ void RTL::set_rtl_item() _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -489,7 +488,7 @@ void RTL::set_rtl_item() _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.origin = ORIGIN_ONBOARD; break; } @@ -502,7 +501,7 @@ void RTL::set_rtl_item() _mission_item.yaw = _destination.yaw; _mission_item.altitude = _destination.alt; _mission_item.altitude_is_relative = false; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; @@ -552,8 +551,8 @@ void RTL::advance_rtl() const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; // vehicle is a vtol and currently in fixed wing mode - const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + const bool vtol_in_fw_mode = _navigator_core.isVTOL() + && _navigator_core.isFixedWing(); switch (_rtl_state) { case RTL_STATE_CLIMB: @@ -635,11 +634,11 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) // We choose the minimum height to be two times the distance from the land position in order to // avoid the vehicle touching the ground while still moving horizontally. const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f * - _navigator->get_acceptance_radius(); + _navigator_core.getAcceptanceRadiusMeter(); float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get(); - if (destination_dist <= _navigator->get_acceptance_radius()) { + if (destination_dist <= _navigator_core.getAcceptanceRadiusMeter()) { return_altitude_amsl = _destination.alt + 2.0f * destination_dist; } else { @@ -663,7 +662,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) void RTL::get_rtl_xy_z_speed(float &xy, float &z) { - uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type; + uint8_t vehicle_type = _navigator_core.getVehicleType(); // Caution: here be dragons! // Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index d7cb01028a..2b623e8001 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -72,7 +72,7 @@ public: RTL_DESTINATION_SAFE_POINT, }; - RTL(Navigator *navigator); + RTL(Navigator *navigator, NavigatorCore &navigator_core); ~RTL() = default; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 2bde007358..11edb172c9 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -41,8 +41,10 @@ #include "takeoff.h" #include "navigator.h" -Takeoff::Takeoff(Navigator *navigator) : - MissionBlock(navigator) +using matrix::wrap_pi; + +Takeoff::Takeoff(Navigator *navigator, NavigatorCore &navigator_core) : + MissionBlock(navigator, navigator_core) { } @@ -64,7 +66,6 @@ Takeoff::on_active() } else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - // set loiter item so position controllers stop doing takeoff logic set_loiter_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -83,22 +84,22 @@ 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 + _navigator->get_takeoff_min_alt(); + if (_navigator_core.isHomeValid()) { //only use home position if it is valid + min_abs_altitude = _navigator_core.getAltitudeAMSLMeters() + _navigator_core.getRelativeTakeoffMinAltitudeMeter(); } else { //e.g. flow - min_abs_altitude = _navigator->get_takeoff_min_alt(); + min_abs_altitude = _navigator_core.getRelativeTakeoffMinAltitudeMeter(); } // Use altitude if it has been set. If home position is invalid use min_abs_altitude - if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { + if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator_core.isHomeValid()) { abs_altitude = rep->current.alt; // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. if (abs_altitude < min_abs_altitude) { if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); + "Using minimum takeoff altitude: %.2f m", (double)_navigator_core.getRelativeTakeoffMinAltitudeMeter()); } abs_altitude = min_abs_altitude; @@ -108,13 +109,13 @@ Takeoff::set_takeoff_position() // 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)_navigator->get_takeoff_min_alt()); + "Using minimum takeoff altitude: %.2f m", (double)_navigator_core.getRelativeTakeoffMinAltitudeMeter()); } - if (abs_altitude < _navigator->get_global_position()->alt) { + if (abs_altitude < _navigator_core.getAltitudeAMSLMeters()) { // If the suggestion is lower than our current alt, let's not go down. - abs_altitude = _navigator->get_global_position()->alt; + abs_altitude = _navigator_core.getAltitudeAMSLMeters(); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude"); } diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h index 6fb5d8ea19..66370dd3b3 100644 --- a/src/modules/navigator/takeoff.h +++ b/src/modules/navigator/takeoff.h @@ -46,7 +46,7 @@ class Takeoff : public MissionBlock { public: - Takeoff(Navigator *navigator); + Takeoff(Navigator *navigator, NavigatorCore &navigator_core); ~Takeoff() = default; void on_activation() override;