Skip to content

Commit

Permalink
Separate takeoff and landing to individual fixed wing states for FW p…
Browse files Browse the repository at this point in the history 
…os control (#19495)
  • Loading branch information
@Jaeyoung-Lim
Jaeyoung-Lim committed Apr 26, 2022
1 parent d6210d1 commit c6ab4c4
Show file tree
Hide file tree
Showing 2 changed files with 125 additions and 68 deletions.
161 changes: 100 additions & 61 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
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) {

_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
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,
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

0 comments on commit c6ab4c4

Please sign in to comment.