diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 11e6a47ac5..9b5bec7a90 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -103,7 +103,6 @@ void FlightModeManager::Run() const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f); _time_stamp_last_loop = time_stamp_now; - _home_position_sub.update(); _vehicle_control_mode_sub.update(); _vehicle_land_detected_sub.update(); _vehicle_status_sub.update(); @@ -330,9 +329,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, constraints = _current_task.task->getConstraints(); } - // limit altitude according to land detector - limitAltitude(setpoint, vehicle_local_position); - if (_takeoff_status_sub.updated()) { takeoff_status_s takeoff_status; @@ -366,25 +362,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, _old_landing_gear_position = landing_gear.landing_gear; } -void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint, - const vehicle_local_position_s &vehicle_local_position) -{ - if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt - || !vehicle_local_position.z_valid || !vehicle_local_position.v_z_valid) { - // there is no altitude limitation present or the required information not available - return; - } - - // maximum altitude == minimal z-value (NED) - const float min_z = _home_position_sub.get().z + (-_param_lndmc_alt_max.get()); - - if (vehicle_local_position.z < min_z) { - // above maximum altitude, only allow downwards flight == positive vz-setpoints (NED) - setpoint.position[2] = min_z; - setpoint.velocity[2] = math::max(setpoint.velocity[2], 0.f); - } -} - FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) { // switch to the running task, nothing to do diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 1b7dab8642..d06884a47e 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -90,7 +90,6 @@ private: void start_flight_task(); void handleCommand(); void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position); - void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position); /** * Switch to a specific task (for normal usage) @@ -143,7 +142,6 @@ private: uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; uORB::SubscriptionData _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::SubscriptionData _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; @@ -155,7 +153,6 @@ private: uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)}; DEFINE_PARAMETERS( - (ParamFloat) _param_lndmc_alt_max, (ParamInt) _param_mpc_pos_mode ); }; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index b68b1255bf..121b365bb5 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -87,24 +87,6 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); */ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); -/** - * Maximum altitude for multicopters - * - * The system will obey this limit as a - * hard altitude limit. This setting will - * be consolidated with the GF_MAX_VER_DIST - * parameter. - * A negative value indicates no altitude limitation. - * - * @unit m - * @min -1 - * @max 10000 - * @decimal 2 - * @group Land Detector - * - */ -PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); - /** * Ground effect altitude for multicopters * diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 08513bfa9f..8ff00d4485 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -58,7 +58,6 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 462a577d31..15408baaff 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -119,7 +119,6 @@ Loiter::set_loiter_position() // convert mission item to current setpoint pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b80cb2b096..b0982762bd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -222,7 +222,6 @@ void Mission::setActiveMissionItems() // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } @@ -256,13 +255,11 @@ void Mission::setActiveMissionItems() if (num_found_items > 0u) { // We have a position, convert it to the setpoint and update setpoint triplet - mission_apply_limitation(next_mission_items[0u]); mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); } if (num_found_items >= 2u) { /* got next mission item, update setpoint triplet */ - mission_apply_limitation(next_mission_items[1u]); mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next); } else { @@ -439,7 +436,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite set_align_mission_item(&_mission_item, &next_mission_items[0u]); /* set position setpoint to target during the transition */ - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index bb03dd996a..6575ae196c 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -285,7 +285,6 @@ MissionBase::on_active() } } - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); reset_mission_item_reached(); @@ -490,7 +489,6 @@ void MissionBase::setEndOfMissionItems() /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; @@ -808,7 +806,6 @@ MissionBase::do_abort_landing() _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 87a9883d7e..6781b38bbd 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -877,29 +877,6 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_ item->autocontinue = true; } -void -MissionBlock::mission_apply_limitation(mission_item_s &item) -{ - // Limit altitude - const float maximum_altitude = _navigator->get_lndmc_alt_max(); - - /* do nothing if altitude max is negative */ - if (maximum_altitude > 0.0f) { - - /* absolute altitude */ - float altitude_abs = item.altitude_is_relative - ? item.altitude + _navigator->get_home_position()->alt - : item.altitude; - - /* limit altitude to maximum allowed altitude */ - if ((maximum_altitude + _navigator->get_home_position()->alt) < altitude_abs) { - item.altitude = item.altitude_is_relative ? - maximum_altitude : - maximum_altitude + _navigator->get_home_position()->alt; - } - } -} - float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index b95ff56c7f..619ea818df 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -205,11 +205,6 @@ protected: */ void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); - /** - * General function used to adjust the mission item based on vehicle specific limitations - */ - void mission_apply_limitation(mission_item_s &item); - void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, HeadingMode heading_mode) const; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1ddb1e8891..68b9f70a47 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -269,7 +269,6 @@ public: int get_takeoff_land_required() const { return _para_mis_takeoff_land_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_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } @@ -407,7 +406,6 @@ private: (ParamFloat) _param_mis_yaw_tmt, (ParamFloat) _param_mis_yaw_err, (ParamFloat) _param_mis_payload_delivery_timeout, - (ParamFloat) _param_lndmc_alt_max, (ParamInt) _param_mis_lnd_abrt_alt ) }; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index fd578c5c8c..63c876b3c5 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -349,8 +349,6 @@ void RtlDirect::set_rtl_item() } else { // Convert mission item to current position setpoint and make it valid. - mission_apply_limitation(_mission_item); - if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 597af4ad9d..6712b0598d 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -122,7 +122,6 @@ void RtlDirectMissionLand::setActiveMissionItems() (int32_t)ceilf(_rtl_alt)); _needs_climbing = false; - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; @@ -180,11 +179,9 @@ void RtlDirectMissionLand::setActiveMissionItems() } if (num_found_items > 0) { - mission_apply_limitation(next_mission_items[0u]); mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); } - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index acb16d2d62..a84b1cddc9 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -132,11 +132,9 @@ void RtlMissionFast::setActiveMissionItems() if (num_found_items > 0) { - mission_apply_limitation(next_mission_items[0u]); mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); } - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 08188a9edb..92e6aef0a1 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -126,12 +126,10 @@ void RtlMissionFastReverse::setActiveMissionItems() reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); if (success) { - mission_apply_limitation(next_mission_item); mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); } } - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 6c5af0de3d..cb467819fa 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -85,8 +85,6 @@ Takeoff::on_active() } } - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); @@ -129,7 +127,6 @@ Takeoff::set_takeoff_position() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 45aeb30143..a6acd64f8c 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -74,7 +74,6 @@ VtolTakeoff::on_active() _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, _loiter_location(0), _loiter_location(1))); _mission_item.force_heading = true; - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.disable_weather_vane = true; pos_sp_triplet->current.cruising_speed = -1.f; @@ -180,7 +179,6 @@ VtolTakeoff::set_takeoff_position() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false;