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

fw_path_navigation: Remove explicit L1 mentioning.

This commit is contained in:
Konrad 2023-02-07 10:22:26 +01:00 committed by Silvan Fuhrer
parent 3e200bca0d
commit 6bdeb43e0d
16 changed files with 31 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
View File

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

2
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

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

2
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

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

6
posix-configs/SITL/init/test/test_shutdown
View File

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

2
posix-configs/bbblue/px4_fw.config
View File

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

2
posix-configs/rpi/pilotpi_fw.config
View File

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

2
posix-configs/rpi/px4_fw.config
View File

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

2
src/modules/fw_path_navigation/CMakeLists.txt
View File

@ -42,7 +42,7 @@ px4_add_module(
FixedwingPositionControl.hpp
DEPENDS
launchdetection
npfg
npfg
runway_takeoff
SlewRate
tecs

8
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

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

12
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
View File

@ -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 <lorenz@px4.io>
@ -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<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
@ -917,7 +913,7 @@ private:
(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
(ParamFloat<px4::params::RWTO_L1_PERIOD>) _param_rwto_l1_period,
(ParamFloat<px4::params::RWTO_NPFG_PERIOD>) _param_rwto_npfg_period,
(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,
(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,

15
src/modules/fw_path_navigation/fw_path_navigation_params.c
View File

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

2
src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.h
View File

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

4
src/modules/fw_path_navigation/runway_takeoff/runway_takeoff_params.c
View File

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

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

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

2
src/modules/navigator/navigator_main.cpp
View File

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

2
src/modules/navigator/navigator_params.c
View File

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