From a787a326e3403d96ffccbad7aaf3492caecb3a93 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 31 Oct 2022 16:56:20 +0100 Subject: [PATCH] Fixed-wing: split out control of steering wheel into seperate message LandingGearWheel Completely detach the steering wheel logic from the yaw controller (beside using the same manual stick input in a manual flight mode). Signed-off-by: Silvan Fuhrer --- msg/CMakeLists.txt | 1 + msg/LandingGearWheel.msg | 3 + .../functions/FunctionLandingGearWheel.hpp | 63 +++++++++++++++++++ src/lib/mixer_module/mixer_module.cpp | 1 + src/lib/mixer_module/mixer_module.hpp | 1 + src/lib/mixer_module/output_functions.yaml | 2 + .../FixedwingAttitudeControl.cpp | 34 +++++++--- .../FixedwingAttitudeControl.hpp | 3 + .../fw_att_control/fw_att_control_params.c | 3 + src/modules/logger/logged_topics.cpp | 1 + 10 files changed, 102 insertions(+), 10 deletions(-) create mode 100644 msg/LandingGearWheel.msg create mode 100644 src/lib/mixer_module/functions/FunctionLandingGearWheel.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 9d5ea63722..00ec9582cd 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -109,6 +109,7 @@ set(msg_files IridiumsbdStatus.msg IrlockReport.msg LandingGear.msg + LandingGearWheel.msg LandingTargetInnovations.msg LandingTargetPose.msg LedControl.msg diff --git a/msg/LandingGearWheel.msg b/msg/LandingGearWheel.msg new file mode 100644 index 0000000000..2ff99fcc56 --- /dev/null +++ b/msg/LandingGearWheel.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_wheel_setpoint # negative is turning left, positive turning right [-1, 1] diff --git a/src/lib/mixer_module/functions/FunctionLandingGearWheel.hpp b/src/lib/mixer_module/functions/FunctionLandingGearWheel.hpp new file mode 100644 index 0000000000..38ee9c4d0a --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionLandingGearWheel.hpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "FunctionProviderBase.hpp" + +#include + +/** + * Functions: Landing_Gear_Wheel + */ +class FunctionLandingGearWheel : public FunctionProviderBase +{ +public: + FunctionLandingGearWheel() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGearWheel(); } + + void update() override + { + landing_gear_wheel_s landing_gear_wheel; + + if (_topic.update(&landing_gear_wheel)) { + _data = landing_gear_wheel.normalized_wheel_setpoint; + } + } + + float value(OutputFunction func) override { return _data; } + +private: + uORB::Subscription _topic{ORB_ID(landing_gear_wheel)}; + float _data{0.f}; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 7a348ed738..29a07ba690 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -59,6 +59,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate}, {OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate}, {OutputFunction::Landing_Gear, &FunctionLandingGear::allocate}, + {OutputFunction::Landing_Gear_Wheel, &FunctionLandingGearWheel::allocate}, {OutputFunction::Parachute, &FunctionParachute::allocate}, {OutputFunction::Gripper, &FunctionGripper::allocate}, {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index f155e43a90..d3dda97e83 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -41,6 +41,7 @@ #include "functions/FunctionGimbal.hpp" #include "functions/FunctionGripper.hpp" #include "functions/FunctionLandingGear.hpp" +#include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" #include "functions/FunctionMotors.hpp" #include "functions/FunctionParachute.hpp" diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 507ee5cf1f..1c57ed9d2a 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -36,6 +36,8 @@ functions: Gripper: 430 + Landing_Gear_Wheel: 440 + # Add your own here: #MyCustomFunction: 10000 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 318fe9b54b..36194a5f24 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -185,6 +185,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r; } } } @@ -518,13 +519,12 @@ void FixedwingAttitudeControl::Run() if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { _roll_ctrl.control_attitude(dt, control_input); _pitch_ctrl.control_attitude(dt, control_input); + _yaw_ctrl.control_attitude(dt, control_input); if (wheel_control) { _wheel_ctrl.control_attitude(dt, control_input); } else { - // runs last, because is depending on output of roll and pitch attitude - _yaw_ctrl.control_attitude(dt, control_input); _wheel_ctrl.reset_integrator(); } @@ -557,7 +557,7 @@ void FixedwingAttitudeControl::Run() /* Publish the rate setpoint for analysis once available */ _rates_sp.roll = body_rates_setpoint(0); _rates_sp.pitch = body_rates_setpoint(1); - _rates_sp.yaw = (wheel_control) ? _wheel_ctrl.get_body_rate_setpoint() : body_rates_setpoint(2); + _rates_sp.yaw = body_rates_setpoint(2); _rates_sp.timestamp = hrt_absolute_time(); @@ -584,19 +584,26 @@ void FixedwingAttitudeControl::Run() _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; - float yaw_u = 0.0f; + const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); + const float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; - if (wheel_control) { - yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input); + // wheel control + float wheel_u = 0.f; - // XXX: this is an abuse -- used to ferry manual yaw inputs from position controller during auto modes - yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get(); + if (_vcontrol_mode.flag_control_manual_enabled) { + // always direct control of steering wheel with yaw stick in manual modes + wheel_u = _manual_control_setpoint.r; } else { - const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); - yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; + // 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 + wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input) + + _att_sp.yaw_sp_move_rate : 0.f; } + _landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f; + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; @@ -604,6 +611,10 @@ void FixedwingAttitudeControl::Run() _rate_control.resetIntegral(); } + if (!PX4_ISFINITE(wheel_u)) { + _wheel_ctrl.reset_integrator(); + } + /* throttle passed through if it is finite */ _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f; @@ -668,6 +679,9 @@ void FixedwingAttitudeControl::Run() } } + _landing_gear_wheel.timestamp = hrt_absolute_time(); + _landing_gear_wheel_pub.publish(_landing_gear_wheel); + updateActuatorControlsStatus(dt); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index c40517a91c..ea6f9535f2 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -138,6 +139,7 @@ private: uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)}; actuator_controls_s _actuator_controls{}; manual_control_setpoint_s _manual_control_setpoint{}; @@ -146,6 +148,7 @@ private: vehicle_local_position_s _local_pos{}; vehicle_rates_setpoint_s _rates_sp{}; vehicle_status_s _vehicle_status{}; + landing_gear_wheel_s _landing_gear_wheel{}; matrix::Dcmf _R{matrix::eye()}; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 57d31c3e44..c43a678564 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -329,6 +329,9 @@ PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); /** * Enable wheel steering controller * + * Only enabled during automatic runway takeoff and landing. + * In all manual modes the wheel is directly controlled with yaw stick. + * * @boolean * @group FW Attitude Control */ diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 40adfede97..1374444f1f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -75,6 +75,7 @@ void LoggedTopics::add_default_topics() add_topic("input_rc", 500); add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("irlock_report", 1000); + add_topic("landing_gear_wheel", 10); add_optional_topic("landing_target_pose", 1000); add_optional_topic("magnetometer_bias_estimate", 200); add_topic("manual_control_setpoint", 200);