From 85a5f2dbb41abca8529ebfdaf9e77d794840b0ba Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 20 Dec 2024 15:20:00 +0300 Subject: [PATCH] progress Signed-off-by: RomanBapst --- msg/FwLateralControlSetpoint.msg | 3 ++ .../FwLateralLongitudinalControl.cpp | 27 ++++++++--- .../FwLateralLongitudinalControl.hpp | 1 + .../FixedwingPositionControl.cpp | 48 +++++++++---------- .../FixedwingPositionControl.hpp | 2 +- .../runway_takeoff/RunwayTakeoff.cpp | 20 +++----- .../runway_takeoff/RunwayTakeoff.h | 6 +-- 7 files changed, 58 insertions(+), 49 deletions(-) diff --git a/msg/FwLateralControlSetpoint.msg b/msg/FwLateralControlSetpoint.msg index d7fbdfdd4e6a..44306c09af60 100644 --- a/msg/FwLateralControlSetpoint.msg +++ b/msg/FwLateralControlSetpoint.msg @@ -5,4 +5,7 @@ float32 heading_setpoint float32 lateral_acceleration_setpoint float32 roll_sp # TODO: remove, only for testing +float32 heading_sp_runway_takeoff +bool reset_integral + # TOPICS fw_lateral_control_setpoint fw_lateral_control_status \ No newline at end of file diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 9dfe3f1986d3..36fda2ed1cc0 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -133,6 +133,14 @@ void FwLateralLongitudinalControl::Run() _control_mode_sub.update(); update_control_state(); + if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled + && _local_pos.z_reset_counter != _z_reset_counter) { + if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) { + // make TECS accept step in altitude and demanded altitude + _tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate); + } + } + float pitch_sp{NAN}; float thrust_sp{NAN}; @@ -154,8 +162,8 @@ void FwLateralLongitudinalControl::Run() _long_control_limits_sub.get().disable_underspeed_protection, longitudinal_sp.height_rate_setpoint ); - pitch_sp = _tecs.get_pitch_setpoint(); - thrust_sp = _tecs.get_throttle_setpoint(); + pitch_sp = PX4_ISFINITE(longitudinal_sp.pitch_sp) ? longitudinal_sp.pitch_sp : _tecs.get_pitch_setpoint(); + thrust_sp = PX4_ISFINITE(longitudinal_sp.thrust_sp) ? longitudinal_sp.thrust_sp : _tecs.get_throttle_setpoint(); publish = true; fw_longitudinal_control_setpoint_s longitudinal_control_status { @@ -171,6 +179,7 @@ void FwLateralLongitudinalControl::Run() fw_lateral_control_setpoint_s sp = {empty_lateral_control_setpoint}; float roll_sp {NAN}; + float yaw_sp {NAN}; _fw_lateral_ctrl_sub.copy(&sp); @@ -206,6 +215,10 @@ void FwLateralLongitudinalControl::Run() roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp); } + _att_sp.reset_integral = sp.reset_integral; + _att_sp.fw_control_yaw_wheel = PX4_ISFINITE(sp.heading_sp_runway_takeoff); + yaw_sp = sp.heading_sp_runway_takeoff; + fw_lateral_control_setpoint_s status = { .timestamp = sp.timestamp, .course_setpoint = sp.course_setpoint, @@ -222,8 +235,8 @@ void FwLateralLongitudinalControl::Run() if (publish) { float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f; float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f; - float yaw_body = _yaw; - float thrust_body_x = PX4_ISFINITE(thrust_sp) ? thrust_sp : 0.0f; + const float yaw_body = PX4_ISFINITE(yaw_sp) ? yaw_sp : _yaw; + const float thrust_body_x = PX4_ISFINITE(thrust_sp) ? thrust_sp : 0.0f; if (_control_mode_sub.get().flag_control_manual_enabled) { roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()), @@ -246,11 +259,13 @@ void FwLateralLongitudinalControl::Run() q.copyTo(_att_sp.q_d); _att_sp.thrust_body[0] = thrust_body_x; + _attitude_sp_pub.publish(_att_sp); } } + _z_reset_counter = _local_pos.z_reset_counter; } } void @@ -419,11 +434,11 @@ int FwLateralLongitudinalControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -fw_pos_control is the fixed-wing position controller. +fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("fw_pos_control", "controller"); + PRINT_MODULE_USAGE_NAME("fw_lat_lon_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index c0d6cb562832..8d5ab57651a3 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -160,6 +160,7 @@ class FwLateralLongitudinalControl final : public ModuleBase LandingNudgingOption::kNudgingDisabled) { _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; @@ -1841,9 +1848,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, } else { // follow the glide slope - - /* lateral guidance */ - const PathControllerOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); @@ -1878,9 +1882,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); longitudinal_control_limits.sink_rate_max = desired_max_sinkrate; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); - - // enable direct yaw control using rudder/wheel - _att_sp.fw_control_yaw_wheel = false; } lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; @@ -2130,10 +2131,6 @@ FixedwingPositionControl::Run() // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { - if (_control_mode.flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) { - // make TECS accept step in altitude and demanded altitude - } - // adjust navigation waypoints in position control mode if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled && _local_pos.xy_reset_counter != _xy_reset_counter) { @@ -2385,7 +2382,6 @@ FixedwingPositionControl::Run() _spoilers_setpoint_pub.publish(spoilers_setpoint); } - _z_reset_counter = _local_pos.z_reset_counter; _xy_reset_counter = _local_pos.xy_reset_counter; perf_end(_loop_perf); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 8943b665e54f..674d2073bf25 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -390,7 +390,6 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_psp_off, (ParamFloat) _param_fw_gnd_spd_min, (ParamFloat) _param_fw_pn_r_slew_max, diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp index 7cfc23b4384c..805659c8473f 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp @@ -105,23 +105,23 @@ bool RunwayTakeoff::controlYaw() return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; } -float RunwayTakeoff::getPitch(float external_pitch_setpoint) +float RunwayTakeoff::getPitch() { if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { return math::radians(param_rwto_psp_.get()); } - return external_pitch_setpoint; + return NAN; } -float RunwayTakeoff::getRoll(float external_roll_setpoint) +float RunwayTakeoff::getRoll() { // until we have enough ground clearance, set roll to 0 if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } - return external_roll_setpoint; + return NAN; } float RunwayTakeoff::getYaw(float external_yaw_setpoint) @@ -134,7 +134,7 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint) } } -float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const +float RunwayTakeoff::getThrottle(const float idle_throttle) const { float throttle = idle_throttle; @@ -147,19 +147,13 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - throttle = param_rwto_max_thr_.get(); - - break; - case RunwayTakeoffState::CLIMBOUT: - // ramp in throttle setpoint over takeoff rotation transition time - throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_, - param_rwto_rot_time_.get()); + throttle = param_rwto_max_thr_.get(); break; case RunwayTakeoffState::FLY: - throttle = external_throttle_setpoint; + throttle = NAN; } return throttle; diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h index 35e463077ec3..1907867b6f70 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h @@ -116,13 +116,13 @@ class __EXPORT RunwayTakeoff : public ModuleParams * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad] * @return Pitch angle setpoint (limited while plane is on runway) [rad] */ - float getPitch(float external_pitch_setpoint); + float getPitch(); /** * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad] * @return Roll angle setpoint [rad] */ - float getRoll(float external_roll_setpoint); + float getRoll(); /** * @brief Returns the appropriate yaw angle setpoint. @@ -145,7 +145,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1] * @return Throttle setpoint, normalized [0,1] */ - float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const; + float getThrottle(const float idle_throttle) const; /** * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]