Add iNDI control module

This commit adds an implementation of a indi position controller for fixed-wings.

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -14,6 +14,7 @@ control_allocator start
 # Start attitude controller.
 #
 fw_rate_control start
+fw_indi_pos_control start
 fw_att_control start
 fw_mode_manager start
 fw_lat_lon_control start

boards/px4/sitl/default.px4board

@@ -23,6 +23,7 @@ CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_FW_INDI_POS_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

src/modules/fw_indi_pos_control/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__fw_indi_pos_control
+	MAIN fw_indi_pos_control
+	SRCS
+		FixedwingIndiPosControl.cpp
+		FixedwingIndiPosControl.hpp
+	DEPENDS
+		px4_work_queue
+		RateControl
+		SlewRate
+	)

src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp

@@ -0,0 +1,523 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2023 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.
+ *
+ ****************************************************************************/
+
+#include "FixedwingIndiPosControl.hpp"
+
+using namespace time_literals;
+using namespace matrix;
+
+using math::constrain;
+using math::interpolate;
+using math::radians;
+
+FixedwingIndiPosControl::FixedwingIndiPosControl(bool vtol) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingIndiPosControl::~FixedwingIndiPosControl()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+FixedwingIndiPosControl::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+int
+FixedwingIndiPosControl::parameters_update()
+{
+	_mass = _param_fw_mass.get();
+	_K_x *= 0.f;
+	_K_x(0,0) = _param_kp_x.get();
+	_K_x(1,1) = _param_kp_y.get();
+	_K_x(2,2) = _param_kp_z.get();
+	_K_q(0,0) = _param_rot_k_roll.get();
+	_K_q(1,1) = _param_rot_k_pitch.get();
+	_K_q(2,2) = 0.5;
+	return PX4_OK;
+}
+
+void
+FixedwingIndiPosControl::vehicle_acceleration_poll()
+{
+	vehicle_acceleration_s acceleration;
+
+	if (_vehicle_acceleration_sub.update(&acceleration)) {
+		Dcmf R_ib(vehicle_attitude_);
+		_acc = R_ib * Vector3f(acceleration.xyz) - Vector3f{0.f, 0.f, 9.81f};
+	}
+
+	if (hrt_absolute_time() - acceleration.timestamp > 20_ms
+	    && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
+		PX4_ERR("linear acceleration sample is too old");
+	}
+}
+
+void
+FixedwingIndiPosControl::vehicle_attitude_poll()
+{
+	vehicle_attitude_s vehicle_attitude;
+
+	if (_vehicle_attitude_sub.update(&vehicle_attitude)) {
+		vehicle_attitude_ = Quatf(vehicle_attitude.q);
+	}
+
+}
+
+
+void
+FixedwingIndiPosControl::vehicle_local_position_poll()
+{
+	vehicle_local_position_s pos;
+
+	if (_vehicle_local_position_sub.update(&pos)) {
+		vehicle_position_ = Vector3f{pos.x, pos.y, pos.z};
+		vehicle_velocity_ = Vector3f{pos.vx, pos.vy, pos.vz};
+		// take accel from faster message, since 50Hz is too slow...
+	}
+}
+
+void
+FixedwingIndiPosControl::airspeed_poll()
+{
+	bool airspeed_valid = _airspeed_valid;
+	airspeed_validated_s airspeed_validated;
+
+	if (_airspeed_validated_sub.update(&airspeed_validated)) {
+
+		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
+		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
+		    && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {
+
+			airspeed_valid = true;
+
+			_airspeed_last_valid = airspeed_validated.timestamp;
+			_true_airspeed = airspeed_validated.true_airspeed_m_s;
+			_cal_airspeed = airspeed_validated.calibrated_airspeed_m_s;
+		}
+
+	} else {
+		// no airspeed updates for one second
+		if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
+			airspeed_valid = false;
+		}
+	}
+
+	_airspeed_valid = airspeed_valid;
+}
+
+Quatf
+FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f)
+{
+
+	Vector3f vel_air = vel - wind_estimate_;
+	// compute force component projected onto lift axis
+	Vector3f vel_normalized = vel_air.normalized();
+	Vector3f f_lift = f - (f * vel_normalized) * vel_normalized;
+	Vector3f lift_normalized = f_lift.normalized();
+	Vector3f wing_normalized = -vel_normalized.cross(lift_normalized);
+	// compute rotation matrix between ENU and FRD frame
+	Dcmf R_bi;
+	R_bi(0, 0) = vel_normalized(0);
+	R_bi(0, 1) = vel_normalized(1);
+	R_bi(0, 2) = vel_normalized(2);
+	R_bi(1, 0) = wing_normalized(0);
+	R_bi(1, 1) = wing_normalized(1);
+	R_bi(1, 2) = wing_normalized(2);
+	R_bi(2, 0) = lift_normalized(0);
+	R_bi(2, 1) = lift_normalized(1);
+	R_bi(2, 2) = lift_normalized(2);
+	R_bi.renormalize();
+
+	float rho_corrected;
+
+	if (_cal_airspeed >= _stall_speed) {
+		rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2);
+
+	} else {
+		rho_corrected = _rho;
+	}
+
+	// compute required AoA
+	Vector3f f_phi = R_bi * f_lift;
+	float AoA = ((2.f * f_phi(2)) / (rho_corrected * _area * (vel_air * vel_air) + 0.001f) - _C_L0) / _C_L1 - _aoa_offset;
+	// compute final rotation matrix
+	Eulerf e(0.f, AoA, 0.f);
+	Dcmf R_pitch(e);
+	Dcmf Rotation(R_pitch * R_bi);
+	// switch from FRD to ENU frame
+	Rotation(1, 0) *= -1.f;
+	Rotation(1, 1) *= -1.f;
+	Rotation(1, 2) *= -1.f;
+	Rotation(2, 0) *= -1.f;
+	Rotation(2, 1) *= -1.f;
+	Rotation(2, 2) *= -1.f;
+
+	Quatf q(Rotation.transpose());
+	return q;
+}
+
+Vector3f
+FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref)
+{
+	Dcmf R_ib(vehicle_attitude_);
+	Dcmf R_bi(R_ib.transpose());
+
+	// apply LP filter to acceleration & velocity
+	Vector3f acc_filtered;
+	// Poll acceleration
+	PX4_INFO("[PX4PosiNDI]   - _acc: %f, %f, %f", double(_acc(0)), double(_acc(1)), double(_acc(2)));
+	acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0));
+	acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1));
+	acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2));
+
+	// =========================================
+	// apply PD control law on the body position
+	// =========================================
+	Vector3f acc_command_local = (_K_x * R_bi * (pos_ref - vehicle_position_) + _K_v * R_bi *
+				      (vel_ref - vehicle_velocity_) + _K_a * R_bi *
+				      (acc_ref - _acc));
+	Vector3f acc_command = R_ib * acc_command_local + acc_ref; ///TODO: Why add acc_ref again?
+
+	// ==================================
+	// compute expected aerodynamic force
+	// ==================================
+	Vector3f _wind_estimate{0.0, 0.0, 0.0}; ///TODO: Plug in wind estimate
+	Vector3f airvel_body = R_bi * (vehicle_velocity_ - _wind_estimate);
+
+	float angle_of_attack = atan2f(airvel_body(2), airvel_body(0)) + _aoa_offset;
+
+	float C_l = _C_L0 + _C_L1 * angle_of_attack;
+	float C_d = _C_D0 + _C_D1 * angle_of_attack + _C_D2 * powf(angle_of_attack, 2);
+	// compute actual air density
+	float rho_corrected;
+
+	if (_cal_airspeed >= _stall_speed) {
+		rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2);
+
+	} else {
+		rho_corrected = _rho;
+	}
+
+	float factor = -0.5f * rho_corrected * _area * sqrtf(airvel_body * airvel_body);
+
+	// Wind axis
+	Vector3f w_x = airvel_body;
+	Vector3f w_z = w_x.cross(Vector3f{0.f, 1.f, 0.f});
+	Vector3f f_current = R_ib * (factor * (C_l * w_z + C_d * w_x));
+	// apply LP filter to force
+	Vector3f f_current_filtered;
+	f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0));
+	f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1));
+	f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2));
+
+	// ================================
+	// get force command in world frame
+	// ================================
+	Vector3f f_command = _mass * (acc_command - acc_filtered) + f_current_filtered;
+
+	// ============================================================================================================
+	// apply some filtering to the force command. This introduces some time delay,
+	// which is not desired for stability reasons, but it rejects some of the noise fed to the low-level controller
+	// ============================================================================================================
+	f_command(0) = _lp_filter_ctrl0[0].apply(f_command(0));
+	f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1));
+	f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2));
+	_f_command = f_command;
+	// limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa)
+
+	// ====================================================================
+	// saturate force command to avoid overly agressive maneuvers and stall
+	// ====================================================================
+	if (_switch_saturation) {
+		float speed = airvel_body * airvel_body;
+		// compute maximum achievable force
+		float f_max;
+
+		if (speed > _stall_speed) {
+			f_max = -factor * sqrtf(airvel_body * airvel_body) * (_C_L0 + _C_L1 * 0.25f); // assume stall at 15° AoA
+
+		} else {
+			f_max = -factor * _stall_speed * (_C_L0 + _C_L1 * 0.25f); // assume stall at 15° AoA
+		}
+
+		// compute current command
+		float f_now = sqrtf(f_command * f_command);
+
+		// saturate current command
+		if (f_now > f_max) {
+			f_command = f_max / f_now * f_command;
+		}
+	}
+
+	PX4_INFO("[PX4PosiNDI]   - f_command: %f, %f, %f", double(f_command(0)), double(f_command(1)), double(f_command(2)));
+	PX4_INFO("[PX4PosiNDI]   - vel_ref: %f, %f, %f", double(vel_ref(0)), double(vel_ref(1)), double(vel_ref(2)));
+
+	// ==========================================================================
+	// get required attitude (assuming we can fly the target velocity), and error
+	// ==========================================================================
+	Dcmf R_ref(get_flat_attitude(vel_ref, f_command));
+	Quatf ref_attitude(R_ref);
+	PX4_INFO("[PX4PosiNDI]   - R_ref: %f, %f, %f, %f", double(ref_attitude(0)), double(ref_attitude(1)),
+		 double(ref_attitude(2)), double(ref_attitude(3)));
+	// get attitude error
+	Dcmf R_ref_true(R_ref.transpose()*R_ib);
+	// get required rotation vector (in body frame)
+	AxisAnglef q_err(R_ref_true);
+	Vector3f w_err;
+
+	// project rotation angle to [-pi,pi]
+	if (q_err.angle()*q_err.angle() < M_PI_F * M_PI_F) {
+		w_err = -q_err.angle() * q_err.axis();
+
+	} else {
+		if (q_err.angle() > 0.f) {
+			w_err = (2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis();
+
+		} else {
+			w_err = (-2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis();
+		}
+	}
+
+	// TODO: Reference rate to rate controller
+	// =========================================
+	// apply PD control law on the body attitude
+	// =========================================
+	// Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref;
+	Vector3f rot_vel_command = _K_q * w_err;
+	PX4_INFO("[PX4PosiNDI]   - w_err: %f, %f, %f", double(w_err(0)), double(w_err(1)), double(w_err(2)));
+
+	if (sqrtf(w_err * w_err) > M_PI_F) {
+		PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err * w_err), (double)q_err.angle(),
+			(double)(q_err.axis()*q_err.axis()));
+	}
+
+
+	// ==============================================================
+	// overwrite rudder rot_acc_command with turn coordination values
+	// ==============================================================
+	Vector3f vel_air = vehicle_velocity_ - _wind_estimate;
+	Vector3f vel_normalized = vel_air.normalized();
+	Vector3f acc_normalized = acc_filtered.normalized();
+	PX4_INFO("[PX4PosiNDI]   - acc_normalized: %f, %f, %f", double(acc_normalized(0)), double(acc_normalized(1)),
+		 double(acc_normalized(2)));
+
+	// compute ideal angular velocity
+	Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized);
+	Vector3f omega_turn_ref;
+	// constuct acc perpendicular to flight path
+	Vector3f acc_perp = acc_filtered - (acc_filtered * vel_normalized) * vel_normalized;
+
+	if (_airspeed_valid && _cal_airspeed > _stall_speed) {
+		omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized();
+		//PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2)));
+
+	} else {
+		omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_stall_speed) * R_bi * omega_turn_ref_normalized.normalized();
+		//PX4_ERR("No valid airspeed message detected or airspeed too low");
+	}
+
+	// apply some smoothing since we don't want HF components in our rudder output
+	omega_turn_ref(2) = _lp_filter_rud.apply(omega_turn_ref(2));
+	PX4_INFO("[PX4PosiNDI]   - omega_turn_ref: %f, %f, %f", double(omega_turn_ref(0)), double(omega_turn_ref(1)),
+		 double(omega_turn_ref(2)));
+	rot_vel_command(2) = omega_turn_ref(2);
+
+	return rot_vel_command;
+}
+
+void FixedwingIndiPosControl::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// only run controller if angular velocity changed
+	if (_vehicle_angular_velocity_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) { //TODO rate!
+
+		// only update parameters if they changed
+		bool params_updated = _parameter_update_sub.updated();
+
+		// check for parameter updates
+		if (params_updated) {
+			// clear update
+			parameter_update_s pupdate;
+			_parameter_update_sub.copy(&pupdate);
+
+			// update parameters from storage
+			updateParams();
+			parameters_update();
+		}
+
+		// Poll local position
+		vehicle_attitude_poll();
+
+		vehicle_local_position_poll();
+		vehicle_acceleration_poll();
+
+		// Poll airspeed
+		airspeed_poll();
+
+		// Poll trajectory setpoints
+		trajectory_setpoint_s trajectory_setpoint;
+
+		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+			///TODO: Check if references are invalid
+			pos_ref_ = Vector3f(trajectory_setpoint.position);
+			vel_ref_ = Vector3f(trajectory_setpoint.velocity);
+			acc_ref_ = Vector3f(trajectory_setpoint.acceleration);
+
+			///TODO: Publish offboard mode
+
+			///TODO: Publish bodyrate commands in offboard mode
+			// if (_control_mode.flag_control_offboard_enabled) {
+			// }
+
+		}
+
+		///TODO: Need to handle invalid velocity (zero)
+		pos_ref_(2) = -30.0;
+
+		vel_ref_(0) = 15.0;
+		vel_ref_(1) = 0.0;
+		vel_ref_(2) = 0.0;
+
+		// Compute bodyrate commands
+		Vector3f rate_command = computeIndi(pos_ref_, vel_ref_, acc_ref_);
+		PX4_INFO("[PX4PosiNDI] angular_rate_command: %f, %f, %f", double(rate_command(0)), double(rate_command(1)),
+			 double(rate_command(2)));
+
+		vehicle_rates_setpoint_s rates_sp;
+		rates_sp.thrust_body[0] = 0.5; ///TODO: Add some throttle
+		rates_sp.thrust_body[1] = 0.0;
+		rates_sp.thrust_body[2] = 0.0;
+		rates_sp.roll = rate_command(0);
+		rates_sp.pitch = rate_command(1);
+		rates_sp.yaw = rate_command(2);
+		_rate_sp_pub.publish(rates_sp);
+
+		// =================================
+		// publish offboard control commands
+		// =================================
+		offboard_control_mode_s ocm{};
+		ocm.body_rate = true;
+		ocm.timestamp = hrt_absolute_time();
+		_offboard_control_mode_pub.publish(ocm);
+
+
+
+	}
+
+	// backup schedule
+	ScheduleDelayed(20_ms);
+
+	perf_end(_loop_perf);
+}
+
+int FixedwingIndiPosControl::task_spawn(int argc, char *argv[])
+{
+	bool vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			vtol = true;
+		}
+	}
+
+	FixedwingIndiPosControl *instance = new FixedwingIndiPosControl(vtol);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int FixedwingIndiPosControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FixedwingIndiPosControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+fw_rate_control is the fixed-wing rate controller.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_rate_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fw_indi_pos_control_main(int argc, char *argv[])
+{
+	return FixedwingIndiPosControl::main(argc, argv);
+}

src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp

@@ -0,0 +1,211 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2023 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 <lib/rate_control/rate_control.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls_status.h>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
+
+using matrix::Eulerf;
+using matrix::Quatf;
+
+using uORB::SubscriptionData;
+
+using namespace time_literals;
+
+class FixedwingIndiPosControl final : public ModuleBase<FixedwingIndiPosControl>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	FixedwingIndiPosControl(bool vtol = false);
+	~FixedwingIndiPosControl() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+
+	matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f);
+
+	matrix::Vector3f computeIndi(matrix::Vector3f pos_ref, matrix::Vector3f vel_ref, matrix::Vector3f acc_ref);
+
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};     	// linear acceleration
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+
+	uORB::SubscriptionMultiArray<control_allocator_status_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};
+
+
+	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Publication<offboard_control_mode_s>	_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
+
+	manual_control_setpoint_s		_manual_control_setpoint{};
+	vehicle_control_mode_s			_vcontrol_mode{};
+	vehicle_rates_setpoint_s		_rates_sp{};
+	vehicle_status_s			_vehicle_status{};
+
+	perf_counter_t _loop_perf;
+
+	hrt_abstime _last_run{0};
+	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
+
+	// controller frequency
+	const float _sample_frequency = 200.f;
+	// Low-Pass filters stage 1
+	const float _cutoff_frequency_1 = 20.f;
+	// smoothing filter to reject HF noise in control output
+	const float _cutoff_frequency_smoothing =
+		20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
+	math::LowPassFilter2p<float> _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// linear acceleration
+	math::LowPassFilter2p<float> _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// force command
+	math::LowPassFilter2p<float> _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// body rates
+	math::LowPassFilter2p<float> _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}};	// force command stage 1
+	math::LowPassFilter2p<float> _lp_filter_rud {_sample_frequency, 10};	// rudder command
+
+	matrix::Vector3f vehicle_position_;
+	matrix::Vector3f vehicle_velocity_;
+	matrix::Vector3f _acc;
+	matrix::Vector3f wind_estimate_;
+	matrix::Quatf vehicle_attitude_;
+	float _cal_airspeed{0.0f};
+	float _rho{1.0};
+	float _true_airspeed{1.0};
+
+
+	matrix::Vector3f pos_ref_;
+	matrix::Vector3f vel_ref_;
+	matrix::Vector3f acc_ref_;
+
+	matrix::Vector3f _f_command {};
+	matrix::Vector3f _m_command {};
+
+	matrix::Matrix3f _K_x;
+	matrix::Matrix3f _K_v;
+	matrix::Matrix3f _K_a;
+	matrix::Matrix3f _K_q;
+
+	// Vehicle parameters (From Easyglider)
+	float _aoa_offset{0.07};
+	float _C_L0{0.3};
+	float _C_D0{0.0288};
+	float _C_L1{2.354};
+	float _C_D1{0.3783};
+	float _C_D2{1.984};
+	float _area{0.4};
+	float _stall_speed{1.0};
+	float _mass{1.0};
+
+	bool _landed{true};
+	bool _switch_saturation{true};
+	bool _airspeed_valid{false};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_INDI_MASS>) _param_fw_mass,
+		(ParamFloat<px4::params::FW_INDI_KP_X>) _param_kp_x,
+		(ParamFloat<px4::params::FW_INDI_KP_Y>) _param_kp_y,
+		(ParamFloat<px4::params::FW_INDI_KP_Z>) _param_kp_z,
+		(ParamFloat<px4::params::FW_INDI_K_ROLL>) _param_rot_k_roll,
+		(ParamFloat<px4::params::FW_INDI_K_PITCH>) _param_rot_k_pitch
+	)
+
+	RateControl _rate_control; ///< class for rate control calculations
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	void		airspeed_poll();
+	void		vehicle_acceleration_poll();
+	void 		vehicle_local_position_poll();
+	void		vehicle_attitude_poll();
+};

src/modules/fw_indi_pos_control/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_FW_INDI_POS_CONTROL
+	bool "fw_indi_pos_control"
+	default n
+	---help---
+		Enable support for fw_att_control
+
+menuconfig USER_FW_INDI_POS_CONTROL
+	bool "fw_indi_pos_control running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_FW_INDI_POS_CONTROL
+	---help---
+		Put fw_indi_pos_control in userspace memory

src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2023 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fw_indi_pos_control_params.c
+ *
+ * Parameters defined by the fixed-wing rate control task
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Thomas Gubler <thomas@px4.io>
+ */
+
+/**
+ * total takeoff mass
+ *
+ * This is the mass of the aircraft, used for the INDI
+ *
+ * @unit kg
+ * @min 1.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(FW_INDI_MASS, 1.4f);
+
+// ========================================================
+// =================== CONTROL GAINS ======================
+// ========================================================
+/**
+ * control gain of position PD-controller (body x-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+ PARAM_DEFINE_FLOAT(FW_INDI_KP_X, 1.0f);
+
+ /**
+ * control gain of position PD-controller (body x-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+ PARAM_DEFINE_FLOAT(FW_INDI_KP_Y, 1.0f);
+
+ /**
+ * control gain of position PD-controller (body x-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+ PARAM_DEFINE_FLOAT(FW_INDI_KP_Z, 1.0f);
+
+ /**
+ * control gain of attitude PD-controller (body roll-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 30.0f);
+
+ /**
+ * control gain of attitude PD-controller (body roll-direction)
+ *
+ * @unit
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+ PARAM_DEFINE_FLOAT(FW_INDI_K_PITCH, 30.0f);