From aa3af7f707dbd1aaa2573c0d0da636a76f97d40a Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 8 Feb 2023 13:22:19 +0100 Subject: [PATCH] fw_pos_control: purge L1 controller --- .../airframes/1030_gazebo-classic_plane | 2 +- .../airframes/1031_gazebo-classic_plane_cam | 2 +- .../1032_gazebo-classic_plane_catapult | 2 +- .../init.d-posix/airframes/1033_jsbsim_rascal | 2 +- .../airframes/1034_flightgear_rascal-electric | 2 +- .../airframes/1035_gazebo-classic_techpod | 4 +- .../init.d-posix/airframes/1036_jsbsim_malolo | 2 +- .../airframes/1037_gazebo-classic_believer | 2 +- .../airframes/1038_gazebo-classic_glider | 2 +- .../airframes/1039_flightgear_rascal | 2 +- .../1039_gazebo-classic_advanced_plane | 2 +- .../1040_gazebo-classic_standard_vtol | 2 +- .../airframes/1041_gazebo-classic_tailsitter | 2 +- .../airframes/1042_gazebo-classic_tiltrotor | 2 +- .../1043_gazebo-classic_standard_vtol_drop | 2 +- .../airframes/1044_gazebo-classic_plane_lidar | 2 +- .../init.d-posix/airframes/4003_gz_rc_cessna | 2 +- .../init.d/airframes/13013_deltaquad | 2 +- src/modules/fw_pos_control_l1/CMakeLists.txt | 1 - .../FixedwingPositionControl.cpp | 316 +++++------------- .../FixedwingPositionControl.hpp | 9 - .../fw_pos_control_l1_params.c | 52 --- 22 files changed, 105 insertions(+), 311 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane index 49293fdd8c..1f3c785912 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane @@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam index baf464dcd0..b5835f41c7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam @@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult index 797aaf941e..57bec2a83f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult @@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal index 0eb8c34092..5eb6f7a863 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal @@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_L1_PERIOD 25 +param set-default NPFG_PERIOD 25 param set-default FW_PR_FF 0.40 param set-default FW_PR_I 0.05 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric index 49b4973bf0..9e92e5249d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric @@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_L1_PERIOD 25 +param set-default NPFG_PERIOD 25 param set-default FW_PR_FF 0.40 param set-default FW_PR_I 0.05 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod index 6f9a4bb2af..6f6b98a06a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod @@ -10,8 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 15 - param set-default FW_P_TC 0.5 param set-default FW_PR_FF 0.40 param set-default FW_PR_I 0.05 @@ -22,7 +20,7 @@ param set-default FW_RR_FF 0.20 param set-default FW_RR_I 0.02 param set-default FW_RR_P 0.22 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_W_EN 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo index 0eb8c34092..5eb6f7a863 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo @@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_L1_PERIOD 25 +param set-default NPFG_PERIOD 25 param set-default FW_PR_FF 0.40 param set-default FW_PR_I 0.05 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer index 856bae2c22..2394ca0b2b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer @@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider index 61bacc045b..f41376de9c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider @@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal index 28d39c39e3..261f8cc1bc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal @@ -11,7 +11,7 @@ param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_L1_PERIOD 25 +param set-default NPFG_PERIOD 25 param set-default FW_PR_FF 0.40 param set-default FW_PR_I 0.05 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane index 795cfcf2e2..0d38532a39 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane @@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_ANG 8 param set-default FW_THR_LND_MAX 0 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_MAN_P_MAX 30 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol index eab595b36d..a2198bfbe7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol @@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201 param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_FF 0.2 param set-default FW_PR_P 0.9 param set-default FW_PSP_OFF 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter index 3aa2f80b0b..454ff8b8b2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter @@ -42,7 +42,7 @@ param set-default PWM_MAIN_FUNC6 201 param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_REV 96 # invert both elevons -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_I 0.2 param set-default FW_PR_P 0.2 param set-default FW_PSP_OFF 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor index a684d42675..f8c4c2fc2c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor @@ -46,7 +46,7 @@ param set-default PWM_MAIN_FUNC7 201 param set-default PWM_MAIN_FUNC8 202 param set-default PWM_MAIN_FUNC9 203 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_FF 0.2 param set-default FW_PR_P 0.9 param set-default FW_PSP_OFF 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop index 9e2eb1599a..a431d8e397 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop @@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201 param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_FF 0.2 param set-default FW_PR_P 0.9 param set-default FW_PSP_OFF 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar index cd3d37b5fb..92ac6152a8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar @@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 2e663b730c..cfda755a73 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -15,7 +15,7 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_ANG 8 -param set-default FW_L1_PERIOD 12 +param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 param set-default FW_PR_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 827d0ae971..678a251177 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -38,7 +38,7 @@ param set-default EKF2_GPS_P_GATE 10 param set-default EKF2_GPS_V_GATE 10 param set-default FW_ARSP_MODE 1 -param set-default FW_L1_PERIOD 25 +param set-default NPFG_PERIOD 25 param set-default FW_PR_FF 0.7 param set-default FW_PR_I 0.18 param set-default FW_PR_P 0.15 diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index d7deaa9870..72e1ca646d 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -41,7 +41,6 @@ px4_add_module( FixedwingPositionControl.cpp FixedwingPositionControl.hpp DEPENDS - l1 launchdetection npfg runway_takeoff diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e97d669af3..595752d03b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -100,12 +100,6 @@ FixedwingPositionControl::parameters_update() param_get(_param_handle_airspeed_trans, &_param_airspeed_trans); } - // L1 control parameters - _l1_control.set_l1_damping(_param_fw_l1_damping.get()); - _l1_control.set_l1_period(_param_fw_l1_period.get()); - _l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get())); - _l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get())); - // NPFG parameters _npfg.setPeriod(_param_npfg_period.get()); _npfg.setDamping(_param_npfg_damping.get()); @@ -399,7 +393,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, } // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained - if (!_l1_control.circle_mode() && !using_npfg_with_wind_estimate()) { + if (!_wind_valid) { /* * This error value ensures that a plane (as long as its throttle capability is * not exceeded) travels towards a waypoint (and is not pushed more and more away @@ -516,50 +510,33 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.nav_roll = _att_sp.roll_body; pos_ctrl_status.nav_pitch = _att_sp.pitch_body; - if (_l1_control.has_guidance_updated()) { - pos_ctrl_status.nav_bearing = _l1_control.nav_bearing(); + npfg_status_s npfg_status = {}; - pos_ctrl_status.target_bearing = _l1_control.target_bearing(); - pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); + npfg_status.wind_est_valid = _wind_valid; - } else { - pos_ctrl_status.nav_bearing = NAN; - pos_ctrl_status.target_bearing = NAN; - pos_ctrl_status.xtrack_error = NAN; - } + const float bearing = _npfg.getBearing(); // dont repeat atan2 calc - if (_param_fw_use_npfg.get()) { - npfg_status_s npfg_status = {}; + pos_ctrl_status.nav_bearing = bearing; + pos_ctrl_status.target_bearing = _target_bearing; + pos_ctrl_status.xtrack_error = _npfg.getTrackError(); + pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f); - npfg_status.wind_est_valid = _wind_valid; + npfg_status.lat_accel = _npfg.getLateralAccel(); + npfg_status.lat_accel_ff = _npfg.getLateralAccelFF(); + npfg_status.heading_ref = _npfg.getHeadingRef(); + npfg_status.bearing = bearing; + npfg_status.bearing_feas = _npfg.getBearingFeas(); + npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas(); + npfg_status.signed_track_error = _npfg.getTrackError(); + npfg_status.track_error_bound = _npfg.getTrackErrorBound(); + npfg_status.airspeed_ref = _npfg.getAirspeedRef(); + npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef(); + npfg_status.adapted_period = _npfg.getAdaptedPeriod(); + npfg_status.p_gain = _npfg.getPGain(); + npfg_status.time_const = _npfg.getTimeConst(); + npfg_status.timestamp = hrt_absolute_time(); - const float bearing = _npfg.getBearing(); // dont repeat atan2 calc - - pos_ctrl_status.nav_bearing = bearing; - pos_ctrl_status.target_bearing = _target_bearing; - pos_ctrl_status.xtrack_error = _npfg.getTrackError(); - pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f); - - npfg_status.lat_accel = _npfg.getLateralAccel(); - npfg_status.lat_accel_ff = _npfg.getLateralAccelFF(); - npfg_status.heading_ref = _npfg.getHeadingRef(); - npfg_status.bearing = bearing; - npfg_status.bearing_feas = _npfg.getBearingFeas(); - npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas(); - npfg_status.signed_track_error = _npfg.getTrackError(); - npfg_status.track_error_bound = _npfg.getTrackErrorBound(); - npfg_status.airspeed_ref = _npfg.getAirspeedRef(); - npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef(); - npfg_status.adapted_period = _npfg.getAdaptedPeriod(); - npfg_status.p_gain = _npfg.getPGain(); - npfg_status.time_const = _npfg.getTimeConst(); - npfg_status.timestamp = hrt_absolute_time(); - - _npfg_status_pub.publish(npfg_status); - - } else { - pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f); - } + _npfg_status_pub.publish(npfg_status); pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); @@ -996,7 +973,7 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp /* current waypoint (the one currently heading for) */ curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f); + const float acc_rad = _npfg.switchDistance(500.0f); if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { @@ -1027,15 +1004,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; } - - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - // LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER (NPFG handles this internally) - if ((dist >= 0.f) - && (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs)) - && !_param_fw_use_npfg.get()) { - // SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION - position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } } } @@ -1046,7 +1014,7 @@ void FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f); + const float acc_rad = _npfg.switchDistance(500.0f); Vector2d curr_wp{0, 0}; Vector2d prev_wp{0, 0}; @@ -1120,30 +1088,24 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1)); - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) { - // Navigate directly on position setpoint and path tangent - matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); - float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : - 0.0f; - navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed), - _wind_vel, curvature); - - } else { - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel); - } - - _att_sp.roll_body = _npfg.getRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) { + // Navigate directly on position setpoint and path tangent + matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); + float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : + 0.0f; + navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed), + _wind_vel, curvature); } else { - _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed)); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); + navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel); } + _att_sp.roll_body = _npfg.getRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, @@ -1186,18 +1148,12 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, _param_fw_airspd_min.get(), ground_speed); - if (_param_fw_use_npfg.get()) { - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - - } else { - _l1_control.navigate_heading(_target_bearing, _yaw, ground_speed); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); - } + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); + _att_sp.roll_body = _npfg.getRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; @@ -1286,21 +1242,13 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(), ground_speed); - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, - get_nav_speed_2d(ground_speed), - _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - - } else { - _l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, - get_nav_speed_2d(ground_speed)); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); - } + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, + get_nav_speed_2d(ground_speed), + _wind_vel); + _att_sp.roll_body = _npfg.getRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -1391,8 +1339,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // tune up the lateral position control guidance when on the ground if (_att_sp.fw_control_yaw_wheel) { _npfg.setPeriod(_param_rwto_l1_period.get()); - _l1_control.set_l1_period(_param_rwto_l1_period.get()); - } const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0), @@ -1407,37 +1353,20 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local); } - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed, - _wind_vel, 0.0f); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed, + _wind_vel, 0.0f); - _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); + _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - const float bearing = _npfg.getBearing(); + // use npfg's bearing to commanded course, controlled via yaw angle while on runway + const float bearing = _npfg.getBearing(); - // heading hold mode will override this bearing setpoint - _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); - - } else { - // make a fake waypoint in the direction of the takeoff bearing - const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT; - - _l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed); - - const float l1_roll_setpoint = _l1_control.get_roll_setpoint(); - _att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint); - - // use l1's nav bearing to command course, controlled via yaw angle while on runway - const float bearing = _l1_control.nav_bearing(); - - // heading hold mode will override this bearing setpoint - _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); - } + // heading hold mode will override this bearing setpoint + _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); @@ -1512,20 +1441,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo && _param_fw_laun_detcn_on.get()) { /* Launch has been detected, hence we have to control the plane. */ - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel, - 0.0f); - _att_sp.roll_body = _npfg.getRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel, + 0.0f); + _att_sp.roll_body = _npfg.getRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - } else { - // make a fake waypoint in the direction of the takeoff bearing - const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT; - _l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); - } const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); @@ -1658,39 +1580,18 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo // tune up the lateral position control guidance when on the ground const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_l1_period.get() + (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); - const float ground_roll_l1_period = flare_ramp_interpolator * _param_rwto_l1_period.get() + - (1.0f - flare_ramp_interpolator) * _param_fw_l1_period.get(); _npfg.setPeriod(ground_roll_npfg_period); - _l1_control.set_l1_period(ground_roll_l1_period); const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = _npfg.getRollSetpoint(); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _att_sp.roll_body = _npfg.getRollSetpoint(); - // use npfg's bearing to commanded course, controlled via yaw angle while on runway - _att_sp.yaw_body = _npfg.getBearing(); - - } else { - // make a fake waypoint beyond the land point in the direction of the landing approach bearing - // (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector) - - const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot( - local_position - local_approach_entrance); - - const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) * - landing_approach_vector.unit_or_zero(); - - _l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); - - // use l1's nav bearing to command course, controlled via yaw angle while on runway - _att_sp.yaw_body = _l1_control.nav_bearing(); - } + // use npfg's bearing to commanded course, controlled via yaw angle while on runway + _att_sp.yaw_body = _npfg.getBearing(); /* longitudinal guidance */ @@ -1762,26 +1663,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = _npfg.getRollSetpoint(); - - } else { - // make a fake waypoint beyond the land point in the direction of the landing approach bearing - // (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector) - - const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot( - local_position - local_approach_entrance); - - const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) * - landing_approach_vector.unit_or_zero(); - - _l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); - } + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _att_sp.roll_body = _npfg.getRollSetpoint(); /* longitudinal guidance */ @@ -1937,18 +1823,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval, Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1)); - if (_param_fw_use_npfg.get()) { - _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); - _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = _npfg.getRollSetpoint(); - calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; - - } else { - /* populate l1 control setpoint */ - _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed); - _att_sp.roll_body = _l1_control.get_roll_setpoint(); - } + _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + _att_sp.roll_body = _npfg.getRollSetpoint(); + calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } @@ -2186,12 +2065,7 @@ FixedwingPositionControl::Run() update_in_air_states(_local_pos.timestamp); // update lateral guidance timesteps for slewrates - if (_param_fw_use_npfg.get()) { - _npfg.setDt(control_interval); - - } else { - _l1_control.set_dt(control_interval); - } + _npfg.setDt(control_interval); // restore nominal TECS parameters in case changed intermittently (e.g. in landing handling) _tecs.set_speed_weight(_param_fw_t_spdweight.get()); @@ -2199,7 +2073,6 @@ FixedwingPositionControl::Run() // restore lateral-directional guidance parameters (changed in takeoff mode) _npfg.setPeriod(_param_npfg_period.get()); - _l1_control.set_l1_period(_param_fw_l1_period.get()); _att_sp.reset_integral = false; _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; @@ -2295,8 +2168,6 @@ FixedwingPositionControl::Run() } } - - _l1_control.reset_has_guidance_updated(); } perf_end(_loop_perf); @@ -2333,19 +2204,13 @@ FixedwingPositionControl::reset_landing_state() } } -bool FixedwingPositionControl::using_npfg_with_wind_estimate() const -{ - // high wind mitigation is handled by NPFG if the wind estimate is valid - return _param_fw_use_npfg.get() && _wind_valid; -} - Vector2f FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed) { Vector2f nav_speed_2d{ground_speed}; - if (_airspeed_valid && !using_npfg_with_wind_estimate()) { + if (_airspeed_valid && !_wind_valid) { // l1 navigation logic breaks down when wind speed exceeds max airspeed // compute 2D groundspeed from airspeed-heading projection const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)}; @@ -2683,14 +2548,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo Vector2f current_setpoint; - if (!_param_fw_use_npfg.get()) { - if (_global_local_proj_ref.isInitialized()) { - current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon); - } - - } else { - current_setpoint = _closest_point_on_path; - } + current_setpoint = _closest_point_on_path; local_position_setpoint.x = current_setpoint(0); local_position_setpoint.y = current_setpoint(1); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index e2d42b7f0a..b2883401d7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -55,7 +55,6 @@ #include #include -#include #include #include #include @@ -415,9 +414,6 @@ private: // CLosest point on path to track matrix::Vector2f _closest_point_on_path; - // L1 guidance - lateral-directional position control - ECL_L1_Pos_Controller _l1_control; - // nonlinear path following guidance - lateral-directional position control NPFG _npfg; @@ -658,8 +654,6 @@ private: void reset_takeoff_state(); void reset_landing_state(); - bool using_npfg_with_wind_estimate() const; - /** * @brief Returns the velocity vector to be input in the lateral-directional guidance laws. * @@ -847,12 +841,9 @@ private: (ParamFloat) _param_fw_gnd_spd_min, - (ParamFloat) _param_fw_l1_damping, - (ParamFloat) _param_fw_l1_period, (ParamFloat) _param_fw_l1_r_slew_max, (ParamFloat) _param_fw_r_lim, - (ParamBool) _param_fw_use_npfg, (ParamFloat) _param_npfg_period, (ParamFloat) _param_npfg_damping, (ParamBool) _param_npfg_en_period_lb, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 650d1239a0..6eddf00155 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -31,48 +31,6 @@ * ****************************************************************************/ -/** - * @file fw_pos_control_l1_params.c - * - * Parameters defined by the L1 position control task - * - * @author Lorenz Meier - */ - -/* - * Controller parameters, accessible via MAVLink - */ - -/** - * L1 period - * - * Used to determine the L1 gain and controller time constant. This parameter is - * proportional to the L1 distance (which points ahead of the aircraft on the path - * it is following). A value of 18-25 seconds works for most aircraft. Shorten - * slowly during tuning until response is sharp without oscillation. - * - * @unit s - * @min 7.0 - * @max 50.0 - * @decimal 1 - * @increment 0.5 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); - -/** - * L1 damping - * - * Damping factor for L1 control. - * - * @min 0.6 - * @max 0.9 - * @decimal 2 - * @increment 0.05 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); - /** * L1 controller roll slew rate limit. * @@ -86,16 +44,6 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); */ PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f); -/** - * Use NPFG as lateral-directional guidance law for fixed-wing vehicles - * - * Replaces L1. - * - * @boolean - * @group FW NPFG Control - */ -PARAM_DEFINE_INT32(FW_USE_NPFG, 1); - /** * NPFG period *