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 <silvan@auterion.com>

msg/CMakeLists.txt

@@ -109,6 +109,7 @@ set(msg_files
 	IridiumsbdStatus.msg
 	IrlockReport.msg
 	LandingGear.msg
+	LandingGearWheel.msg
 	LandingTargetInnovations.msg
 	LandingTargetPose.msg
 	LedControl.msg

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]

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 <uORB/topics/landing_gear_wheel.h>
+
+/**
+ * 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};
+};

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},

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"

src/lib/mixer_module/output_functions.yaml

@@ -36,6 +36,8 @@ functions:
 
     Gripper: 430
 
+    Landing_Gear_Wheel: 440
+
     # Add your own here:
     #MyCustomFunction: 10000
 

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);
 	}
 

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -64,6 +64,7 @@
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/landing_gear_wheel.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
@@ -138,6 +139,7 @@ private:
 	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
+	uORB::Publication<landing_gear_wheel_s>		_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<float, 3>()};
 

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
  */

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);