FW Attitude Controller: Wheel controller rework

add RunwayControl messge to pass wheel steering controls to wheel controller

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Runway takeoff: specify that RWTO_TKOFF is directly applied during takeoff

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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

wheel rate controller: use speed scaler quadratically on integrator

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

wheel yaw controller: use a time constant of 0.1

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

FW Attitude Controller: lock heading setpoint for wheels to initial heading

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/CMakeLists.txt

@@ -94,6 +94,7 @@ set(msg_files
 	FixedWingLongitudinalSetpoint.msg
 	FixedWingLateralGuidanceStatus.msg
 	FixedWingLateralStatus.msg
+	FixedWingRunwayControl.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GeofenceStatus.msg

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.

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

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
+		// 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;
+
+		float groundspeed_scale = 1.f;
 		float wheel_u = 0.f;
 
-			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;
+		if (wheel_controller_enabled) {
+			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;
+
+			}
+
+			// 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());
 
-			} else {
 			vehicle_angular_velocity_s angular_velocity{};
 			_vehicle_rates_sub.copy(&angular_velocity);
 
-				// 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;
+			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 {
+			_wheel_ctrl.reset_integrator();
+			_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);
-
-		} 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);
-
-		}
 	}
 
 	// backup schedule

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -54,6 +54,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/autotune_attitude_control_status.h>
+#include <uORB/topics/fixed_wing_runway_control.h>
 #include <uORB/topics/landing_gear_wheel.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
@@ -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<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,

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

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

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -68,6 +68,7 @@
 #include <uORB/topics/fixed_wing_lateral_setpoint.h>
 #include <uORB/topics/fixed_wing_lateral_guidance_status.h>
 #include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
+#include <uORB/topics/fixed_wing_runway_control.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/launch_detection_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -198,6 +199,7 @@ private:
 	uORB::PublicationData<fixed_wing_lateral_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)};
 	uORB::PublicationData<fixed_wing_longitudinal_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)};
 	uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
+	uORB::Publication<fixed_wing_runway_control_s> _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{};

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

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