From 0941ae7579f42a8baf0790181ec9a0f8de2a2317 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 2 Nov 2022 15:09:02 +0100 Subject: [PATCH] Navigator: remove MIS_LTRMIN_ALT param, and use new param MIS_LND_ABRT_ALT for landing abort MIS_LTRMIN_ALt was used to limit the go-to altitude of a LOITER_TO_ALT (not the exit altitude, but the altitude that the vehicle went to to fly to WP), and during landing abort to climb to at least this altitude. The min entry altitude of LOITER_TO_ALT I remove with this commit, while for the min alt during abort I added the new parameter MIS_LND_ABRT_ALT. Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/1030_plane | 1 - .../init.d-posix/airframes/1033_rascal | 1 - .../airframes/1034_rascal-electric | 1 - .../init.d-posix/airframes/1035_techpod | 1 - .../init.d-posix/airframes/1036_malolo | 1 - .../init.d-posix/airframes/1037_believer | 1 - .../airframes/1039_advanced_plane | 1 - .../init.d-posix/airframes/1060_rover | 1 - .../init.d-posix/airframes/1061_r1_rover | 1 - .../init.d-posix/airframes/1062_tf-r1 | 1 - .../init.d-posix/airframes/1070_boat | 1 - .../init.d-posix/airframes/17001_tf-g1 | 1 - .../init.d-posix/airframes/17002_tf-g2 | 1 - .../init.d/airframes/1002_standard_vtol.hil | 1 - .../airframes/50000_generic_ground_vehicle | 1 - .../airframes/50003_aion_robotics_r1_rover | 1 - .../airframes/50004_nxpcup_car_dfrobot_gpx | 1 - ROMFS/px4fmu_common/init.d/rc.boat_defaults | 1 - ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 - ROMFS/px4fmu_common/init.d/rc.rover_defaults | 1 - src/modules/navigator/mission.cpp | 9 +++--- src/modules/navigator/mission_block.cpp | 10 +------ src/modules/navigator/mission_params.c | 28 +++++++++---------- src/modules/navigator/navigator.h | 6 ++-- src/modules/navigator/rtl.cpp | 9 +----- 25 files changed, 22 insertions(+), 60 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane index 560bd809e6..a5394c12b4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane @@ -41,7 +41,6 @@ param set-default FW_T_SINK_MIN 2.2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal index c7659a2371..c61e4cac65 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal @@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 param set-default MIS_DIST_WPS 10000 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric index c7659a2371..c61e4cac65 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric @@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 param set-default MIS_DIST_WPS 10000 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod index 18928b0ff4..39860cc420 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod @@ -27,7 +27,6 @@ param set-default FW_L1_PERIOD 12 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo index bd4f9da1b4..37ae5bd0d8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo @@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 param set-default MIS_DIST_WPS 10000 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer index 923687d5d6..4cf8a325ae 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer @@ -34,7 +34,6 @@ param set-default FW_T_TAS_TC 2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_advanced_plane index c421bfec04..cccb1bf50b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_advanced_plane @@ -31,7 +31,6 @@ param set-default FW_T_SINK_MIN 2.2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover index 74ed35e863..135dbbfbcf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover @@ -18,7 +18,6 @@ param set-default GND_THR_CRUISE 0.3 param set-default GND_THR_MAX 0.5 param set-default GND_THR_MIN 0 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover index c48e495d6a..2ef7a8a34d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover @@ -18,7 +18,6 @@ param set-default GND_THR_CRUISE 0.3 param set-default GND_THR_MAX 0.5 param set-default GND_THR_MIN 0 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 index 0134210800..d3cc92e2ee 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 @@ -25,7 +25,6 @@ param set-default GND_THR_CRUISE 0.3 param set-default GND_THR_MAX 0.5 param set-default GND_THR_MIN 0 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat index be68232e37..b6556b710c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat @@ -18,7 +18,6 @@ param set-default GND_THR_CRUISE 0.85 param set-default GND_THR_MAX 1 param set-default GND_THR_MIN 0 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 index 395310d75e..f733f9cf25 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 @@ -20,7 +20,6 @@ param set-default FW_W_EN 1 param set-default FW_RR_P 0.08 -param set-default MIS_LTRMIN_ALT 50 param set-default MIS_TAKEOFF_ALT 3 param set-default NAV_ACC_RAD 20 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 index 4ed9a2bbde..15875b115a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 @@ -20,7 +20,6 @@ param set-default FW_W_EN 1 param set-default FW_RR_P 0.08 -param set-default MIS_LTRMIN_ALT 50 param set-default MIS_TAKEOFF_ALT 7 param set-default NAV_ACC_RAD 20 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index e66ec9f683..48841868a7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -31,7 +31,6 @@ param set-default MC_PITCH_P 6 param set-default MC_PITCHRATE_P 0.2 param set-default MC_ROLL_P 6 param set-default MC_ROLLRATE_P 0.3 -param set-default MIS_LTRMIN_ALT 10 param set-default MIS_TAKEOFF_ALT 10 param set-default MIS_YAW_TMT 10 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle index 40a6569938..e5c4633b57 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle @@ -39,7 +39,6 @@ param set-default GND_THR_CRUISE 0.3 param set-default GND_THR_MAX 0.5 param set-default GND_THR_MIN 0 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 9835b9d4a7..ed7a7912c8 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -44,7 +44,6 @@ param set-default GND_SPEED_D 0.001 param set-default GND_SPEED_IMAX 0.125 param set-default GND_SPEED_THR_SC 1 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx index d06d3487de..d3660aa22a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx @@ -45,7 +45,6 @@ param set-default GND_SPEED_D 0.001 param set-default GND_SPEED_IMAX 0.125 param set-default GND_SPEED_THR_SC 1 -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 0.5 diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_defaults index 93c8820883..dbce64221c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.boat_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.boat_defaults @@ -13,7 +13,6 @@ param set-default MAV_TYPE 11 # # Default parameters for UGVs. # -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 2 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 8cf2cff58c..dc972e9cdf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -43,7 +43,6 @@ param set-default RTL_LAND_DELAY -1 param set-default NAV_ACC_RAD 10 param set-default MIS_DIST_WPS 5000 -param set-default MIS_LTRMIN_ALT 25 param set-default MIS_TAKEOFF_ALT 25 param set-default MIS_TKO_LAND_REQ 2 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults index cab52039d4..322af3d0b0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -13,7 +13,6 @@ param set-default MAV_TYPE 10 # # Default parameters for UGVs. # -param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 param set-default NAV_ACC_RAD 2 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ddbee0c64b..41c2854878 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1436,15 +1436,14 @@ Mission::cruising_speed_sp_update() void Mission::do_abort_landing() { - // Abort FW landing - // first climb out then loiter over intended landing location + // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT if (_mission_item.nav_cmd != NAV_CMD_LAND) { return; } const float alt_landing = get_absolute_altitude_for_item(_mission_item); - const float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), + const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), _navigator->get_global_position()->alt); // turn current landing waypoint into an indefinite loiter @@ -1469,10 +1468,10 @@ Mission::do_abort_landing() publish_navigator_mission_item(); // for logging _navigator->set_position_setpoint_triplet_updated(); - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.\t", + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", (int)(alt_sp - alt_landing)); events::send(events::ID("mission_holding_above_landing"), events::Log::Info, - "Holding at {1:.0m_v} above landing", alt_sp - alt_landing); + "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); // reset mission index to start of landing if (_land_start_available) { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 7a996efb92..1d876fe88a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -732,15 +732,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi break; 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()); - - } else { - sp->alt = _navigator->get_global_position()->alt; - } + sp->alt = _navigator->get_global_position()->alt; // FALLTHROUGH case NAV_CMD_LOITER_TIME_LIMIT: diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 584e5a9ac5..8de343dac6 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -72,21 +72,6 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); */ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); -/** - * Minimum Loiter altitude - * - * This is the minimum altitude the system will always obey. The intent is to stay out of ground effect. - * set to -1, if there shouldn't be a minimum loiter altitude - * - * @unit m - * @min -1 - * @max 80 - * @decimal 1 - * @increment 0.5 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, -1.0f); - /** * Maximal horizontal distance from home to first waypoint * @@ -172,3 +157,16 @@ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); * @group Mission */ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); + +/** + * Landing abort min altitude + * + * Minimum altitude above landing point that the vehicle will climb to after an aborted landing. + * Then vehicle will loiter in this altitude until further command is received. + * Only applies to fixed-wing vehicles. + * + * @unit m + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f191fdeda5..2b6e345dee 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -304,7 +304,7 @@ public: void geofence_breach_check(bool &have_geofence_position_data); // Param access - float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } + int get_landing_abort_min_alt() const { return _param_mis_lnd_abrt_alt.get(); } float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } @@ -447,12 +447,12 @@ private: (ParamFloat) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/ // non-navigator parameters: Mission (MIS_*) - (ParamFloat) _param_mis_ltrmin_alt, (ParamFloat) _param_mis_takeoff_alt, (ParamInt) _para_mis_takeoff_land_req, (ParamFloat) _param_mis_yaw_tmt, (ParamFloat) _param_mis_yaw_err, (ParamFloat) _param_mis_payload_delivery_timeout, - (ParamFloat) _param_lndmc_alt_max + (ParamFloat) _param_lndmc_alt_max, + (ParamInt) _param_mis_lnd_abrt_alt ) }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d4de6c18dd..028ddc3be1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -336,14 +336,7 @@ void RTL::set_rtl_item() switch (_rtl_state) { case RTL_STATE_CLIMB: { - // 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) { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - - } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - } + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; _mission_item.lat = gpos.lat; _mission_item.lon = gpos.lon;