diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index e3143fdf7e..6ae2f229a0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -64,7 +64,6 @@ param set-default FW_BAT_SCALE_EN 1 param set-default FW_THR_ALT_SCL 2.7 param set-default FW_T_RLL2THR 20 -param set-default LNDMC_ALT_MAX 9999 param set-default LNDMC_XY_VEL_MAX 1 param set-default LNDMC_Z_VEL_MAX 0.7 diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index ee4d971c37..b11e3e794d 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -1,5 +1,4 @@ uint64 timestamp # time since system start (microseconds) -float32 alt_max # maximum altitude in [m] that can be reached bool freefall # true if vehicle is currently in free-fall bool ground_contact # true if vehicle has ground contact but is not landed (1. stage) bool maybe_landed # true if the vehicle might have landed (2. stage) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 1901cd8ece..dfada9e30b 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -501,14 +501,14 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position) { - if (_vehicle_land_detected_sub.get().alt_max < 0.0f || !_home_position_sub.get().valid_alt + 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 + (-_vehicle_land_detected_sub.get().alt_max); + 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) diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index b0f9fdf06e..f1dce1f07f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -158,6 +158,7 @@ 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/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 34e0f49fdd..85746874f5 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -122,7 +122,6 @@ void LandDetector::Run() const bool ground_contactDetected = _ground_contact_hysteresis.get_state(); const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state(); const bool landDetected = _landed_hysteresis.get_state(); - const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : (float)INFINITY; const bool in_ground_effect = _ground_effect_hysteresis.get_state(); // publish at 1 Hz, very first time, or when the result has changed @@ -131,8 +130,7 @@ void LandDetector::Run() (_land_detected.freefall != freefallDetected) || (_land_detected.maybe_landed != maybe_landedDetected) || (_land_detected.ground_contact != ground_contactDetected) || - (_land_detected.in_ground_effect != in_ground_effect) || - (fabsf(_land_detected.alt_max - alt_max) > FLT_EPSILON)) { + (_land_detected.in_ground_effect != in_ground_effect)) { if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */ // We did take off @@ -143,7 +141,6 @@ void LandDetector::Run() _land_detected.freefall = freefallDetected; _land_detected.maybe_landed = maybe_landedDetected; _land_detected.ground_contact = ground_contactDetected; - _land_detected.alt_max = alt_max; _land_detected.in_ground_effect = in_ground_effect; _land_detected.in_descend = _get_in_descend(); _land_detected.has_low_throttle = _get_has_low_throttle(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 09b502dfa9..dc25d4fb69 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -126,11 +126,6 @@ protected: */ virtual bool _get_freefall_state() { return false; } - /** - * @return maximum altitude that can be reached - */ - virtual float _get_max_altitude() { return INFINITY; } - /** * @return true if vehicle could be in ground effect (close to ground) */ @@ -163,7 +158,6 @@ private: vehicle_land_detected_s _land_detected = { .timestamp = 0, - .alt_max = -1.0f, .freefall = false, .ground_contact = true, .maybe_landed = true, diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 15921ef30d..8cf57c95d6 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -323,16 +323,6 @@ bool MulticopterLandDetector::_get_landed_state() return _maybe_landed_hysteresis.get_state(); } -float MulticopterLandDetector::_get_max_altitude() -{ - if (_param_lndmc_alt_max.get() < 0.0f) { - return INFINITY; - - } else { - return _param_lndmc_alt_max.get(); - } -} - float MulticopterLandDetector::_get_gnd_effect_altitude() { if (_param_lndmc_alt_gnd_effect.get() < 0.0f) { diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d405cea635..5382b5024c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -76,7 +76,6 @@ protected: bool _get_horizontal_movement() override { return _horizontal_movement; } bool _get_vertical_movement() override { return _vertical_movement; } bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; } - float _get_max_altitude() override; void _set_hysteresis_factor(const int factor) override; private: @@ -140,7 +139,6 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, (ParamFloat) _param_lndmc_trig_time, - (ParamFloat) _param_lndmc_alt_max, (ParamFloat) _param_lndmc_rot_max, (ParamFloat) _param_lndmc_xy_vel_max, (ParamFloat) _param_lndmc_z_vel_max, diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 59101622cb..dd183b07d4 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -120,7 +120,6 @@ private: vehicle_land_detected_s _vehicle_land_detected { .timestamp = 0, - .alt_max = -1.0f, .freefall = false, .ground_contact = true, .maybe_landed = true, diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8a02fc9714..2ba4e854d5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -770,12 +770,11 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_ void MissionBlock::mission_apply_limitation(mission_item_s &item) { - /* - * Limit altitude - */ + // Limit altitude + const float maximum_altitude = _navigator->get_lndmc_alt_max(); /* do nothing if altitude max is negative */ - if (_navigator->get_land_detected()->alt_max > 0.0f) { + if (maximum_altitude > 0.0f) { /* absolute altitude */ float altitude_abs = item.altitude_is_relative @@ -783,17 +782,12 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) : item.altitude; /* limit altitude to maximum allowed altitude */ - if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) { + if ((maximum_altitude + _navigator->get_home_position()->alt) < 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; - + maximum_altitude : + maximum_altitude + _navigator->get_home_position()->alt; } } - - /* - * Add other limitations here - */ } float diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f585275b24..26384335f1 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -299,6 +299,7 @@ public: 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_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } float get_vtol_reverse_delay() const { return _param_reverse_delay; } @@ -329,7 +330,10 @@ private: (ParamFloat) _param_mis_takeoff_alt, (ParamBool) _param_mis_takeoff_req, (ParamFloat) _param_mis_yaw_tmt, - (ParamFloat) _param_mis_yaw_err + (ParamFloat) _param_mis_yaw_err, + + (ParamFloat) _param_lndmc_alt_max + ) struct traffic_buffer_s {