From f1e6727613d835fcc622cd4af4e81ab45c637fd5 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 8 Feb 2023 13:22:19 +0100 Subject: [PATCH 1/7] 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 49293fdd8cf9..1f3c785912d7 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 baf464dcd075..b5835f41c792 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 797aaf941e87..57bec2a83f54 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 0eb8c3409225..5eb6f7a8630c 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 49b4973bf0de..9e92e5249d14 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 6f9a4bb2afb4..6f6b98a06aa4 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 0eb8c3409225..5eb6f7a8630c 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 856bae2c2271..2394ca0b2bba 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 61bacc045b30..f41376de9c55 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 28d39c39e395..261f8cc1bcfb 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 795cfcf2e2db..0d38532a3962 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 eab595b36dcd..a2198bfbe70f 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 3aa2f80b0b7b..454ff8b8b288 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 a684d426751a..f8c4c2fc2c93 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 9e2eb1599a21..a431d8e397ff 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 cd3d37b5fbfc..92ac6152a88d 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 2e663b730c22..cfda755a7376 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 827d0ae971a5..678a2511770b 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 d7deaa987004..72e1ca646dd5 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 e97d669af3e5..595752d03bfd 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(); - - pos_ctrl_status.target_bearing = _l1_control.target_bearing(); - pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); + npfg_status_s npfg_status = {}; - } else { - pos_ctrl_status.nav_bearing = NAN; - pos_ctrl_status.target_bearing = NAN; - pos_ctrl_status.xtrack_error = NAN; - } + npfg_status.wind_est_valid = _wind_valid; - if (_param_fw_use_npfg.get()) { - npfg_status_s npfg_status = {}; + const float bearing = _npfg.getBearing(); // dont repeat atan2 calc - npfg_status.wind_est_valid = _wind_valid; + 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); - const float bearing = _npfg.getBearing(); // dont repeat atan2 calc + 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(); - 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); - - _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); - - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - - // 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; + _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); - _l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed); + _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); - const float l1_roll_setpoint = _l1_control.get_roll_setpoint(); - _att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - // use l1's nav bearing to command course, controlled via yaw angle while on runway - const float bearing = _l1_control.nav_bearing(); + // 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); - } + // 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(); - - // 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(); + _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 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 e2d42b7f0a77..b2883401d795 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 @@ class FixedwingPositionControl final : public ModuleBase) _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 650d1239a0aa..6eddf0015510 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 * From 09828294a444b09a5869257ed49e0babd475a393 Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 7 Feb 2023 09:59:40 +0100 Subject: [PATCH 2/7] fw_pos_control_l1: renaming to fw_path_navigation, l1 control is not used anymore, use a more generic naming. --- boards/airmind/mindpx-v2/default.px4board | 2 +- boards/ark/fmu-v6x/default.px4board | 2 +- boards/av/x-v1/default.px4board | 2 +- boards/beaglebone/blue/default.px4board | 2 +- boards/cuav/nora/default.px4board | 2 +- boards/cuav/x7pro/default.px4board | 2 +- boards/cubepilot/cubeorange/default.px4board | 2 +- boards/cubepilot/cubeorangeplus/default.px4board | 2 +- boards/cubepilot/cubeyellow/default.px4board | 2 +- boards/emlid/navio2/default.px4board | 2 +- boards/holybro/durandal-v1/default.px4board | 2 +- boards/holybro/kakuteh7/default.px4board | 2 +- boards/holybro/kakuteh7mini/default.px4board | 2 +- boards/holybro/kakuteh7v2/default.px4board | 2 +- boards/holybro/pix32v5/default.px4board | 2 +- boards/matek/h743-mini/default.px4board | 2 +- boards/matek/h743-slim/default.px4board | 2 +- boards/matek/h743/default.px4board | 2 +- boards/modalai/fc-v1/default.px4board | 2 +- boards/modalai/fc-v2/default.px4board | 2 +- boards/mro/ctrl-zero-classic/default.px4board | 2 +- boards/mro/ctrl-zero-f7-oem/default.px4board | 2 +- boards/mro/ctrl-zero-f7/default.px4board | 2 +- boards/mro/ctrl-zero-h7-oem/default.px4board | 2 +- boards/mro/ctrl-zero-h7/default.px4board | 2 +- boards/mro/pixracerpro/default.px4board | 2 +- boards/mro/x21-777/default.px4board | 2 +- boards/mro/x21/default.px4board | 2 +- boards/nxp/fmuk66-e/default.px4board | 2 +- boards/nxp/fmuk66-v3/default.px4board | 2 +- boards/nxp/mr-canhubk3/fmu.px4board | 2 +- boards/nxp/mr-canhubk3/sysview.px4board | 2 +- boards/px4/fmu-v2/fixedwing.px4board | 2 +- boards/px4/fmu-v3/default.px4board | 2 +- boards/px4/fmu-v4/default.px4board | 2 +- boards/px4/fmu-v4pro/default.px4board | 2 +- boards/px4/fmu-v5/default.px4board | 2 +- boards/px4/fmu-v5/protected.px4board | 2 +- boards/px4/fmu-v5x/default.px4board | 2 +- boards/px4/fmu-v6c/default.px4board | 2 +- boards/px4/fmu-v6u/default.px4board | 2 +- boards/px4/fmu-v6x/default.px4board | 2 +- boards/px4/raspberrypi/default.px4board | 2 +- boards/px4/sitl/default.px4board | 2 +- boards/scumaker/pilotpi/default.px4board | 2 +- boards/sky-drones/smartap-airlink/default.px4board | 2 +- boards/thepeach/k1/default.px4board | 2 +- boards/thepeach/r1/default.px4board | 2 +- .../CMakeLists.txt | 4 ++-- .../FixedwingPositionControl.cpp | 6 +++--- .../FixedwingPositionControl.hpp | 2 +- src/modules/fw_path_navigation/Kconfig | 12 ++++++++++++ .../fw_path_navigation_params.c} | 0 .../launchdetection/CMakeLists.txt | 0 .../launchdetection/LaunchDetector.cpp | 0 .../launchdetection/LaunchDetector.h | 0 .../launchdetection/launchdetection_params.c | 0 .../runway_takeoff/CMakeLists.txt | 0 .../runway_takeoff/RunwayTakeoff.cpp | 0 .../runway_takeoff/RunwayTakeoff.h | 0 .../runway_takeoff/runway_takeoff_params.c | 0 src/modules/fw_pos_control_l1/Kconfig | 12 ------------ 62 files changed, 66 insertions(+), 66 deletions(-) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/CMakeLists.txt (96%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/FixedwingPositionControl.cpp (99%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/FixedwingPositionControl.hpp (99%) create mode 100644 src/modules/fw_path_navigation/Kconfig rename src/modules/{fw_pos_control_l1/fw_pos_control_l1_params.c => fw_path_navigation/fw_path_navigation_params.c} (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/launchdetection/CMakeLists.txt (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/launchdetection/LaunchDetector.cpp (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/launchdetection/LaunchDetector.h (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/launchdetection/launchdetection_params.c (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/runway_takeoff/CMakeLists.txt (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/runway_takeoff/RunwayTakeoff.cpp (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/runway_takeoff/RunwayTakeoff.h (100%) rename src/modules/{fw_pos_control_l1 => fw_path_navigation}/runway_takeoff/runway_takeoff_params.c (100%) delete mode 100644 src/modules/fw_pos_control_l1/Kconfig diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index 590b34575716..282c6b0f048d 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 4c02a4974212..e47dafdfc5e3 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 6c662f494d65..13001cc4f9a6 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 0214c6202130..be5f802bbb4c 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -31,7 +31,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index b722747e6ac0..ba678c35ff17 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 01dd934f97e3..e4716acee05d 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -51,7 +51,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index ac2e548cce4a..fa6c467b6c9e 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index 6d02a3514f4d..21a3ae5f7761 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index e1736a38a56a..87af360c81e6 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 588cfb9290bc..0773fe9f88b2 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -33,7 +33,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index 1141cafd3f64..4bd90984bea6 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 346bcd649d06..9622984b054c 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -39,7 +39,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index 6ed79bfb9324..eaab0a8ed21a 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -38,7 +38,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index 9d388ca301df..80c53ce10d62 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -39,7 +39,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index 3db522639caa..0ebbaf59c335 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index cfd114da0799..d7220fee7125 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -29,7 +29,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index 17911f8ff6b3..491c3e384693 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -30,7 +30,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board index 707fe02e11d5..82b5cff455b7 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -31,7 +31,7 @@ CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index bab77b72b44e..e3148558c9fa 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -48,7 +48,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index 1174ad99261c..617745a288ab 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index efa1bfc96c29..b70252483636 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index bb3f22cb5806..92509040550e 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index 87cdf1567458..6a544512b436 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index efa1bfc96c29..b70252483636 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 36d48eae10ac..e02f8cf13fe4 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index d72b343cece4..05b9c179823f 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index 845d4ec19d12..8ea461426b54 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index 340cb08c9340..04aa8d0bd6f3 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 600fe52fea9e..ee4063256387 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index ec693b14c152..e4b2dd946231 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -48,7 +48,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index b22d389893e2..feafdf2b9aad 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -20,7 +20,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/nxp/mr-canhubk3/sysview.px4board b/boards/nxp/mr-canhubk3/sysview.px4board index b22d389893e2..feafdf2b9aad 100644 --- a/boards/nxp/mr-canhubk3/sysview.px4board +++ b/boards/nxp/mr-canhubk3/sysview.px4board @@ -20,7 +20,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y diff --git a/boards/px4/fmu-v2/fixedwing.px4board b/boards/px4/fmu-v2/fixedwing.px4board index e3bc31147275..61f1539e9d12 100644 --- a/boards/px4/fmu-v2/fixedwing.px4board +++ b/boards/px4/fmu-v2/fixedwing.px4board @@ -9,5 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 379f2d764200..993492bb7b94 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -49,7 +49,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index d74886e769e5..9e218b1eee56 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index 9c0d72a706d4..e08094c0092c 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 01a58989e46d..63b9f857e7d7 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -55,7 +55,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index 4d204fa2c8fe..bb2dd7a051fc 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -18,7 +18,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL_L1=n +CONFIG_MODULES_FW_PATH_NAVIGATION=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 0689cd2264b6..1f208074f007 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -58,7 +58,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index dda675325958..76f8655deb9a 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -42,7 +42,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index c9b0a5a5e96d..20563bb92296 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 7c0fe4f8cf83..73bd2dd9a0ba 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 3f4cee7be691..1f93c77beb0d 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -30,7 +30,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index d57168750471..f85f30f424bf 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -17,7 +17,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 59f6d98c7740..ca284cc29e03 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -30,7 +30,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board index f3d96e775702..9f029ca3c6dc 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/thepeach/k1/default.px4board b/boards/thepeach/k1/default.px4board index b22018a9fe1a..3c08cf707aea 100644 --- a/boards/thepeach/k1/default.px4board +++ b/boards/thepeach/k1/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/boards/thepeach/r1/default.px4board b/boards/thepeach/r1/default.px4board index b22018a9fe1a..3c08cf707aea 100644 --- a/boards/thepeach/r1/default.px4board +++ b/boards/thepeach/r1/default.px4board @@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_PATH_NAVIGATION=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_path_navigation/CMakeLists.txt similarity index 96% rename from src/modules/fw_pos_control_l1/CMakeLists.txt rename to src/modules/fw_path_navigation/CMakeLists.txt index 72e1ca646dd5..f0ee265db3c3 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_path_navigation/CMakeLists.txt @@ -35,8 +35,8 @@ add_subdirectory(launchdetection) add_subdirectory(runway_takeoff) px4_add_module( - MODULE modules__fw_pos_control_l1 - MAIN fw_pos_control_l1 + MODULE modules__fw_path_navigation + MAIN fw_path_navigation SRCS FixedwingPositionControl.cpp FixedwingPositionControl.hpp diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp similarity index 99% rename from src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp rename to src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 595752d03bfd..7e8046c9ad1a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -2729,11 +2729,11 @@ int FixedwingPositionControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -fw_pos_control_l1 is the fixed wing position controller. +fw_path_navigation is the fixed wing path navigation. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller"); + PRINT_MODULE_USAGE_NAME("fw_path_navigation", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); @@ -2741,7 +2741,7 @@ fw_pos_control_l1 is the fixed wing position controller. return 0; } -extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]) +extern "C" __EXPORT int fw_path_navigation_main(int argc, char *argv[]) { return FixedwingPositionControl::main(argc, argv); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp similarity index 99% rename from src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp rename to src/modules/fw_path_navigation/FixedwingPositionControl.hpp index b2883401d795..cf3204912776 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -33,7 +33,7 @@ /** - * @file fw_pos_control_l1_main.hpp + * @file fw_path_navigation_main.hpp * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll * angle, equivalent to a lateral motion (for copters and rovers). * diff --git a/src/modules/fw_path_navigation/Kconfig b/src/modules/fw_path_navigation/Kconfig new file mode 100644 index 000000000000..7a7fbde149f3 --- /dev/null +++ b/src/modules/fw_path_navigation/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_FW_PATH_NAVIGATION + bool "fw_path_navigation" + default n + ---help--- + Enable support for fw_path_navigation + +menuconfig USER_FW_PATH_NAVIGATION + bool "fw_path_navigation running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_FW_PATH_NAVIGATION + ---help--- + Put fw_path_navigation in userspace memory diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_path_navigation/fw_path_navigation_params.c similarity index 100% rename from src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c rename to src/modules/fw_path_navigation/fw_path_navigation_params.c diff --git a/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt b/src/modules/fw_path_navigation/launchdetection/CMakeLists.txt similarity index 100% rename from src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt rename to src/modules/fw_path_navigation/launchdetection/CMakeLists.txt diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/modules/fw_path_navigation/launchdetection/LaunchDetector.cpp similarity index 100% rename from src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp rename to src/modules/fw_path_navigation/launchdetection/LaunchDetector.cpp diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_path_navigation/launchdetection/LaunchDetector.h similarity index 100% rename from src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h rename to src/modules/fw_path_navigation/launchdetection/LaunchDetector.h diff --git a/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c b/src/modules/fw_path_navigation/launchdetection/launchdetection_params.c similarity index 100% rename from src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c rename to src/modules/fw_path_navigation/launchdetection/launchdetection_params.c diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/CMakeLists.txt b/src/modules/fw_path_navigation/runway_takeoff/CMakeLists.txt similarity index 100% rename from src/modules/fw_pos_control_l1/runway_takeoff/CMakeLists.txt rename to src/modules/fw_path_navigation/runway_takeoff/CMakeLists.txt diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.cpp similarity index 100% rename from src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp rename to src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.cpp diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h similarity index 100% rename from src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h rename to src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c similarity index 100% rename from src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c rename to src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c diff --git a/src/modules/fw_pos_control_l1/Kconfig b/src/modules/fw_pos_control_l1/Kconfig deleted file mode 100644 index 88008a18b76b..000000000000 --- a/src/modules/fw_pos_control_l1/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_FW_POS_CONTROL_L1 - bool "fw_pos_control_l1" - default n - ---help--- - Enable support for fw_pos_control_l1 - -menuconfig USER_FW_POS_CONTROL_L1 - bool "fw_pos_control_l1 running as userspace module" - default n - depends on BOARD_PROTECTED && MODULES_FW_POS_CONTROL_L1 - ---help--- - Put fw_pos_control_l1 in userspace memory From 62e078e2b4d7f961480e56fe7b6e7262125995be Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 7 Feb 2023 10:22:26 +0100 Subject: [PATCH 3/7] fw_path_navigation: Remove explicit L1 mentioning. --- .../init.d/airframes/13014_vtol_babyshark | 2 +- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 2 +- posix-configs/SITL/init/test/test_shutdown | 6 +++--- posix-configs/bbblue/px4_fw.config | 2 +- posix-configs/rpi/pilotpi_fw.config | 2 +- posix-configs/rpi/px4_fw.config | 2 +- src/modules/fw_path_navigation/CMakeLists.txt | 2 +- .../FixedwingPositionControl.cpp | 8 ++++---- .../FixedwingPositionControl.hpp | 12 ++++-------- .../fw_path_navigation_params.c | 15 +++++++-------- .../runway_takeoff/RunwayTakeoff.h | 2 +- .../runway_takeoff/runway_takeoff_params.c | 4 ++-- src/modules/navigator/mission_block.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_params.c | 2 +- 16 files changed, 31 insertions(+), 36 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index b6e595782652..b040af3065b1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -30,7 +30,7 @@ param set-default BAT1_N_CELLS 6 param set-default FW_AIRSPD_MAX 30 param set-default FW_AIRSPD_MIN 19 param set-default FW_AIRSPD_TRIM 23 -param set-default FW_L1_R_SLEW_MAX 40 +param set-default FW_PN_R_SLEW_MAX 40 param set-default FW_PSP_OFF 3 param set-default FW_P_LIM_MAX 18 param set-default FW_P_LIM_MIN -25 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 426a9b83d1ed..4da8b7b757ca 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -20,7 +20,7 @@ control_allocator start # fw_rate_control start fw_att_control start -fw_pos_control_l1 start +fw_path_navigation start airspeed_selector start # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 56ba89f99dc7..3aab9459bc64 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -37,7 +37,7 @@ fi fw_rate_control start vtol fw_att_control start vtol -fw_pos_control_l1 start vtol +fw_path_navigation start vtol fw_autotune_attitude_control start vtol # Start Land Detector diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 7a66bb4577ef..6e604967768f 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -29,7 +29,7 @@ flight_mode_manager start mc_pos_control start vtol mc_att_control start vtol mc_rate_control start vtol -fw_pos_control_l1 start vtol +fw_path_navigation start vtol fw_att_control start vtol airspeed_selector start @@ -59,7 +59,7 @@ flight_mode_manager status mc_pos_control status mc_att_control status mc_rate_control status -fw_pos_control_l1 status +fw_path_navigation status fw_att_control status airspeed_selector status dataman status @@ -74,7 +74,7 @@ mc_att_control stop fw_att_control stop flight_mode_manager stop mc_pos_control stop -fw_pos_control_l1 stop +fw_path_navigation stop navigator stop commander stop land_detector stop diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 158d5239c0c7..0a71683c6196 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -56,7 +56,7 @@ ekf2 start land_detector start fixedwing fw_att_control start -fw_pos_control_l1 start +fw_path_navigation start mavlink start -n SoftAp -x -u 14556 -r 1000000 -p # -n name of wifi interface: SoftAp, wlan or your custom interface, diff --git a/posix-configs/rpi/pilotpi_fw.config b/posix-configs/rpi/pilotpi_fw.config index 723500793106..1ee5e6193910 100644 --- a/posix-configs/rpi/pilotpi_fw.config +++ b/posix-configs/rpi/pilotpi_fw.config @@ -63,7 +63,7 @@ airspeed_selector start land_detector start fixedwing flight_mode_manager start fw_att_control start -fw_pos_control_l1 start +fw_path_navigation start mavlink start -x -u 14556 -r 1000000 -p diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 7e65605bef02..468ff8a9d9f7 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -41,7 +41,7 @@ navigator start ekf2 start land_detector start fixedwing fw_att_control start -fw_pos_control_l1 start +fw_path_navigation start mavlink start -x -u 14556 -r 1000000 -p diff --git a/src/modules/fw_path_navigation/CMakeLists.txt b/src/modules/fw_path_navigation/CMakeLists.txt index f0ee265db3c3..c42df6ca989e 100644 --- a/src/modules/fw_path_navigation/CMakeLists.txt +++ b/src/modules/fw_path_navigation/CMakeLists.txt @@ -42,7 +42,7 @@ px4_add_module( FixedwingPositionControl.hpp DEPENDS launchdetection - npfg + npfg runway_takeoff SlewRate tecs diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 7e8046c9ad1a..470862f5f876 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -113,7 +113,7 @@ FixedwingPositionControl::parameters_update() _npfg.setRollTimeConst(_param_npfg_roll_time_const.get()); _npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get()); _npfg.setRollLimit(radians(_param_fw_r_lim.get())); - _npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get())); + _npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max.get())); _npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get()); // TECS parameters @@ -1338,7 +1338,7 @@ 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()); + _npfg.setPeriod(_param_rwto_npfg_period.get()); } const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0), @@ -1578,7 +1578,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ // 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() + + const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() + (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); _npfg.setPeriod(ground_roll_npfg_period); @@ -1853,7 +1853,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, // do slew rate limiting on roll if enabled float roll_sp_new = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get()); + const float roll_rate_slew_rad = radians(_param_fw_pn_r_slew_max.get()); if (control_interval > 0.f && roll_rate_slew_rad > 0.f) { roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval, diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index cf3204912776..e79a632b70f3 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -34,10 +34,10 @@ /** * @file fw_path_navigation_main.hpp - * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll + * Implementation of a generic path navigation. Outputs a bank / roll * angle, equivalent to a lateral motion (for copters and rovers). * - * The implementation for the controllers is in the ECL library. This class only + * The implementation for the controllers is in a separate library. This class only * interfaces to the library. * * @author Lorenz Meier @@ -156,10 +156,6 @@ static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f; // altitude while waiting for navigator to flag it exceeded static constexpr float kClearanceAltitudeBuffer = 10.0f; -// [m] a very large number to hopefully avoid the "fly back" case in L1 waypoint following logic once passed the second -// waypoint in the segment. this is unecessary with NPFG. -static constexpr float L1_VIRTUAL_TAKEOFF_WP_DIST = 1.0e6f; - // [m/s] maximum rate at which the touchdown position can be nudged static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f; @@ -841,7 +837,7 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_gnd_spd_min, - (ParamFloat) _param_fw_l1_r_slew_max, + (ParamFloat) _param_fw_pn_r_slew_max, (ParamFloat) _param_fw_r_lim, (ParamFloat) _param_npfg_period, @@ -917,7 +913,7 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_wing_span, (ParamFloat) _param_fw_wing_height, - (ParamFloat) _param_rwto_l1_period, + (ParamFloat) _param_rwto_npfg_period, (ParamBool) _param_rwto_nudge, (ParamFloat) _param_fw_lnd_fl_time, diff --git a/src/modules/fw_path_navigation/fw_path_navigation_params.c b/src/modules/fw_path_navigation/fw_path_navigation_params.c index 6eddf0015510..6756fd241761 100644 --- a/src/modules/fw_path_navigation/fw_path_navigation_params.c +++ b/src/modules/fw_path_navigation/fw_path_navigation_params.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * L1 controller roll slew rate limit. + * Path navigation roll slew rate limit. * * The maximum change in roll angle setpoint per second. * @@ -40,9 +40,9 @@ * @min 0 * @decimal 0 * @increment 1 - * @group FW L1 Control + * @group FW Path Control */ -PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f); +PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f); /** * NPFG period @@ -153,8 +153,7 @@ PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f); * NPFG switch distance multiplier * * Multiplied by the track error boundary to determine when the aircraft switches - * to the next waypoint and/or path segment. Should be less than 1. 1/pi (0.32) - * sets the switch distance equivalent to that of the L1 controller. + * to the next waypoint and/or path segment. Should be less than 1. * * @min 0.1 * @max 1.0 @@ -243,7 +242,7 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f); * @max 65.0 * @decimal 1 * @increment 0.5 - * @group FW L1 Control + * @group FW Path Control */ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); @@ -327,7 +326,7 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); * @max 30.0 * @decimal 1 * @increment 0.5 - * @group FW L1 Control + * @group FW Path Control */ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); @@ -746,7 +745,7 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); * @max 3 * @bit 0 Alternative stick configuration (height rate on throttle stick, airspeed on pitch stick) * @bit 1 Enable airspeed setpoint via sticks in altitude and position flight mode - * @group FW L1 Control + * @group FW Path Control */ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2); diff --git a/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h index 94fc02735e55..35e463077ec3 100644 --- a/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h @@ -119,7 +119,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams float getPitch(float external_pitch_setpoint); /** - * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from L1) [rad] + * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad] * @return Roll angle setpoint [rad] */ float getRoll(float external_roll_setpoint); diff --git a/src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c index 7f791a01c668..af629c523042 100644 --- a/src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c @@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f); /** - * L1 period while steering on runway + * NPFG period while steering on runway * * @unit s * @min 1.0 @@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f); * @increment 0.1 * @group Runway Takeoff */ -PARAM_DEFINE_FLOAT(RWTO_L1_PERIOD, 5.0f); +PARAM_DEFINE_FLOAT(RWTO_NPFG_PERIOD, 5.0f); /** * Enable use of yaw stick for nudging the wheel during runway ground roll diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index aee4b1a1cc2b..84edcc0e561c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -255,7 +255,7 @@ MissionBlock::is_mission_item_reached_or_completed() * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter - * radius + L1 distance. Time inside and turn count is handled elsewhere. + * radius + navigation switch distance. Time inside and turn count is handled elsewhere. */ // check if within loiter radius around wp, if yes then set altitude sp to mission item diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 31ff6a7d8a9d..ec5596ee826d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1177,7 +1177,7 @@ float Navigator::get_acceptance_radius() float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - // for fixed-wing and rover, return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance) + // for fixed-wing and rover, return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. navigation switch distance) if (_vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && PX4_ISFINITE(pos_ctrl_status.acceptance_radius) && pos_ctrl_status.timestamp != 0) { diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 6037cb879c2c..1beed87bac8e 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -58,7 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 80.0f); * Acceptance Radius * * Default acceptance radius, overridden by acceptance radius of waypoint if set. - * For fixed wing the L1 turning distance is used for horizontal acceptance. + * For fixed wing the npfg switch distance is used for horizontal acceptance. * * @unit m * @min 0.05 From 8ec44641cbcfe2ff6145789db8bd6dc52695ee65 Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 7 Feb 2023 10:24:35 +0100 Subject: [PATCH 4/7] FixedWingPositionControl: remove get_nav_speed_2d function as npfg can handle this internally. --- .../FixedwingPositionControl.cpp | 35 ++++--------------- .../FixedwingPositionControl.hpp | 11 ------ 2 files changed, 6 insertions(+), 40 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 470862f5f876..34ced43c8484 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -827,7 +827,7 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se if (!PX4_ISFINITE(_transition_waypoint(0))) { double lat_transition, lon_transition; - // Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track + // Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track // during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn // is set to the transition heading by Navigator, or current yaw if setpoint is not valid. const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw; @@ -1027,7 +1027,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co } else { // No valid previous waypoint, go for the current wp. - // This is automatically handled by the L1/NPFG libraries. + // This is automatically handled by the NPFG libraries. prev_wp(0) = pos_sp_curr.lat; prev_wp(1) = pos_sp_curr.lon; } @@ -1096,11 +1096,11 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co 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), + navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); } else { - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel); + navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1187,7 +1187,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // No valid previous waypoint, go for the current wp. - // This is automatically handled by the L1/NPFG libraries. + // This is automatically handled by the NPFG libraries. prev_wp(0) = pos_sp_curr.lat; prev_wp(1) = pos_sp_curr.lon; } @@ -1245,7 +1245,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _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), + ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -2204,29 +2204,6 @@ FixedwingPositionControl::reset_landing_state() } } -Vector2f -FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed) -{ - - Vector2f nav_speed_2d{ground_speed}; - - 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)}; - - // angle between air_speed_2d and ground_speed - const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length())); - - // if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection - if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) { - nav_speed_2d = air_speed_2d; - } - } - - return nav_speed_2d; -} - float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max) { diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index e79a632b70f3..ca586fd684b2 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -650,17 +650,6 @@ class FixedwingPositionControl final : public ModuleBase Date: Tue, 7 Feb 2023 14:53:43 +0100 Subject: [PATCH 5/7] FwPosControl: Update behavior of navigating to a waypoint when the previous waypoint is not valid. Go along the line of the current aircraft position to the desired waypoint. --- .../fw_path_navigation/FixedwingPositionControl.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 34ced43c8484..713ce21e8d9a 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -1026,10 +1026,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co prev_wp(1) = pos_sp_prev.lon; } else { - // No valid previous waypoint, go for the current wp. - // This is automatically handled by the NPFG libraries. - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; + // No valid previous waypoint, go along the line between aircraft and current waypoint + prev_wp = curr_pos; } float tecs_fw_thr_min; @@ -1186,10 +1184,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons prev_wp(1) = pos_sp_prev.lon; } else { - // No valid previous waypoint, go for the current wp. - // This is automatically handled by the NPFG libraries. - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; + // No valid previous waypoint, go along the line between aircraft and current waypoint + prev_wp = curr_pos; } float airspeed_sp = -1.f; From 7a47147d441296bce3c885177470abf0647fbc2a Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 8 Feb 2023 13:45:11 +0100 Subject: [PATCH 6/7] FixedwingPositionControl: Explicitly set wind to zero when it is not valid. --- .../fw_path_navigation/FixedwingPositionControl.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 713ce21e8d9a..df6ef6dc792c 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -303,6 +303,11 @@ FixedwingPositionControl::wind_poll() // invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout _wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT; } + + if (!_wind_valid) { + _wind_vel(0) = 0.f; + _wind_vel(1) = 0.f; + } } void @@ -1095,7 +1100,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co 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(), ground_speed, - _wind_vel, curvature); + _wind_vel, curvature); } else { navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); @@ -1242,7 +1247,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, - _wind_vel); + _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; From 1309ecb77634102fd6aa27d4078868490f471fbe Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 8 Feb 2023 15:19:52 +0100 Subject: [PATCH 7/7] parameter_translation: Add parameter translation for renamed L1 parameters --- src/lib/parameters/param_translation.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 7b0c3578c1b1..5a5603d4cf3d 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -211,4 +211,25 @@ bool param_modify_on_import(bson_node_t node) } return false; + + //2023-02-08: translate L1 parameters after removing l1 control + { + if (strcmp("RWTO_L1_PERIOD", node->name) == 0) { + strcpy(node->name, "RWTO_NPFG_PERIOD"); + PX4_INFO("copying %s -> %s", "RWTO_L1_PERIOD", "RWTO_NPFG_PERIOD"); + return true; + } + + if (strcmp("FW_L1_R_SLEW_MAX", node->name) == 0) { + strcpy(node->name, "FW_PN_R_SLEW_MAX"); + PX4_INFO("copying %s -> %s", "FW_L1_R_SLEW_MAX", "FW_PN_R_SLEW_MAX"); + return true; + } + + if (strcmp("FW_L1_PERIOD", node->name) == 0) { + strcpy(node->name, "NPFG_PERIOD"); + PX4_INFO("copying %s -> %s", "FW_L1_PERIOD", "NPFG_PERIOD"); + return true; + } + } }