forked from Archive/PX4-Autopilot
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
GitHub
parent
f40ede6087
commit
077baeae52
|
|
@ -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,8 +712,6 @@ 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.
|
||||
|
|
@ -720,12 +722,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
|
|||
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
|
||||
}
|
||||
|
||||
} 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;
|
||||
}
|
||||
|
||||
} else {
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO;
|
||||
}
|
||||
|
|
@ -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,7 +999,6 @@ 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).
|
||||
|
|
@ -1009,7 +1010,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
|
|||
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return position_sp_type;
|
||||
}
|
||||
|
|
@ -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,7 +2525,13 @@ 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) {
|
||||
|
||||
|
|
@ -3062,6 +3119,7 @@ float FixedwingPositionControl::getLoadFactor()
|
|||
|
||||
}
|
||||
|
||||
|
||||
extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[])
|
||||
{
|
||||
return FixedwingPositionControl::main(argc, argv);
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue