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

Support gliding setpoints for autonomous soaring (#14643)

Browse Source 
* Enable offboard gliding position setpoints

Adds a capability to make the vehicle glide when given position setpoints.

Gliding setpoints are enabled by setting POSITION_TARGET_TYPEMASK_Z_IGNORE, POSITION_TARGET_TYPEMASK_VZ_IGNORE,  POSITION_TARGET_TYPEMASK_AZ_IGNORE on the typemask of position setpoints.
Include zero cruising throttle
Invalid setpoints with nan

* Handle nans for curise_throttle
This commit is contained in:
JaeyoungLim
2020-09-14 10:56:29 +02:00
committed by GitHub
parent 8bc512dc93
commit 0592f6ba6e
5 changed files with 55 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+43 -8
View File
@@ -668,8 +668,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
float mission_throttle = _param_fw_thr_cruise.get();
if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
pos_sp_curr.cruising_throttle > 0.01f) {
pos_sp_curr.cruising_throttle >= 0.0f) {
mission_throttle = pos_sp_curr.cruising_throttle;
}
@@ -684,16 +683,34 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
if (mission_throttle < _param_fw_thr_min.get()) {
/* enable gliding with this waypoint */
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;
} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = mission_throttle;
}
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
mission_throttle,
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
@@ -741,13 +758,31 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
}
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
if (mission_throttle < _param_fw_thr_min.get()) {
/* enable gliding with this waypoint */
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;
} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = _param_fw_thr_cruise.get();
}
tecs_update_pitch_throttle(now, alt_sp,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_fw_thr_cruise.get(),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));