Add fw indi lib

F

WIP Unit tests

src/lib/CMakeLists.txt

@@ -62,6 +62,7 @@ add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)
 add_subdirectory(perf EXCLUDE_FROM_ALL)
 add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL)
+add_subdirectory(fw_indi_control EXCLUDE_FROM_ALL)
 add_subdirectory(pid EXCLUDE_FROM_ALL)
 add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL)

src/lib/fw_indi_control/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2019 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_library(FixedWingIndiControl
+	fw_indi_control.cpp
+	fw_indi_control.hpp
+)
+target_compile_options(FixedWingIndiControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(FixedWingIndiControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(FixedWingIndiControl PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC fw_indi_control_test.cpp LINKLIBS FixedWingIndiControl)

src/lib/fw_indi_control/fw_indi_control.cpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-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 rate_control.cpp
+ */
+
+#include "fw_indi_control.hpp"
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+Quatf
+FixedWingIndiControl::get_flat_attitude(Vector3f vel, Vector3f f)
+{
+
+	const Vector3f vel_air = vel - wind_estimate_;
+
+	// compute force component projected onto lift axis
+	const Vector3f vel_normalized = vel_air.normalized();
+
+	Vector3f f_drag = (f * vel_normalized) * vel_normalized;
+	Vector3f f_lift = f - f_drag;
+
+	const Vector3f lift_normalized = f_lift.normalized();
+	const 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);
+
+	Quatf q(Rotation);
+	return q;
+}

src/lib/fw_indi_control/fw_indi_control.hpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-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 rate_control.hpp
+ *
+ * PID 3 axis angular rate / angular velocity control.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+#include <mathlib/mathlib.h>
+#include <uORB/topics/rate_ctrl_status.h>
+
+class FixedWingIndiControl
+{
+public:
+	FixedWingIndiControl() = default;
+	~FixedWingIndiControl() = default;
+
+	void set_coefficients(float area, float aoa_offset, float C_L0, float C_L1) {
+		_area = area;
+		_aoa_offset = aoa_offset;
+		_C_L0 = C_L0;
+		_C_L1 = C_L1;
+	}
+	matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f);
+
+private:
+
+	// Airdata
+	matrix::Vector3f wind_estimate_;
+	float _cal_airspeed;
+	float _stall_speed;
+	float _true_airspeed;
+	float _rho{1.0f};
+
+	// Model parameters
+	float _area{0.4f};
+	float _C_L0{0.3f};
+	float _C_L1{2.354f};
+	float _aoa_offset{0.07f};
+};

src/lib/fw_indi_control/fw_indi_control_test.cpp

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 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 <gtest/gtest.h>
+#include <lib/fw_indi_control/fw_indi_control.hpp>
+
+using namespace matrix;
+
+TEST(FixedWingIndiControlTest, AllZeroCase)
+{
+	FixedWingIndiControl indi_control;
+	Vector3f forward_vel{10.0, 0.0, 0.0};
+	Vector3f f_command{0.0, 0.0, 10.0};
+	indi_control.set_coefficients(1.0, 0.0, 0.0, 1.0);
+	Quatf attitude = indi_control.get_flat_attitude(forward_vel, f_command);
+	EXPECT_EQ(attitude, Quatf());
+}

src/modules/fw_indi_pos_control/CMakeLists.txt

@@ -39,6 +39,6 @@ px4_add_module(
 		FixedwingIndiPosControl.hpp
 	DEPENDS
 		px4_work_queue
-		RateControl
+		FixedWingIndiControl
 		SlewRate
 	)

src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp

@@ -162,7 +162,7 @@ private:
 	matrix::Quatf vehicle_attitude_;
 
 	float _cal_airspeed{0.0f};
-	float _rho{1.0};
+	float _rho{1.225f};
 	float _true_airspeed{1.0};
 
 
@@ -205,8 +205,6 @@ private:
 		(ParamFloat<px4::params::FW_INDI_K_PITCH>) _param_rot_k_pitch
 	)
 
-	RateControl _rate_control; ///< class for rate control calculations
-
 	/**
 	 * Update our local parameter cache.
 	 */