From f2d14b18168b7d964a0cd655ac53c831d02d70b0 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sun, 17 Apr 2022 23:40:59 +0200 Subject: [PATCH 1/8] Separate takeoff and landing to individual fixedwing states --- .../FixedwingPositionControl.cpp | 152 +++++++++++++----- .../FixedwingPositionControl.hpp | 13 +- 2 files changed, 119 insertions(+), 46 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 905667253dc3..6899c0b164f1 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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) { @@ -913,14 +929,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 */ @@ -939,21 +947,7 @@ 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; @@ -968,20 +962,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c } } - // 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(); - } + _att_sp.pitch_body = get_tecs_pitch(); if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); @@ -1441,9 +1422,19 @@ 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 - _control_position_last_called) * 1e-6f, 0.01f, 0.05f); + _control_position_last_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 */ @@ -1642,12 +1633,59 @@ 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 */ + 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; + + // auto runway takeoff + use_tecs_pitch &= !(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()); + + if (use_tecs_pitch) { + _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 - _control_position_last_called) * 1e-6f, 0.01f, 0.05f); + _control_position_last_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 */ @@ -1978,6 +2016,30 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo false, radians(_param_fw_p_lim_min.get())); } + + /* 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); + + if (use_tecs_pitch) { + _att_sp.pitch_body = get_tecs_pitch(); + } + + if (!_vehicle_status.in_transition_to_fw) { + publishLocalPositionSetpoint(pos_sp_curr); + } } void @@ -2379,6 +2441,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; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index d86211e5689d..6cccf4a276c9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -279,6 +279,8 @@ class FixedwingPositionControl final : public ModuleBase Date: Sun, 24 Apr 2022 15:44:13 +0200 Subject: [PATCH 2/8] Address review comments Address --- .../FixedwingPositionControl.cpp | 77 +++++-------------- .../FixedwingPositionControl.hpp | 16 +++- 2 files changed, 35 insertions(+), 58 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 6899c0b164f1..e4b8948cc004 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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); @@ -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) { @@ -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(); } @@ -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); @@ -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(); } @@ -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); @@ -2015,27 +1993,12 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec _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 */ - 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); - - if (use_tecs_pitch) { - _att_sp.pitch_body = get_tecs_pitch(); - } + // 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); @@ -2047,7 +2010,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(); @@ -2108,8 +2071,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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 6cccf4a276c9..9b7cd8f41c81 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -195,7 +195,11 @@ class FixedwingPositionControl final : public ModuleBase Date: Sun, 24 Apr 2022 15:50:29 +0200 Subject: [PATCH 3/8] constexpr Min and max timesteps for auto modes --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 14 +++++++++----- .../fw_pos_control_l1/FixedwingPositionControl.hpp | 3 +++ 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e4b8948cc004..2607a5ca7b45 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -831,7 +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 - _last_time_position_control_called) * 1e-6f, 0.01f, 0.05f); + 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()) { @@ -1414,7 +1415,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 - _last_time_position_control_called) * 1e-6f, 0.01f, 0.05f); + 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()) { @@ -1654,7 +1656,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 - _last_time_position_control_called) * 1e-6f, 0.01f, 0.05f); + 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()) { @@ -2071,7 +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 - _last_time_position_control_called) * 1e-6f, 0.01f, 0.05f); + 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 @@ -2549,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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 9b7cd8f41c81..3db32ef3e456 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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.01; +static constexpr float MAX_AUTO_TIMESTEP = 0.05; + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { From 6a3fb87689a80cdafa505b37d3f2332b448ba5cc Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sun, 24 Apr 2022 18:34:27 +0200 Subject: [PATCH 4/8] Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp Co-authored-by: Thomas Stastny --- src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 3db32ef3e456..1067d21a85b4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -120,8 +120,8 @@ 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.01; -static constexpr float MAX_AUTO_TIMESTEP = 0.05; +static constexpr float MIN_AUTO_TIMESTEP = 0.01; // minimum time step between auto control updates [s] +static constexpr float MAX_AUTO_TIMESTEP = 0.05; // maximum time step between auto control updates [s] class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem From 92215d7815d47f5446b23251af33fc46c8ad1fce Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 25 Apr 2022 19:48:50 +0200 Subject: [PATCH 5/8] Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp Co-authored-by: Silvan Fuhrer --- src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1067d21a85b4..ab24a9c1236e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -373,7 +373,7 @@ class FixedwingPositionControl final : public ModuleBase Date: Mon, 25 Apr 2022 19:48:59 +0200 Subject: [PATCH 6/8] Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp Co-authored-by: Silvan Fuhrer --- src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index ab24a9c1236e..71a0cf284d82 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -120,7 +120,7 @@ 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.01; // minimum time step between auto control updates [s] +static constexpr float MIN_AUTO_TIMESTEP = 0.01f; // minimum time step between auto control updates [s] static constexpr float MAX_AUTO_TIMESTEP = 0.05; // maximum time step between auto control updates [s] class FixedwingPositionControl final : public ModuleBase, public ModuleParams, From 02c67ede344fcad698400dbafaf84ab457c3568d Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 25 Apr 2022 19:49:11 +0200 Subject: [PATCH 7/8] Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp Co-authored-by: Silvan Fuhrer --- src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 71a0cf284d82..87bc0ff4ecff 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -372,7 +372,7 @@ class FixedwingPositionControl final : public ModuleBase Date: Mon, 25 Apr 2022 19:49:25 +0200 Subject: [PATCH 8/8] Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp Co-authored-by: Silvan Fuhrer --- src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 87bc0ff4ecff..beeb910c8ffe 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -121,7 +121,7 @@ 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.05; // maximum 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, public ModuleParams, public px4::WorkItem