Skip to content

Commit

Permalink
Address review comments
Browse files Browse the repository at this point in the history 
Address
  • Loading branch information
@Jaeyoung-Lim
Jaeyoung-Lim committed Apr 24, 2022
1 parent f2d14b1 commit 7c90ad7
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 55 deletions.
74 changes: 20 additions & 54 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -831,8 +831,8 @@ 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, 0.01f, 0.05f);
_last_time_position_control_called = now;

if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
Expand Down Expand Up @@ -952,16 +952,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_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();
}

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

if (!_vehicle_status.in_transition_to_fw) {
Expand Down Expand Up @@ -1021,13 +1016,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 @@ -1425,8 +1414,8 @@ void
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 - _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, 0.01f, 0.05f);
_last_time_position_control_called = now;

if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
Expand Down Expand Up @@ -1647,23 +1636,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec

} 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 &= !(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled());

if (use_tecs_pitch) {
// 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();
}

Expand All @@ -1676,8 +1654,8 @@ void
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 - _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, 0.01f, 0.05f);
_last_time_position_control_called = now;

if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
Expand Down Expand Up @@ -2018,22 +1996,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
}

/* 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();
}

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

// flaring during landing
use_tecs_pitch &= !(_land_noreturn_vertical);
// 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 (use_tecs_pitch) {
if (!(_land_noreturn_vertical)) {
_att_sp.pitch_body = get_tecs_pitch();
}

Expand All @@ -2047,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 @@ -2108,8 +2074,8 @@ 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, 0.01f, 0.05f);
_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
16 changes: 15 additions & 1 deletion src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,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 @@ -360,6 +364,16 @@ 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);

/**
* @brief Vehicle control while in takeoff
*
* @param now Current system time [us]
* @param curr_pos Current Local position of vehicle [m]
* @param ground_speed Local 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);
Expand Down

0 comments on commit 7c90ad7

Please sign in to comment.