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>

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