progress

Signed-off-by: RomanBapst <[email protected]>

msg/FwLateralControlSetpoint.msg

@@ -5,4 +5,7 @@ float32 heading_setpoint
 float32 lateral_acceleration_setpoint
 float32 roll_sp # TODO: remove, only for testing
 
+float32 heading_sp_runway_takeoff
+bool reset_integral
+
 # TOPICS fw_lateral_control_setpoint fw_lateral_control_status

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -133,6 +133,14 @@ void FwLateralLongitudinalControl::Run()
 		_control_mode_sub.update();
 		update_control_state();
 
+		if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
+		    && _local_pos.z_reset_counter != _z_reset_counter) {
+			if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
+				// make TECS accept step in altitude and demanded altitude
+				_tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate);
+			}
+		}
+
 		float pitch_sp{NAN};
 		float thrust_sp{NAN};
 
@@ -154,8 +162,8 @@ void FwLateralLongitudinalControl::Run()
 						   _long_control_limits_sub.get().disable_underspeed_protection,
 						   longitudinal_sp.height_rate_setpoint
 						  );
-			pitch_sp = _tecs.get_pitch_setpoint();
-			thrust_sp = _tecs.get_throttle_setpoint();
+			pitch_sp = PX4_ISFINITE(longitudinal_sp.pitch_sp) ? longitudinal_sp.pitch_sp : _tecs.get_pitch_setpoint();
+			thrust_sp = PX4_ISFINITE(longitudinal_sp.thrust_sp) ? longitudinal_sp.thrust_sp : _tecs.get_throttle_setpoint();
 			publish = true;
 
 			fw_longitudinal_control_setpoint_s longitudinal_control_status {
@@ -171,6 +179,7 @@ void FwLateralLongitudinalControl::Run()
 
 		fw_lateral_control_setpoint_s sp = {empty_lateral_control_setpoint};
 		float roll_sp {NAN};
+		float yaw_sp {NAN};
 
 		_fw_lateral_ctrl_sub.copy(&sp);
 
@@ -206,6 +215,10 @@ void FwLateralLongitudinalControl::Run()
 				roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);
 			}
 
+			_att_sp.reset_integral = sp.reset_integral;
+			_att_sp.fw_control_yaw_wheel = PX4_ISFINITE(sp.heading_sp_runway_takeoff);
+			yaw_sp = sp.heading_sp_runway_takeoff;
+
 			fw_lateral_control_setpoint_s status = {
 				.timestamp = sp.timestamp,
 				.course_setpoint = sp.course_setpoint,
@@ -222,8 +235,8 @@ void FwLateralLongitudinalControl::Run()
 		if (publish) {
 			float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
 			float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
-			float yaw_body = _yaw;
-			float thrust_body_x = PX4_ISFINITE(thrust_sp) ? thrust_sp : 0.0f;
+			const float yaw_body = PX4_ISFINITE(yaw_sp) ? yaw_sp : _yaw;
+			const float thrust_body_x = PX4_ISFINITE(thrust_sp) ? thrust_sp : 0.0f;
 
 			if (_control_mode_sub.get().flag_control_manual_enabled) {
 				roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
@@ -246,11 +259,13 @@ void FwLateralLongitudinalControl::Run()
 				q.copyTo(_att_sp.q_d);
 
 				_att_sp.thrust_body[0] = thrust_body_x;
+
 				_attitude_sp_pub.publish(_att_sp);
 			}
 
 		}
 
+		_z_reset_counter = _local_pos.z_reset_counter;
 	}
 }
 void
@@ -419,11 +434,11 @@ int FwLateralLongitudinalControl::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-fw_pos_control is the fixed-wing position controller.
+fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints.
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("fw_pos_control", "controller");
+	PRINT_MODULE_USAGE_NAME("fw_lat_lon_control", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp

@@ -160,6 +160,7 @@ class FwLateralLongitudinalControl final : public ModuleBase<FwLateralLongitudin
 
 
 	hrt_abstime _last_time_loop_ran{};
+	uint8_t _z_reset_counter{0};
 	bool _tecs_is_running{false};
 	bool _airspeed_valid{false};
 	uint64_t _time_airspeed_last_valid{0};

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -48,7 +48,7 @@ using matrix::Vector2d;
 using matrix::Vector3f;
 using matrix::wrap_pi;
 
-const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .heading_setpoint = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN};
+const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .heading_setpoint = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN, .heading_sp_runway_takeoff = NAN, .reset_integral = false};
 const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .height_rate_setpoint = NAN, .altitude_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN};
 FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	ModuleParams(nullptr),
@@ -739,6 +739,8 @@ void FixedwingPositionControl::control_idle()
 	_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
 
 	fw_longitudinal_control_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint};
+	long_contrl_sp.pitch_sp = math::radians(_param_fw_psp_off.get());
+	long_contrl_sp.thrust_sp = 0.0f;
 	long_contrl_sp.timestamp = now;
 	_longitudinal_ctrl_sp_pub.publish(long_contrl_sp);
 
@@ -1323,28 +1325,30 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 		fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint;
 		fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
-
+		fw_lateral_ctrl_sp.reset_integral = _runway_takeoff.resetIntegrators();
+		fw_lateral_ctrl_sp.heading_sp_runway_takeoff = _runway_takeoff.controlYaw() ? _runway_takeoff.getYaw(
+					sp.course_setpoint) : NAN;
+		fw_lateral_ctrl_sp.roll_sp = _runway_takeoff.getRoll();
 		_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
-
+		const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt);
+		lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+		lateral_limits.lateral_accel_max = rollAngleToLateralAccel(roll_wingtip_strike);
+		_lateral_ctrl_limits_pub.publish(lateral_limits);
 
 		// update tecs
 		const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
 		const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()),
 					math::radians(_param_fw_p_lim_min.get()));
 
-		if (_runway_takeoff.resetIntegrators()) {
-			// reset integrals except yaw (which also counts for the wheel controller)
-			_att_sp.reset_integral = true;
-
-			// throttle is open loop anyway during ground roll, no need to wind up the integrator
-		}
 
 		const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = {
 			.timestamp = hrt_absolute_time(),
 			.height_rate_setpoint = NAN,
 			.altitude_setpoint = altitude_setpoint_amsl,
 			.equivalent_airspeed_setpoint = target_airspeed,
+			.pitch_sp = _runway_takeoff.getPitch(),
+			.thrust_sp = _runway_takeoff.getThrottle(_param_fw_thr_idle.get())
 		};
 
 		_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
@@ -1421,6 +1425,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
+			const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt);
+			lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
+			lateral_limits.lateral_accel_max = rollAngleToLateralAccel(roll_wingtip_strike);
+			_lateral_ctrl_limits_pub.publish(lateral_limits);
+
 			const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ?
 							   _param_fw_thr_idle.get() : _param_fw_thr_max.get();
 			const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = {
@@ -1443,11 +1452,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			//float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 		} else {
-			/* Tell the attitude controller to stop integrating while we are waiting for the launch */
-			_att_sp.reset_integral = true;
 			fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
 			fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 			fw_lateral_ctrl_sp.lateral_acceleration_setpoint = 0.f;
+			/* Tell the attitude controller to stop integrating while we are waiting for the launch */
+			fw_lateral_ctrl_sp.reset_integral = true;
 			_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
 			fw_longitudinal_control_setpoint_s long_control_sp{empty_longitudinal_control_setpoint};
@@ -1568,6 +1577,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 		fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint;
 		fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
+		fw_lateral_ctrl_sp.heading_sp_runway_takeoff = sp.course_setpoint;
 		_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 
 		const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, terrain_alt);
@@ -1687,7 +1697,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		_att_sp.fw_control_yaw_wheel = false;
 	}
 
-
 	_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
 	_spoilers_setpoint = _param_fw_spoilers_lnd.get();
 
@@ -1774,6 +1783,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 		fw_lateral_ctrl_sp.course_setpoint = sp.course_setpoint;
 		fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward;
+		fw_lateral_ctrl_sp.heading_sp_runway_takeoff = sp.course_setpoint;
 
 		_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
 		/* longitudinal guidance */
@@ -1822,9 +1832,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		longitudinal_control_limits.disable_underspeed_protection = true;
 		_longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits);
 
-		// enable direct yaw control using rudder/wheel
-		_att_sp.fw_control_yaw_wheel = true;
-
 		// XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface.
 		if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) {
 			_att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw;
@@ -1841,9 +1848,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 	} else {
 
 		// follow the glide slope
-
-		/* lateral guidance */
-
 		const PathControllerOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
 						pos_sp_curr.loiter_direction_counter_clockwise,
 						ground_speed, _wind_vel);
@@ -1878,9 +1882,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get();
 		longitudinal_control_limits.sink_rate_max = desired_max_sinkrate;
 		_longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits);
-
-		// enable direct yaw control using rudder/wheel
-		_att_sp.fw_control_yaw_wheel = false;
 	}
 
 	lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()};
@@ -2130,10 +2131,6 @@ FixedwingPositionControl::Run()
 
 		// handle estimator reset events. we only adjust setpoins for manual modes
 		if (_control_mode.flag_control_manual_enabled) {
-			if (_control_mode.flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
-				// make TECS accept step in altitude and demanded altitude
-			}
-
 			// adjust navigation waypoints in position control mode
 			if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
 			    && _local_pos.xy_reset_counter != _xy_reset_counter) {
@@ -2385,7 +2382,6 @@ FixedwingPositionControl::Run()
 			_spoilers_setpoint_pub.publish(spoilers_setpoint);
 		}
 
-		_z_reset_counter = _local_pos.z_reset_counter;
 		_xy_reset_counter = _local_pos.xy_reset_counter;
 
 		perf_end(_loop_perf);

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -390,7 +390,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	// ESTIMATOR RESET COUNTERS
 	uint8_t _xy_reset_counter{0};
-	uint8_t _z_reset_counter{0};
 	uint64_t _time_last_xy_reset{0};
 
 	// LATERAL-DIRECTIONAL GUIDANCE
@@ -845,6 +844,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 					     const matrix::Vector2f &wind_vel);
 
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
 		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
 
 		(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,

src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp

@@ -105,23 +105,23 @@ bool RunwayTakeoff::controlYaw()
 	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }
 
-float RunwayTakeoff::getPitch(float external_pitch_setpoint)
+float RunwayTakeoff::getPitch()
 {
 	if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
 		return math::radians(param_rwto_psp_.get());
 	}
 
-	return external_pitch_setpoint;
+	return NAN;
 }
 
-float RunwayTakeoff::getRoll(float external_roll_setpoint)
+float RunwayTakeoff::getRoll()
 {
 	// until we have enough ground clearance, set roll to 0
 	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
 		return 0.0f;
 	}
 
-	return external_roll_setpoint;
+	return NAN;
 }
 
 float RunwayTakeoff::getYaw(float external_yaw_setpoint)
@@ -134,7 +134,7 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint)
 	}
 }
 
-float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const
+float RunwayTakeoff::getThrottle(const float idle_throttle) const
 {
 	float throttle = idle_throttle;
 
@@ -147,19 +147,13 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external
 		break;
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
-		throttle = param_rwto_max_thr_.get();
-
-		break;
-
 	case RunwayTakeoffState::CLIMBOUT:
-		// ramp in throttle setpoint over takeoff rotation transition time
-		throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_,
-				param_rwto_rot_time_.get());
+		throttle = param_rwto_max_thr_.get();
 
 		break;
 
 	case RunwayTakeoffState::FLY:
-		throttle = external_throttle_setpoint;
+		throttle = NAN;
 	}
 
 	return throttle;

src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h

@@ -116,13 +116,13 @@ class __EXPORT RunwayTakeoff : public ModuleParams
 	 * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad]
 	 * @return Pitch angle setpoint (limited while plane is on runway) [rad]
 	 */
-	float getPitch(float external_pitch_setpoint);
+	float getPitch();
 
 	/**
 	 * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad]
 	 * @return Roll angle setpoint [rad]
 	 */
-	float getRoll(float external_roll_setpoint);
+	float getRoll();
 
 	/**
 	 * @brief Returns the appropriate yaw angle setpoint.
@@ -145,7 +145,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams
 	 * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1]
 	 * @return Throttle setpoint, normalized [0,1]
 	 */
-	float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const;
+	float getThrottle(const float idle_throttle) const;
 
 	/**
 	 * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]