diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index b6e5957826..b040af3065 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 426a9b83d1..4da8b7b757 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 56ba89f99d..3aab9459bc 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 7a66bb4577..6e60496776 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 158d5239c0..0a71683c61 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 7235007931..1ee5e61939 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 7e65605bef..468ff8a9d9 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 f0ee265db3..c42df6ca98 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 7e8046c9ad..470862f5f8 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 cf32049127..e79a632b70 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 @@ private: (ParamFloat) _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 @@ private: (ParamFloat) _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 6eddf00155..6756fd2417 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 94fc02735e..35e463077e 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 @@ public: 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 7f791a01c6..af629c5230 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 aee4b1a1cc..84edcc0e56 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 31ff6a7d8a..ec5596ee82 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 6037cb879c..1beed87bac 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