fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 06:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

integrate optional NPFG library for wind-aware fixed-wing guidance

Browse Source
This commit is contained in:
Thomas Stastny
2022-01-14 22:05:53 +01:00
committed by Silvan Fuhrer
parent 66cbbf9f2e
commit 082e320191
11 changed files with 1948 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/CMakeLists.txt
+1
View File
@@ -44,6 +44,7 @@ px4_add_module(
l1
launchdetection
landing_slope
npfg
runway_takeoff
SlewRate
tecs
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+281 -71
View File
@@ -110,6 +110,22 @@ FixedwingPositionControl::parameters_update()
_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());
_npfg.enablePeriodLB(_param_npfg_en_period_lb.get());
_npfg.enablePeriodUB(_param_npfg_en_period_ub.get());
_npfg.enableMinGroundSpeed(_param_npfg_en_min_gsp.get());
_npfg.enableTrackKeeping(_param_npfg_en_track_keeping.get());
_npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get());
_npfg.setMinGroundSpeed(_param_fw_gnd_spd_min.get());
_npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get());
_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.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
// TECS parameters
_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
@@ -279,6 +295,28 @@ FixedwingPositionControl::airspeed_poll()
}
}
void
FixedwingPositionControl::wind_poll()
{
if (_wind_sub.updated()) {
wind_s wind;
_wind_sub.update(&wind);
// assumes wind is valid if finite
_wind_valid = PX4_ISFINITE(wind.windspeed_north)
&& PX4_ISFINITE(wind.windspeed_east);
_time_wind_last_received = hrt_absolute_time();
_wind_vel(0) = wind.windspeed_north;
_wind_vel(1) = wind.windspeed_east;
} else {
// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < T_WIND_EST_TIMEOUT;
}
}
void
FixedwingPositionControl::manual_control_setpoint_poll()
{
@@ -369,7 +407,9 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con
}
// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
if (!_l1_control.circle_mode()) {
const bool not_using_wind_est = !_param_fw_use_npfg.get() || !_wind_valid;
if (!_l1_control.circle_mode() && not_using_wind_est) {
/*
* 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
@@ -417,7 +457,6 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con
return airspeed_setpoint;
}
void
FixedwingPositionControl::tecs_status_publish()
{
@@ -504,11 +543,42 @@ FixedwingPositionControl::status_publish()
}
if (_param_fw_use_npfg.get() && _wind_valid) {
npfg_status_s npfg_status = {};
npfg_status.wind_est_valid = _wind_valid;
const float bearing = _npfg.getBearing(); // dont repeat atan2 calc
pos_ctrl_status.nav_bearing = bearing;
pos_ctrl_status.target_bearing = _npfg.targetBearing();
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);
}
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
pos_ctrl_status.yaw_acceptance = NAN;
pos_ctrl_status.timestamp = hrt_absolute_time();
@@ -724,7 +794,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
_control_position_last_called = now;
_l1_control.set_dt(dt);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setDt(dt);
} else {
_l1_control.set_dt(dt);
}
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps
@@ -751,7 +826,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_hdg_hold_yaw = _yaw;
/* get circle mode */
const bool was_circle_mode = _l1_control.circle_mode();
const bool was_circle_mode = (_param_fw_use_npfg.get() && _wind_valid) ? _npfg.circleMode() : _l1_control.circle_mode();
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
@@ -962,7 +1037,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
const float acc_rad = _l1_control.switch_distance(500.0f);
const float acc_rad = (_param_fw_use_npfg.get()
&& _wind_valid) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
uint8_t position_sp_type = setpoint_type;
@@ -1003,7 +1079,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
if ((dist >= 0.f)
&& (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))) {
&& (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))
&& !(_param_fw_use_npfg.get() && _wind_valid)) {
// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
@@ -1022,7 +1099,8 @@ void
FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, 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 = _l1_control.switch_distance(500.0f);
const float acc_rad = (_param_fw_use_npfg.get()
&& _wind_valid) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
@@ -1103,12 +1181,24 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
_att_sp.roll_body = _l1_control.get_roll_setpoint();
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else {
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, 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(now, position_sp_alt,
get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1153,13 +1243,25 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
//Offboard velocity control
Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateBearing(_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();
}
_att_sp.yaw_body = _yaw;
tecs_update_pitch_throttle(now, position_sp_alt,
get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1236,15 +1338,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
loiter_direction = (loiter_radius > 0) ? 1 : -1;
}
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, 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;
const bool in_circle_mode = (_param_fw_use_npfg.get() && _wind_valid) ? _npfg.circleMode() : _l1_control.circle_mode();
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
&& _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) {
&& in_circle_mode && _param_fw_lnd_earlycfg.get()) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter altitude control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
@@ -1253,6 +1350,24 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
_att_sp.apply_flaps = true;
}
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateLoiter(curr_wp, curr_pos, loiter_radius, loiter_direction, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else {
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, 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;
if (in_takeoff_situation()) {
alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get());
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
@@ -1272,7 +1387,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
}
tecs_update_pitch_throttle(now, alt_sp,
get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
@@ -1332,18 +1447,34 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
_current_latitude, _current_longitude, &_mavlink_log_pub);
float target_airspeed = get_auto_airspeed_setpoint(now,
_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
dt);
/*
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
* If we use the navigator heading or not is decided later.
*/
_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed, _wind_vel);
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else {
_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
}
_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
// update tecs
const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
get_auto_airspeed_setpoint(now, _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
dt),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(takeoff_pitch_max_deg),
_param_fw_thr_min.get(),
@@ -1353,8 +1484,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
// assign values
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
@@ -1393,8 +1522,21 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
/* Launch has been detected, hence we have to control the plane. */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
dt);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else {
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, 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
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
@@ -1414,7 +1556,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
_param_fw_airspd_trim.get(),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(takeoff_pitch_max_deg),
_param_fw_thr_min.get(),
@@ -1428,7 +1570,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
} else {
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed, dt),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
@@ -1541,23 +1683,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
events::send(events::ID("fixedwing_position_control_landing"), events::Log::Info, "Landing, heading hold");
}
if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
} else {
// normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, 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
if (_land_noreturn_horizontal) {
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
}
/* Vertical landing control */
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
@@ -1614,12 +1739,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
/* kill the throttle if param requests it */
float throttle_max = _param_fw_thr_max.get();
/* enable direct yaw control using rudder/wheel */
if (_land_noreturn_horizontal) {
_att_sp.yaw_body = _target_bearing;
_att_sp.fw_control_yaw = true;
}
if (((_current_altitude < terrain_alt + _landingslope.motor_lim_relative_alt()) &&
(wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP
_land_motor_lim) {
@@ -1643,10 +1762,56 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
}
const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
/* lateral guidance */
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
if (_land_noreturn_horizontal) {
// heading hold
_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
} else {
// normal navigation
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
}
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
} else {
if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
} else {
// normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
}
_att_sp.roll_body = _l1_control.get_roll_setpoint();
}
if (_land_noreturn_horizontal) {
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
}
/* enable direct yaw control using rudder/wheel */
if (_land_noreturn_horizontal) {
_att_sp.yaw_body = _target_bearing;
_att_sp.fw_control_yaw = true;
} else {
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt),
target_airspeed,
radians(_param_fw_lnd_fl_pmin.get()),
radians(_param_fw_lnd_fl_pmax.get()),
0.0f,
@@ -1714,9 +1879,42 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
}
const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
/* lateral guidance */
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
if (_land_noreturn_horizontal) {
// heading hold
_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
} else {
// normal navigation
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
}
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
} else {
if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
} else {
// normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, 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(now, altitude_desired,
get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt),
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
@@ -1821,17 +2019,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, altitude_sp_amsl,
get_manual_airspeed_setpoint(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_cruise.get(),
false,
pitch_limit_min,
false,
height_rate_sp);
float target_airspeed = get_manual_airspeed_setpoint();
/* heading control */
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
@@ -1872,10 +2060,19 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
if (_param_fw_use_npfg.get() && _wind_valid) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else {
/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
}
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
if (in_takeoff_situation()) {
@@ -1885,6 +2082,18 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
}
}
tecs_update_pitch_throttle(now, altitude_sp_amsl,
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_cruise.get(),
false,
pitch_limit_min,
false,
height_rate_sp);
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
@@ -2058,6 +2267,7 @@ FixedwingPositionControl::Run()
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();
wind_poll();
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+27
View File
@@ -56,6 +56,7 @@
#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/landing_slope/Landingslope.hpp>
#include <lib/mathlib/mathlib.h>
@@ -73,6 +74,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/npfg_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
@@ -89,6 +91,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <uORB/topics/orbit_status.h>
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
@@ -114,6 +117,8 @@ static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort l
static constexpr float THROTTLE_THRESH =
0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
10_s; // time after which the wind estimate is disabled if no longer updating
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
@@ -143,6 +148,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
@@ -157,6 +163,7 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; ///< vehicle local position setpoint publication
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
@@ -230,6 +237,11 @@ private:
float _airspeed{0.0f};
float _eas2tas{1.0f};
/* wind estimates */
Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
bool _wind_valid{false}; ///< flag if a valid wind estimate exists
hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
float _pitch{0.0f};
float _yaw{0.0f};
float _yawrate{0.0f};
@@ -258,6 +270,7 @@ private:
hrt_abstime _time_in_fixed_bank_loiter{0};
ECL_L1_Pos_Controller _l1_control;
NPFG _npfg;
TECS _tecs;
uint8_t _position_sp_type{0};
@@ -284,6 +297,7 @@ private:
void vehicle_command_poll();
void vehicle_control_mode_poll();
void vehicle_status_poll();
void wind_poll();
void status_publish();
void landing_status_publish();
@@ -390,6 +404,19 @@ private:
(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,
(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
(ParamBool<px4::params::NPFG_TRACK_KEEP>) _param_npfg_en_track_keeping,
(ParamBool<px4::params::NPFG_EN_MIN_GSP>) _param_npfg_en_min_gsp,
(ParamBool<px4::params::NPFG_WIND_REG>) _param_npfg_en_wind_reg,
(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
(ParamFloat<px4::params::NPFG_PERIOD_SF>) _param_npfg_period_safety_factor,
(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+144
View File
@@ -85,6 +85,150 @@ 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, 0);
/**
* NPFG period
*
* Period of the NPFG control law.
*
* @unit s
* @min 1.0
* @max 100.0
* @decimal 1
* @increment 0.1
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_PERIOD, 10.0f);
/**
* NPFG damping ratio
*
* Damping ratio of the NPFG control law.
*
* @min 0.10
* @max 1.00
* @decimal 2
* @increment 0.01
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f);
/**
* Enable automatic lower bound on the NPFG period
*
* Avoids limit cycling from a too aggressively tuned period/damping combination.
* If set to false, also disables the upper bound NPFG_PERIOD_UB.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1);
/**
* Enable automatic upper bound on the NPFG period
*
* Adapts period to maintain track keeping in variable winds and path curvature.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1);
/**
* Enable track keeping excess wind handling logic.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_TRACK_KEEP, 1);
/**
* Enable minimum forward ground speed maintaining excess wind handling logic
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_EN_MIN_GSP, 1);
/**
* Enable wind excess regulation.
*
* Disabling this parameter further disables all other airspeed incrementation options.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_WIND_REG, 1);
/**
* Maximum, minimum forward ground speed for track keeping in excess wind
*
* The maximum value of the minimum forward ground speed that may be commanded
* by the track keeping excess wind handling logic. Commanded in full at the normalized
* track error fraction of the track error boundary and reduced to zero on track.
*
* @unit m/s
* @min 0.0
* @max 10.0
* @decimal 1
* @increment 0.5
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_GSP_MAX_TK, 5.0f);
/**
* Roll time constant
*
* Time constant of roll controller command / response, modeled as first order delay.
* Used to determine lower period bound. Setting zero disables automatic period bounding.
*
* @unit s
* @min 0.00
* @max 2.00
* @decimal 2
* @increment 0.05
* @group FW NPFG Control
*/
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.
*
* @min 0.1
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
/**
* Period safety factor
*
* Multiplied by period for conservative minimum period bounding (when period lower
* bounding is enabled). 1.0 bounds at marginal stability.
*
* @min 1.0
* @max 10.0
* @decimal 1
* @increment 0.1
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f);
/**
* Cruise throttle
*
src/modules/logger/logged_topics.cpp
+1
View File
@@ -75,6 +75,7 @@ void LoggedTopics::add_default_topics()
add_topic("manual_control_switches");
add_topic("mission_result");
add_topic("navigator_mission_item");
add_topic("npfg_status", 100);
add_topic("offboard_control_mode", 100);
add_topic("onboard_computer_status", 10);
add_topic("parameter_update");