dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-11-02 15:09:02 +01:00 committed by Daniel Agar
parent ed39eb4672
commit 0941ae7579
25 changed files with 22 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1039_advanced_plane
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx
View File

@ -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

1
ROMFS/px4fmu_common/init.d/rc.boat_defaults
View File

@ -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

1
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -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

1
ROMFS/px4fmu_common/init.d/rc.rover_defaults
View File

@ -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

9
src/modules/navigator/mission.cpp
View File

@ -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<float>(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) {

10
src/modules/navigator/mission_block.cpp
View File

@ -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:

28
src/modules/navigator/mission_params.c
View File

@ -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);

6
src/modules/navigator/navigator.h
View File

@ -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<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
// non-navigator parameters: Mission (MIS_*)
(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
(ParamInt<px4::params::MIS_TKO_LAND_REQ>) _para_mis_takeoff_land_req,
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
(ParamFloat<px4::params::MIS_PD_TO>) _param_mis_payload_delivery_timeout,
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
(ParamInt<px4::params::MIS_LND_ABRT_ALT>) _param_mis_lnd_abrt_alt
)
};

9
src/modules/navigator/rtl.cpp
View File

@ -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;