Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Separate takeoff and landing to individual fixed wing states for FW pos control #19495

Merged
merged 8 commits into from
Apr 26, 2022
Merged

Separate takeoff and landing to individual fixed wing states for FW pos control #19495

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
f2d14b1
Separate takeoff and landing to individual fixedwing states
Jaeyoung-Lim Apr 17, 2022
47e65c4
Address review comments
Jaeyoung-Lim Apr 24, 2022
c00ddc4
constexpr Min and max timesteps for auto modes
Jaeyoung-Lim Apr 24, 2022
6a3fb87
Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
Jaeyoung-Lim Apr 24, 2022
92215d7
Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
Jaeyoung-Lim Apr 25, 2022
cebe989
Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
Jaeyoung-Lim Apr 25, 2022
02c67ed
Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
Jaeyoung-Lim Apr 25, 2022
8b6b80c
Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
Jaeyoung-Lim Apr 25, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
161 changes: 100 additions & 61 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -755,7 +755,23 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool

if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
_control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// TAKEOFF: handle like a regular POSITION setpoint if already flying
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
_control_mode_current = FW_POSCTRL_MODE_AUTO;

} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
}

} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND
&& !_vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;

} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
}

} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled
&& pos_sp_curr_valid) {
Expand Down Expand Up @@ -815,8 +831,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
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)
{
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
_control_position_last_called = now;
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;

if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
Expand Down Expand Up @@ -913,14 +930,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
case position_setpoint_s::SETPOINT_TYPE_LOITER:
control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
break;

case position_setpoint_s::SETPOINT_TYPE_LAND:
control_auto_landing(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;

case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
control_auto_takeoff(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
break;
}

/* reset landing state */
Expand All @@ -939,49 +948,17 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
}

/* Copy thrust output for publication, handle special cases */
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && // launchdetector only available in auto
_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
!_runway_takeoff.runwayTakeoffEnabled()) {

/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons.
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
_att_sp.thrust_body[0] = _param_fw_thr_idle.get();

} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {

_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());

} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
tstastny marked this conversation as resolved.
Show resolved Hide resolved

_att_sp.thrust_body[0] = 0.0f;

} else {
/* Copy thrust and pitch values from tecs */
if (_landed) {
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), 1.f);

} else {
_att_sp.thrust_body[0] = get_tecs_thrust();
}
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
}

// decide when to use pitch setpoint from TECS because in some cases pitch
// setpoint is generated by other means
bool use_tecs_pitch = true;

// auto runway takeoff
use_tecs_pitch &= !(position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));

// flaring during landing
use_tecs_pitch &= !(position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);

if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}
/* Copy thrust and pitch values from tecs */
_att_sp.pitch_body = get_tecs_pitch();

if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(current_sp);
Expand Down Expand Up @@ -1040,13 +1017,7 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
_att_sp.yaw_body = 0.f;

if (_landed) {
_att_sp.thrust_body[0] = _param_fw_thr_min.get();

} else {
_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.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();
}

Expand Down Expand Up @@ -1441,9 +1412,20 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
}

void
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;

if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);

} else {
_l1_control.set_dt(dt);
}

/* current waypoint (the one currently heading for) */
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
Vector2d prev_wp{0, 0}; /* previous waypoint */
Expand Down Expand Up @@ -1642,12 +1624,49 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
}
}

if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) {

/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons.
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
_att_sp.thrust_body[0] = _param_fw_thr_idle.get();

} else if (_runway_takeoff.runwayTakeoffEnabled()) {

_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());

} else {
/* Copy thrust and pitch values from tecs */
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
}

// only use TECS pitch setpoint if launch has not been detected and runway takeoff is not enabled
if (!(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())) {
_att_sp.pitch_body = get_tecs_pitch();
}

if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(pos_sp_curr);
}
}

void
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;

if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);

} else {
_l1_control.set_dt(dt);
}

/* current waypoint (the one currently heading for) */
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
Vector2d prev_wp{0, 0}; /* previous waypoint */
Expand Down Expand Up @@ -1977,6 +1996,15 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
_param_fw_thr_cruise.get(),
false,
radians(_param_fw_p_lim_min.get()));
_att_sp.pitch_body = get_tecs_pitch();
}

/* Copy thrust and pitch values from tecs */
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();

if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(pos_sp_curr);
}
}

Expand All @@ -1985,7 +2013,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
const Vector2f &ground_speed)
{
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
_control_position_last_called = now;
_last_time_position_control_called = now;

/* Get demanded airspeed */
float altctrl_airspeed = get_manual_airspeed_setpoint();
Expand Down Expand Up @@ -2046,8 +2074,9 @@ void
FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed)
{
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
_control_position_last_called = now;
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;

// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
Expand Down Expand Up @@ -2379,6 +2408,16 @@ FixedwingPositionControl::Run()
break;
}

case FW_POSCTRL_MODE_AUTO_LANDING: {
control_auto_landing(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
break;
}

case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
control_auto_takeoff(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
break;
}

case FW_POSCTRL_MODE_MANUAL_POSITION: {
control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
break;
Expand Down Expand Up @@ -2514,7 +2553,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
bool climbout_mode, float climbout_pitch_min_rad,
bool disable_underspeed_detection, float hgt_rate_sp)
{
const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
_last_tecs_update = now;

// do not run TECS if we are not in air
Expand Down
32 changes: 25 additions & 7 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,9 @@ static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed
static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
10_s; // time after which the wind estimate is disabled if no longer updating

static constexpr float MIN_AUTO_TIMESTEP = 0.01f; // minimum time step between auto control updates [s]
static constexpr float MAX_AUTO_TIMESTEP = 0.05f; // maximum time step between auto control updates [s]

class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
{
Expand Down Expand Up @@ -195,7 +198,11 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
position_setpoint_s _hdg_hold_prev_wp {}; ///< position where heading hold started
position_setpoint_s _hdg_hold_curr_wp {}; ///< position to which heading hold flies

hrt_abstime _control_position_last_called{0}; ///< last call of control_position
/**
* @brief Last absolute time position control has been called [us]
*
*/
hrt_abstime _last_time_position_control_called{0};

bool _landed{true};

Expand Down Expand Up @@ -279,6 +286,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_AUTO_ALTITUDE,
FW_POSCTRL_MODE_AUTO_CLIMBRATE,
FW_POSCTRL_MODE_AUTO_TAKEOFF,
FW_POSCTRL_MODE_AUTO_LANDING,
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_OTHER
Expand Down Expand Up @@ -358,11 +367,20 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
void control_auto_velocity(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);
void control_auto_takeoff(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,

/**
* @brief Vehicle control while in takeoff
*
* @param now Current system time [us]
* @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_prev previous position setpoint
* @param pos_sp_curr current position setpoint
*/
void control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_auto_landing(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
Expand All @@ -373,8 +391,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
float get_tecs_thrust();

float get_manual_airspeed_setpoint();
float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed,
const Vector2f &ground_speed, float dt);
float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed,
float dt);

void reset_takeoff_state(bool force = false);
void reset_landing_state();
Expand Down