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

Avoid waypoint following during backtransition which can lead to strong banking (#22642) 

* avoid waypoint following during backtransition
- if vehicle overshoots transition, make controller track the prev/current
waypoint line, this avoids large banking maneuvers that delay the transition further

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* navigator: use SETPOINT_TYPE_LAND as setpoint type for VTOL_LAND waypoints
such that fixed wing position controller can distinguish from other types

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FixedWingPositionControl: split vtol backtransition logic into separate method

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* review changes

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* prevent loiter when approaching vtol backtransition and invalidate previous waypoint during mission landing

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* removed unused parameter

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* small renaming of transition mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst 2024-01-29 11:45:40 +03:00 committed by GitHub
parent f40ede6087
commit 077baeae52
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 109 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -686,8 +686,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
// Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE.
// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
if (doing_backtransition) {
_control_mode_current = FW_POSCTRL_MODE_TRANSITON;
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
@ -708,22 +712,14 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (!_vehicle_status.in_transition_mode) {
// Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
// Straight landings are currently only possible in Missions, and there the previous WP
// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
if (_position_setpoint_previous_valid) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
}
// Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
// Straight landings are currently only possible in Missions, and there the previous WP
// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
if (_position_setpoint_previous_valid) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
}
} else {
@ -838,7 +834,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
position_setpoint_s current_sp = pos_sp_curr;
move_position_setpoint_for_vtol_transition(current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp, pos_sp_next);
_position_sp_type = position_sp_type;
@ -971,7 +967,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
}
uint8_t
FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr,
const position_setpoint_s &pos_sp_next)
{
uint8_t position_sp_type = pos_sp_curr.type;
@ -986,8 +983,13 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
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) {
const bool approaching_vtol_backtransition = _vehicle_status.is_vtol
&& pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid
&& pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid;
// check if we should switch to loiter but only if we are not expecting a backtransition to happen
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) {
float dist_xy = -1.f;
float dist_z = -1.f;
@ -997,17 +999,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
_current_latitude, _current_longitude, _current_altitude,
&dist_xy, &dist_z);
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// Achieve position setpoint altitude via loiter when laterally close to WP.
// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
&& (dist_z > _param_nav_fw_alt_rad.get())
&& (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
// Achieve position setpoint altitude via loiter when laterally close to WP.
// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
&& (dist_z > _param_nav_fw_alt_rad.get())
&& (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
}
@ -2178,6 +2178,53 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
_att_sp.pitch_body = get_tecs_pitch();
}
void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr)
{
float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
if (_position_setpoint_previous_valid) {
Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
navigateLine(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
} else {
// if we don't have a previous waypoint for line following, then create one using the current position at the
// start of the transition
if (!_lpos_where_backtrans_started.isAllFinite()) {
_lpos_where_backtrans_started = curr_pos_local;
}
navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
}
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
tecs_update_pitch_throttle(control_interval,
pos_sp_curr.alt,
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
_att_sp.pitch_body = get_tecs_pitch();
}
float
FixedwingPositionControl::get_tecs_pitch()
{
@ -2381,7 +2428,11 @@ FixedwingPositionControl::Run()
}
}
_vehicle_status_sub.update(&_vehicle_status);
if (_vehicle_status_sub.update(&_vehicle_status)) {
if (!_vehicle_status.in_transition_mode) {
_lpos_where_backtrans_started = Vector2f(NAN, NAN);
}
}
Vector2d curr_pos(_current_latitude, _current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
@ -2474,8 +2525,14 @@ FixedwingPositionControl::Run()
break;
}
case FW_POSCTRL_MODE_TRANSITON: {
control_backtransition(control_interval, ground_speed, _pos_sp_triplet.previous,
_pos_sp_triplet.current);
break;
}
}
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
if (_control_mode.flag_control_manual_enabled) {
@ -3062,6 +3119,7 @@ float FixedwingPositionControl::getLoadFactor()
}
extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[])
{
return FixedwingPositionControl::main(argc, argv);

19
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -101,6 +101,7 @@
#ifdef CONFIG_FIGURE_OF_EIGHT
#include "figure_eight/FigureEight.hpp"
#include <uORB/topics/figure_eight_status.h>
#endif // CONFIG_FIGURE_OF_EIGHT
using namespace launchdetection;
@ -222,6 +223,8 @@ private:
vehicle_local_position_s _local_pos{};
vehicle_status_s _vehicle_status{};
Vector2f _lpos_where_backtrans_started;
bool _position_setpoint_previous_valid{false};
bool _position_setpoint_current_valid{false};
bool _position_setpoint_next_valid{false};
@ -243,6 +246,7 @@ private:
FW_POSCTRL_MODE_AUTO_PATH,
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_TRANSITON,
FW_POSCTRL_MODE_OTHER
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
@ -540,7 +544,8 @@ private:
* @param pos_sp_curr Current position setpoint
* @return Adjusted position setpoint type
*/
uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr);
uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr,
const position_setpoint_s &pos_sp_next);
/* automatic control methods */
@ -685,6 +690,18 @@ private:
*/
void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
/**
* @brief Controls flying towards a transition waypoint and then transitioning to MC mode.
*
* @param control_interval Time since last position control call [s]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint
*/
void control_backtransition(const float control_interval, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
float get_tecs_pitch();
float get_tecs_thrust();

4
src/modules/navigator/mission_base.cpp
View File

@ -574,10 +574,6 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
// make previous setpoint invalid, such that there will be no prev-current line following
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
} else if (needs_to_land) {

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

@ -110,7 +110,8 @@ MissionBlock::is_mission_item_reached_or_completed()
if (int(_mission_item.params[0]) == 3) {
// transition to RW requested, only accept waypoint if vehicle state has changed accordingly
return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_navigator->get_vstatus()->in_transition_mode;
} else if (int(_mission_item.params[0]) == 4) {
// transition to FW requested, only accept waypoint if vehicle state has changed accordingly

1
src/modules/navigator/rtl_direct.cpp
View File

@ -310,6 +310,7 @@ void RtlDirect::set_rtl_item()
dest.alt = loiter_altitude;
setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
_rtl_state = RTLState::LAND;