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

FWPosCtrl: Rearrange VTOL transition code into eparate functions.

This commit is contained in:
Konrad 2023-05-31 16:21:32 +02:00
parent ea8b985a2f
commit ffa37adae3
2 changed files with 146 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -707,6 +707,22 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
return; // do not publish the setpoint
}
if (_control_mode.flag_control_auto_enabled && _vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
if (_control_mode.flag_control_position_enabled) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION;
} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED;
}
return;
}
if (_control_mode.flag_control_manual_enabled && _vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION;
return;
}
FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current;
_skipping_takeoff_detection = false;
@ -716,40 +732,24 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
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;
_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
// 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_TAKEOFF;
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
// skip takeoff detection when switching from any other mode, auto or manual,
// while already in air.
// TODO: find a better place for / way of doing this
_skipping_takeoff_detection = true;
}
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
// skip takeoff detection when switching from any other mode, auto or manual,
// while already in air.
// TODO: find a better place for / way of doing this
_skipping_takeoff_detection = true;
}
} 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 {
// 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 {
@ -769,8 +769,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
_time_in_fixed_bank_loiter = now;
}
if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
&& !_vehicle_status.in_transition_mode) {
if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)) {
if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
// Need to init because last loop iteration was in a different mode
events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
@ -833,36 +832,24 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
}
void
FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
FixedwingPositionControl::set_position_setpoint_for_vtol_transition(const position_setpoint_s &current_sp)
{
// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
// shifting hacks
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
&lat_transition,
&lon_transition);
// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
&lat_transition,
&lon_transition);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
current_sp.lat = _transition_waypoint(0);
current_sp.lon = _transition_waypoint(1);
} else {
/* reset transition waypoint, will be set upon entering front transition */
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
}
@ -871,16 +858,13 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
const position_setpoint_s &pos_sp_next)
{
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(pos_sp_curr);
_position_sp_type = position_sp_type;
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
|| current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
publishOrbitStatus(current_sp);
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
publishOrbitStatus(pos_sp_curr);
}
switch (position_sp_type) {
@ -891,15 +875,15 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
break;
case position_setpoint_s::SETPOINT_TYPE_POSITION:
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
break;
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
control_auto_velocity(control_interval, curr_pos, ground_speed, pos_sp_curr);
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr, pos_sp_next);
break;
}
@ -917,10 +901,60 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
_att_sp.pitch_body = get_tecs_pitch();
if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(current_sp);
publishLocalPositionSetpoint(pos_sp_curr);
}
}
void FixedwingPositionControl::control_auto_transition_position(const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_curr)
{
set_position_setpoint_for_vtol_transition(pos_sp_curr);
Vector2d curr_wp = Vector2d(_transition_waypoint(0), _transition_waypoint(1));
Vector2d prev_wp{curr_pos};
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
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));
_npfg.setAirspeedNom(_airspeed); // We stop the motor, so use the current airspeed as estimation.
_npfg.setAirspeedMax(_airspeed);
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle
_att_sp.thrust_body[0] = 0.0f;
}
void FixedwingPositionControl::control_auto_transition_fixed()
{
_att_sp.roll_body = radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle
_att_sp.thrust_body[0] = 0.0f;
}
void FixedwingPositionControl::control_manual_transition()
{
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle
_att_sp.thrust_body[0] = 0.0f;
}
void
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
{
@ -2092,23 +2126,14 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
float
FixedwingPositionControl::get_tecs_pitch()
{
if (_tecs_is_running) {
return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
}
return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
// return level flight pitch offset to prevent stale tecs state when it's not running
return radians(_param_fw_psp_off.get());
}
float
FixedwingPositionControl::get_tecs_thrust()
{
if (_tecs_is_running) {
return min(_tecs.get_throttle_setpoint(), 1.f);
}
// return 0 to prevent stale tecs state when it's not running
return 0.0f;
return min(_tecs.get_throttle_setpoint(), 1.f);
}
void
@ -2301,6 +2326,12 @@ FixedwingPositionControl::Run()
_flaps_setpoint = 0.f;
_spoilers_setpoint = 0.f;
/* reset transition waypoint, will be set upon entering transition */
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION) {
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
}
// by default we don't want yaw to be contoller directly with rudder
_att_sp.fw_control_yaw_wheel = false;
@ -2352,6 +2383,16 @@ FixedwingPositionControl::Run()
break;
}
case FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED: {
control_auto_transition_fixed();
break;
}
case FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION: {
control_auto_transition_position(curr_pos, ground_speed, _pos_sp_triplet.current);
break;
}
case FW_POSCTRL_MODE_MANUAL_POSITION: {
control_manual_position(control_interval, curr_pos, ground_speed);
break;
@ -2362,6 +2403,11 @@ FixedwingPositionControl::Run()
break;
}
case FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION: {
control_manual_transition();
break;
}
case FW_POSCTRL_MODE_OTHER: {
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
@ -2508,15 +2554,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
const float desired_max_sinkrate, const float desired_max_climbrate,
bool disable_underspeed_detection, float hgt_rate_sp)
{
_tecs_is_running = true;
// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
// (it should also not run during VTOL blending because airspeed is too low still)
if (_vehicle_status.is_vtol) {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
_tecs_is_running = false;
}
if (_vehicle_status.in_transition_mode) {
// we're in transition
_was_in_transition = true;
@ -2547,12 +2588,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
}
}
if (!_tecs_is_running) {
// next time we run TECS we should reinitialize states
_reinitialize_tecs = true;
return;
}
// We need an altitude lock to calculate the TECS control
if (_local_pos.timestamp == 0) {
_reinitialize_tecs = true;

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

@ -242,8 +242,11 @@ private:
FW_POSCTRL_MODE_AUTO_TAKEOFF,
FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION,
FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED,
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION,
FW_POSCTRL_MODE_OTHER
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
@ -390,7 +393,6 @@ private:
TECS _tecs;
bool _reinitialize_tecs{true};
bool _tecs_is_running{false};
hrt_abstime _time_last_tecs_update{0}; // [us]
// VTOL / TRANSITION
@ -511,12 +513,12 @@ private:
void update_in_air_states(const hrt_abstime now);
/**
* @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL
* @brief Set the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL
* transition.
*
* @param[in,out] current_sp Current position setpoint
*/
void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
void set_position_setpoint_for_vtol_transition(const position_setpoint_s &current_sp);
/**
* @brief Changes the position setpoint type to achieve the desired behavior in some instances.
@ -541,6 +543,26 @@ private:
void control_auto(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 position_setpoint_s &pos_sp_next);
/**
* @brief Automatic position control for VTOL transition
*
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_curr current position setpoint
*/
void control_auto_transition_position(const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_curr);
/**
* @brief Automatic fixed bank control for VTOL transition
*/
void control_auto_transition_fixed();
/**
* @brief Manual control for for VTOL transition
*/
void control_manual_transition();
/**
* @brief Controls altitude and airspeed for a fixed-bank loiter.
*