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

fw_pos_control: purge L1 controller

This commit is contained in:
Konrad 2023-02-08 13:22:19 +01:00 committed by Silvan Fuhrer
parent fbc80c9bf5
commit aa3af7f707
22 changed files with 105 additions and 311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric
View File

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

4
ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1042_gazebo-classic_tiltrotor
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar
View File

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

2
ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna
View File

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

2
ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
View File

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

1
src/modules/fw_pos_control_l1/CMakeLists.txt
View File

@ -41,7 +41,6 @@ px4_add_module(
FixedwingPositionControl.cpp
FixedwingPositionControl.hpp
DEPENDS
l1
launchdetection
npfg
runway_takeoff

150
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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,19 +510,6 @@ 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();
} else {
pos_ctrl_status.nav_bearing = NAN;
pos_ctrl_status.target_bearing = NAN;
pos_ctrl_status.xtrack_error = NAN;
}
if (_param_fw_use_npfg.get()) {
npfg_status_s npfg_status = {};
npfg_status.wind_est_valid = _wind_valid;
@ -557,10 +538,6 @@ FixedwingPositionControl::status_publish()
_npfg_status_pub.publish(npfg_status);
} else {
pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
}
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,7 +1088,6 @@ 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);
@ -1139,11 +1106,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} 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();
}
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
tecs_update_pitch_throttle(control_interval,
@ -1186,7 +1148,6 @@ 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);
@ -1194,11 +1155,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_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();
}
_att_sp.yaw_body = _yaw;
tecs_update_pitch_throttle(control_interval,
@ -1286,7 +1242,6 @@ 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,
@ -1295,13 +1250,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_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();
}
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
float alt_sp = pos_sp_curr.alt;
@ -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,7 +1353,6 @@ 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,
@ -1423,22 +1368,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
// heading hold mode will override this bearing setpoint
_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
} else {
// make a fake waypoint in the direction of the takeoff bearing
const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);
const float l1_roll_setpoint = _l1_control.get_roll_setpoint();
_att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint);
// use l1's nav bearing to command course, controlled via yaw angle while on runway
const float bearing = _l1_control.nav_bearing();
// heading hold mode will override this bearing setpoint
_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
}
// update tecs
const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()),
@ -1512,7 +1441,6 @@ 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,
@ -1520,12 +1448,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_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,14 +1580,10 @@ 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);
@ -1675,23 +1593,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
_att_sp.yaw_body = _npfg.getBearing();
} else {
// make a fake waypoint beyond the land point in the direction of the landing approach bearing
// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
local_position - local_approach_entrance);
const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
landing_approach_vector.unit_or_zero();
_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
// use l1's nav bearing to command course, controlled via yaw angle while on runway
_att_sp.yaw_body = _l1_control.nav_bearing();
}
/* longitudinal guidance */
const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator);
@ -1762,27 +1663,12 @@ 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();
}
/* longitudinal guidance */
// open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits
@ -1937,19 +1823,12 @@ 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();
}
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
}
@ -2186,20 +2065,14 @@ 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);
}
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
// 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;
}
local_position_setpoint.x = current_setpoint(0);
local_position_setpoint.y = current_setpoint(1);

9
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -55,7 +55,6 @@
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
#include <lib/npfg/npfg.hpp>
#include <lib/tecs/TECS.hpp>
#include <lib/mathlib/mathlib.h>
@ -415,9 +414,6 @@ private:
// CLosest point on path to track
matrix::Vector2f _closest_point_on_path;
// L1 guidance - lateral-directional position control
ECL_L1_Pos_Controller _l1_control;
// nonlinear path following guidance - lateral-directional position control
NPFG _npfg;
@ -658,8 +654,6 @@ private:
void reset_takeoff_state();
void reset_landing_state();
bool using_npfg_with_wind_estimate() const;
/**
* @brief Returns the velocity vector to be input in the lateral-directional guidance laws.
*
@ -847,12 +841,9 @@ private:
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
(ParamFloat<px4::params::FW_L1_DAMPING>) _param_fw_l1_damping,
(ParamFloat<px4::params::FW_L1_PERIOD>) _param_fw_l1_period,
(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
(ParamBool<px4::params::FW_USE_NPFG>) _param_fw_use_npfg,
(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,

52
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -31,48 +31,6 @@
*
****************************************************************************/
/**
* @file fw_pos_control_l1_params.c
*
* Parameters defined by the L1 position control task
*
* @author Lorenz Meier <lorenz@px4.io>
*/
/*
* 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
*