From 26009461728de880923f803d087404d4a326f271 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 11 Apr 2025 17:08:28 +0200 Subject: [PATCH] FW Attitude Controller: Wheel controller rework add RunwayControl messge to pass wheel steering controls to wheel controller Signed-off-by: Silvan Fuhrer Runway takeoff: specify that RWTO_TKOFF is directly applied during takeoff Signed-off-by: Silvan Fuhrer msg/RateCtrlStatus: remove unused wheel_rate_integ field The wheel rate controller is not run in the moduels that are now running the MC/FW rate controllers, so thsi field canot be filled. Signed-off-by: Silvan Fuhrer wheel rate controller: use speed scaler quadratically on integrator Signed-off-by: Silvan Fuhrer wheel yaw controller: use a time constant of 0.1 Signed-off-by: Silvan Fuhrer FW Attitude Controller: lock heading setpoint for wheels to initial heading Signed-off-by: Silvan Fuhrer --- msg/CMakeLists.txt | 1 + msg/FixedWingRunwayControl.msg | 8 ++ msg/RateCtrlStatus.msg | 1 - .../FixedwingAttitudeControl.cpp | 106 ++++++++---------- .../FixedwingAttitudeControl.hpp | 3 + .../fw_att_control/fw_wheel_controller.cpp | 4 +- .../FixedwingPositionControl.cpp | 32 ++++-- .../FixedwingPositionControl.hpp | 2 + .../runway_takeoff/runway_takeoff_params.c | 2 +- src/modules/logger/logged_topics.cpp | 1 + 10 files changed, 89 insertions(+), 71 deletions(-) create mode 100644 msg/FixedWingRunwayControl.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index e3008ae917..29af39b27f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -94,6 +94,7 @@ set(msg_files FixedWingLongitudinalSetpoint.msg FixedWingLateralGuidanceStatus.msg FixedWingLateralStatus.msg + FixedWingRunwayControl.msg GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg diff --git a/msg/FixedWingRunwayControl.msg b/msg/FixedWingRunwayControl.msg new file mode 100644 index 0000000000..a74d702c10 --- /dev/null +++ b/msg/FixedWingRunwayControl.msg @@ -0,0 +1,8 @@ +# Auxiliary control fields for fixed-wing runway takeoff/landing + +# Passes information from the FixedWingModeManager to the FixedWingAttitudeController + +uint64 timestamp # [us] time since system start + +bool wheel_steering_enabled # Flag that enables the wheel steering. +float32 wheel_steering_nudging_rate # [norm] [-1, 1] [FRD] Manual wheel nudging, added to controller output. NAN is interpreted as 0. diff --git a/msg/RateCtrlStatus.msg b/msg/RateCtrlStatus.msg index 3f5644699d..b1e4d4feb1 100644 --- a/msg/RateCtrlStatus.msg +++ b/msg/RateCtrlStatus.msg @@ -4,4 +4,3 @@ uint64 timestamp # time since system start (microseconds) float32 rollspeed_integ float32 pitchspeed_integ float32 yawspeed_integ -float32 wheel_rate_integ # FW only and optional diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c9a6d921fc..f9328635e6 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -47,6 +47,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : { /* fetch initial parameter values */ parameters_update(); + _landing_gear_wheel_pub.advertise(); } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -82,6 +83,7 @@ FixedwingAttitudeControl::parameters_update() _wheel_ctrl.set_k_ff(_param_fw_wr_ff.get()); _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get()); _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get())); + _wheel_ctrl.set_time_constant(0.1f); } void @@ -267,13 +269,6 @@ void FixedwingAttitudeControl::Run() vehicle_land_detected_poll(); - bool wheel_control = false; - - // TODO listen to a runway_takeoff_status to determine when to control wheel - if (_param_fw_w_en.get() && _vcontrol_mode.flag_control_auto_enabled) { - wheel_control = true; - } - /* if we are in rotary wing mode, do nothing */ if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { perf_end(_loop_perf); @@ -295,27 +290,6 @@ void FixedwingAttitudeControl::Run() _rates_sp.reset_integral = false; } - float groundspeed_scale = 1.f; - - if (wheel_control) { - if (_local_pos_sub.updated()) { - vehicle_local_position_s vehicle_local_position; - - if (_local_pos_sub.copy(&vehicle_local_position)) { - _groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy * - vehicle_local_position.vy); - } - } - - // Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim - float gspd_scaling_trim = (_param_fw_airspd_stall.get()); - - if (_groundspeed > gspd_scaling_trim) { - groundspeed_scale = gspd_scaling_trim / _groundspeed; - - } - } - /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { @@ -333,13 +307,6 @@ void FixedwingAttitudeControl::Run() _yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); - if (wheel_control) { - _wheel_ctrl.control_attitude(euler_sp.psi(), euler_angles.psi()); - - } else { - _wheel_ctrl.reset_integrator(); - } - /* Update input data for rate controllers */ Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(), _yaw_ctrl.get_body_rate_setpoint()); @@ -380,40 +347,61 @@ void FixedwingAttitudeControl::Run() _rate_sp_pub.publish(_rates_sp); } } + } - // wheel control - float wheel_u = 0.f; + // steering wheel control + fixed_wing_runway_control_s runway_control{}; + _fixed_wing_runway_control_sub.copy(&runway_control); + const bool runway_control_recent = hrt_elapsed_time(&runway_control.timestamp) < 1_s; + const bool wheel_controller_enabled = _param_fw_w_en.get() && _vcontrol_mode.flag_control_auto_enabled + && runway_control_recent && runway_control.wheel_steering_enabled; - if (_vcontrol_mode.flag_control_manual_enabled) { - // always direct control of steering wheel with yaw stick in manual modes - wheel_u = _manual_control_setpoint.yaw; + float groundspeed_scale = 1.f; + float wheel_u = 0.f; - } else { - vehicle_angular_velocity_s angular_velocity{}; - _vehicle_rates_sub.copy(&angular_velocity); + if (wheel_controller_enabled) { + if (_local_pos_sub.updated()) { + vehicle_local_position_s vehicle_local_position; - // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from - // position controller during auto modes _manual_control_setpoint.r gets passed - // whenever nudging is enabled, otherwise zero - const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, angular_velocity.xyz[2], _groundspeed, - groundspeed_scale); - wheel_u = wheel_control ? wheel_controller_output + _att_sp.yaw_sp_move_rate : 0.f; + if (_local_pos_sub.copy(&vehicle_local_position)) { + _groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy * + vehicle_local_position.vy); + } } - _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; - _landing_gear_wheel.timestamp = hrt_absolute_time(); - _landing_gear_wheel_pub.publish(_landing_gear_wheel); + // Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim + float gspd_scaling_trim = (_param_fw_airspd_stall.get()); + + if (_groundspeed > gspd_scaling_trim) { + groundspeed_scale = gspd_scaling_trim / _groundspeed; + + } + + // set now yaw setpoint once we're entering the first time + if (!PX4_ISFINITE(_steering_wheel_yaw_setpoint)) { + _steering_wheel_yaw_setpoint = euler_angles.psi(); + } + + _wheel_ctrl.control_attitude(_steering_wheel_yaw_setpoint, euler_angles.psi()); + + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + + const float wheel_controller_output = wheel_controller_enabled ? _wheel_ctrl.control_bodyrate(dt, + angular_velocity.xyz[2], _groundspeed, + groundspeed_scale) : 0.f; + + wheel_u = wheel_controller_output + runway_control.wheel_steering_nudging_rate; } else { - // full manual _wheel_ctrl.reset_integrator(); - - _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(_manual_control_setpoint.yaw) ? - _manual_control_setpoint.yaw : 0.f; - _landing_gear_wheel.timestamp = hrt_absolute_time(); - _landing_gear_wheel_pub.publish(_landing_gear_wheel); - + _steering_wheel_yaw_setpoint = NAN; + wheel_u = _manual_control_setpoint.yaw; // direct yaw stick to wheel steering } + + _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; + _landing_gear_wheel.timestamp = hrt_absolute_time(); + _landing_gear_wheel_pub.publish(_landing_gear_wheel); } // backup schedule diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 43432a4a3a..73a08c00e0 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -100,6 +101,7 @@ private: uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; + uORB::Subscription _fixed_wing_runway_control_sub{ORB_ID(fixed_wing_runway_control)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ @@ -129,6 +131,7 @@ private: bool _landed{true}; float _groundspeed{0.f}; bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states + float _steering_wheel_yaw_setpoint{NAN}; DEFINE_PARAMETERS( (ParamFloat) _param_fw_airspd_max, diff --git a/src/modules/fw_att_control/fw_wheel_controller.cpp b/src/modules/fw_att_control/fw_wheel_controller.cpp index e26d9a5d38..3868a97369 100644 --- a/src/modules/fw_att_control/fw_wheel_controller.cpp +++ b/src/modules/fw_att_control/fw_wheel_controller.cpp @@ -58,7 +58,7 @@ float WheelController::control_bodyrate(float dt, float body_z_rate, float groun if (_k_i > 0.f && groundspeed > 1.f) { // only start integrating when above 1m/s - float id = rate_error * dt * groundspeed_scaler; + float id = rate_error * dt * groundspeed_scaler * groundspeed_scaler; if (_last_output < -1.f) { /* only allow motion to center: increase value */ @@ -74,7 +74,7 @@ float WheelController::control_bodyrate(float dt, float body_z_rate, float groun /* Apply PI rate controller and store non-limited output */ _last_output = _body_rate_setpoint * _k_ff * groundspeed_scaler + - groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p + _integrator); + groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p) + _integrator; return math::constrain(_last_output, -1.f, 1.f); } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 672a718dc8..fc19682529 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -71,6 +71,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _flaps_setpoint_pub.advertise(); _spoilers_setpoint_pub.advertise(); _fixed_wing_lateral_guidance_status_pub.advertise(); + _fixed_wing_runway_control_pub.advertise(); parameters_update(); } @@ -1094,11 +1095,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _runway_takeoff.update(now, takeoff_airspeed, _airspeed_eas, _current_altitude - _takeoff_ground_alt, clearance_altitude_amsl - _takeoff_ground_alt); - // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. - if (_param_rwto_nudge.get()) { - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; - } - const Vector2f start_pos_local = _global_local_proj_ref.project(_takeoff_init_position(0), _takeoff_init_position(1)); const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); @@ -1154,6 +1150,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _new_landing_gear_position = landing_gear_s::GEAR_UP; } + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f; + + _fixed_wing_runway_control_pub.publish(fw_runway_control); + } else { /* Perform launch detection */ if (!_skipping_takeoff_detection && _param_fw_laun_detcn_on.get() && @@ -1440,6 +1443,14 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _ctrl_configuration_handler.setSinkRateTarget(desired_max_sinkrate); } + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? + _manual_control_setpoint.yaw : 0.f; + + _fixed_wing_runway_control_pub.publish(fw_runway_control); + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1595,6 +1606,14 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _ctrl_configuration_handler.setSinkRateTarget(desired_max_sinkrate); } + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ? + _manual_control_setpoint.yaw : 0.f; + + _fixed_wing_runway_control_pub.publish(fw_runway_control); + _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(getMaxRollAngleNearGround(_current_altitude, terrain_alt))); @@ -2002,9 +2021,6 @@ FixedwingPositionControl::Run() // by default set speed weight to the param value, can be overwritten inside the methods below _ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get()); - // default to zero - is used (IN A HACKY WAY) to pass direct nose wheel steering via yaw stick to the actuators during auto takeoff - _att_sp.yaw_sp_move_rate = 0.0f; - if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT && _control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR) { reset_landing_state(); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index aea0dd5a89..33a0cd87e7 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -198,6 +199,7 @@ private: uORB::PublicationData _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)}; uORB::PublicationData _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)}; uORB::Publication _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)}; + uORB::Publication _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)}; manual_control_setpoint_s _manual_control_setpoint{}; position_setpoint_triplet_s _pos_sp_triplet{}; diff --git a/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c index e33fa9d616..68afdeffa1 100644 --- a/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c @@ -48,7 +48,7 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); /** - * Max throttle during runway takeoff. + * Throttle during runway takeoff. * * @unit norm * @min 0.0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 691c57adb4..3d011cbc5c 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -154,6 +154,7 @@ void LoggedTopics::add_default_topics() add_topic("lateral_control_configuration"); add_optional_topic("fixed_wing_lateral_guidance_status", 100); add_optional_topic("fixed_wing_lateral_status", 100); + add_optional_topic("fixed_wing_runway_control", 100); // multi topics add_optional_topic_multi("actuator_outputs", 100, 3);