added cmakelists and functions

boards/px4/sitl/default.cmake

@@ -36,6 +36,7 @@ px4_add_board(
 		flight_mode_manager
 		fw_att_control
 		fw_pos_control_l1
+		fw_dyn_soar_control
 		gyro_calibration
 		gyro_fft
 		land_detector

src/modules/fw_dyn_soar_control/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015-2017 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.
+#
+############################################################################
+
+#add_subdirectory(launchdetection)
+
+px4_add_module(
+	MODULE modules__fw_dyn_soar_control
+	MAIN fw_dyn_soar_control
+	SRCS
+		FixedwingPositionINDIControl.cpp
+		FixedwingPositionINDIControl.hpp
+	DEPENDS
+	)

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -33,25 +33,29 @@
 
 #include "FixedwingPositionINDIControl.hpp"
 
+using namespace std;
+
 using math::constrain;
 using math::max;
 using math::min;
 using math::radians;
 
 using matrix::Dcmf;
+using matrix::Matrix;
 using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Vector2f;
 using matrix::Vector2d;
 using matrix::Vector3f;
+using matrix::Vector;
 using matrix::wrap_pi;
 
 
 FixedwingPositionINDIControl::FixedwingPositionINDIControl() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
-	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
+	_alpha_sp_pub(ORB_ID(vehicle_angular_acceleration_setpoint)),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 	// limit to 50 Hz
 	_local_pos_sub.set_interval_ms(20);
@@ -71,15 +75,161 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
     _wind_estimate = wind;
 }
 
-Vector
+Vector<float, FixedwingPositionINDIControl::_num_basis_funs>
 FixedwingPositionINDIControl::_get_basis_funs(float t)
 {
-    std::vector<float> vec = {1};
+    Vector<float, _num_basis_funs> vec;
+    vec(0) = 1;
     float sigma = 0.5/_num_basis_funs;
-    for(int i=1; i<_num_basis_funs; i++){
-        float fun1 = sinf(M_PI*t);
-        float fun2 = exp(-pow((t-i/_num_basis_funs),2)/sigma);
-        vec.push_back(fun1*fun2);
+    for(uint i=1; i<_num_basis_funs; i++){
+        float fun1 = sinf(M_PI_F*t);
+        float fun2 = exp(-powf((t-i/_num_basis_funs),2)/sigma);
+        vec(i) = fun1*fun2;
     }
-    return Vector(vec);
-}
+    return vec;
+}
+
+Vector<float, FixedwingPositionINDIControl::_num_basis_funs>
+FixedwingPositionINDIControl::_get_d_dt_basis_funs(float t)
+{
+    Vector<float, _num_basis_funs> vec;
+    vec(0) = 1;
+    float sigma = 0.5/_num_basis_funs;
+    for(uint i=1; i<_num_basis_funs; i++){
+        float fun1 = sinf(M_PI_F*t);
+        float fun2 = exp(-powf((t-i/_num_basis_funs),2)/sigma);
+        vec(i) = fun2*(M_PI_F*sigma*cosf(M_PI_F*t)-2*(t-i/_num_basis_funs)*fun1)/sigma;
+    }
+    return vec;
+}
+
+Vector<float, FixedwingPositionINDIControl::_num_basis_funs>
+FixedwingPositionINDIControl::_get_d2_dt2_basis_funs(float t)
+{
+    Vector<float, _num_basis_funs> vec;
+    vec(0) = 1;
+    float sigma = 0.5/_num_basis_funs;
+    for(uint i=1; i<_num_basis_funs; i++){
+        float fun1 = sinf(M_PI_F*t);
+        float fun2 = exp(-powf((t-i/_num_basis_funs),2)/sigma);
+        vec(i) = fun2 * (fun1 * (4*powf((i/_num_basis_funs-t),2) - \
+                        sigma*(powf(M_PI_F,2)*sigma + 2)) + 4*M_PI_F*sigma*(i/_num_basis_funs-t)*cosf(M_PI_F*t))/(powf(sigma,2));
+ 
+    }
+    return vec;
+}
+
+Vector3f
+FixedwingPositionINDIControl::_get_position_ref(float t)
+{
+    Vector<float, _num_basis_funs> basis = _get_basis_funs(t);
+    float x = _basis_coeffs_x*basis;
+    float y = _basis_coeffs_y*basis;
+    float z = _basis_coeffs_z*basis;
+    return Vector3f{x, y, z};
+}
+
+Vector3f
+FixedwingPositionINDIControl::_get_velocity_ref(float t, float T)
+{
+    Vector<float, _num_basis_funs> basis = _get_d_dt_basis_funs(t);
+    float x = _basis_coeffs_x*basis;
+    float y = _basis_coeffs_y*basis;
+    float z = _basis_coeffs_z*basis;
+    return Vector3f{x, y, z}/T;
+}
+
+Vector3f
+FixedwingPositionINDIControl::_get_acceleration_ref(float t, float T)
+{
+    Vector<float, _num_basis_funs> basis = _get_d2_dt2_basis_funs(t);
+    float x = _basis_coeffs_x*basis;
+    float y = _basis_coeffs_y*basis;
+    float z = _basis_coeffs_z*basis;
+    return Vector3f{x, y, z}/powf(T,2);
+}
+
+Quatf
+FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
+{
+    Vector3f vel = _get_velocity_ref(t,T);
+    Vector3f vel_air = vel - _wind_estimate;
+    Vector3f acc = _get_acceleration_ref(t,T);
+    // add gravity
+    acc(2) += 9.81;
+    // compute required force
+    Vector3f f = FW_MASS*acc;
+    // compute force component projected onto lift axis
+    Vector3f vel_normalized = normalized(vel_air);
+    Vector3f f_lift = f - f*vel_normalized;
+    Vector3f f_lift_normalized = normalized(f_lift);
+    Vector3f wing_normalized = -vel_normalized.cross(lift_normalized);
+    // compute rotation matrix
+    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);
+    // compute required AoA
+    Vector3f f_phi = R_bi*f_lift;
+    float AoA = (2*f_phi(2))/(1.223*0.4*powf(unit(vel_air),2)) - 0.356)/2.354;
+    // compute final rotation matrix
+    Euler e;
+    Euler(0, AoA, 0);
+    Dcmf R_pitch;
+    R_pitch(e);
+    Dcmf Rotation;
+    Rotation(R_pitch*R_bi);
+    // switch from FRD to ENU frame
+    Rotation(1,0) *= -1;
+    Rotation(1,1) *= -1;
+    Rotation(1,2) *= -1;
+    Rotation(2,0) *= -1;
+    Rotation(2,1) *= -1;
+    Rotation(2,2) *= -1;
+
+
+
+
+
+
+
+    return 1;
+}
+
+
+int FixedwingPositionINDIControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FixedwingPositionINDIControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+fw_dyn_soar_control is the fixed wing controller for soaring tasks.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_dyn_soar_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_dyn_soar_control_main(int argc, char *argv[])
+{
+	return FixedwingPositionINDIControl::main(argc, argv);
+}

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -13,6 +13,8 @@
 
 #include <float.h>
 
+#include <vector>
+#include <array>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
@@ -41,6 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_odometry.h>
@@ -55,6 +58,14 @@
 
 using namespace time_literals;
 
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector;
+using matrix::Vector2f;
+using matrix::Vector2d;
+using matrix::Vector3f;
+
 
 class FixedwingPositionINDIControl final : public ModuleBase<FixedwingPositionINDIControl>, public ModuleParams,
 	public px4::WorkItem
@@ -97,30 +108,17 @@ private:
 	
 
     // Publishers
-	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
-	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
-
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s>		_alpha_sp_pub;
+	
     // Message structs
 	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
-	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items
-	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint
 	vehicle_control_mode_s		_control_mode {};		///< control mode
 	vehicle_local_position_s	_local_pos {};			///< vehicle local position
 	vehicle_status_s		    _vehicle_status {};		///< vehicle status
 
-	double _current_latitude{0};
-	double _current_longitude{0};
-	float _current_altitude{0.f};
 
 	perf_counter_t	_loop_perf;				///< loop performance counter
 
-	float _pitch{0.0f};
-	float _yaw{0.0f};
-	float _yawrate{0.0f};
-
-	matrix::Vector3f _body_acceleration{};
-	matrix::Vector3f _body_velocity{};
-
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
 	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state
@@ -139,16 +137,52 @@ private:
 	void		vehicle_status_poll();
 	void        wind_poll();
 
+	//
 	void		status_publish();
 
-	DEFINE_PARAMETERS(
+	const int _num_points = 30;				// number of points on the precomputed trajectory
+	const static size_t _num_basis_funs = 15;			// number of basis functions used for the trajectory approximation
 
-		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
-		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
-		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
+	// controller methods
+	void _set_wind_estimate(Vector3f wind);
+	Vector<float, _num_basis_funs> _get_basis_funs(float t=0);			// compute the vector of basis functions at normalized time t in [0,1]
+	Vector<float, _num_basis_funs> _get_d_dt_basis_funs(float t=0);	// compute the vector of basis function gradients at normalized time t in [0,1]
+	Vector<float, _num_basis_funs> _get_d2_dt2_basis_funs(float t=0);	// compute the vector of basis function curvatures at normalized time t in [0,1]
+	void _load_basis_coefficients();		// load the coefficients of the current path approximation
+	Vector3f _get_position_ref(float t=0);	// get the reference position on the current path, at normalized time t in [0,1]
+	Vector3f _get_velocity_ref(float t=0, float T=1);	// get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_acceleration_ref(float t=0, float T=1);	// get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Quatf _get_attitude_ref(float t=0, float T=1);	// get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_angular_velocity_ref(float t=0, float T=1);	// get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_angular_acceleration_ref(float t=0, float T=1);	// get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	float _get_closest_t(Vector3f pos);				// get the normalized time, at which the reference path is closest to the current position
+	Quatf _get_attitude(Vector3f vel, Vector3f f);	// get the attitude to produce force f while flying with velocity vel
+	void _compute_NDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha);
+	void _compute_INDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha);
 
-	)
+	// control variables
+	Vector<float, _num_basis_funs> _basis_coeffs_x;				// coefficients of the current path
+	Vector<float, _num_basis_funs> _basis_coeffs_y;				// coefficients of the current path
+	Vector<float, _num_basis_funs> _basis_coeffs_z;				// coefficients of the current path
+	Vector3f _pos;
+	Vector3f _pos_sp;
+	Vector3f _vel;
+	Vector3f _vel_sp;
+	Vector3f _acc;
+	Vector3f _acc_sp;
+	Quatf _att;
+	Quatf _att_sp;
+	Vector3f _omega;
+	Vector3f _omega_sp;
+	Vector3f _alpha;
+	Vector3f _alpha_sp;
+	Vector3f _wind_estimate;
 
+	// filter variables
+	std::array<Vector3f, 3> _f_list;
+	std::array<Vector3f, 3> _a_list;
+	std::array<Vector3f, 3> _f_lpf_list;
+	std::array<Vector3f, 3> _a_lpf_list;
 };
 
 

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -12,63 +12,178 @@
 
 /**
  * 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_MASS, 1.4f);
 
 /**
  * total wing area used for lift and drag computation
- * @unit m2
+ * 
+ * @unit m^2
+ * @min 0.1
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
  */
 PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f);
 
-/**
- * total wing area used for lift and drag computation
- * @unit m2
- */
-PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f);
 
 /**
  * air density used for lift and drag computation
- * @unit kg/m3
+ * 
+ * @unit 
+ * @min 0.5
+ * @max 1.225
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
  */
 PARAM_DEFINE_FLOAT(RHO, 1.223f);
 
 /**
  * estimated lift coefficients used for lift and drag computation
+ * 
  * Used as L = C_l0 + C_l1*alpha,
  * where alpha is the angle of attack.
- * @unit ()
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
  */
 PARAM_DEFINE_FLOAT(C_l0, 0.356f);
+
+/**
+ * estimated lift coefficients used for lift and drag computation
+ * 
+ * Used as L = C_l0 + C_l1*alpha,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
 PARAM_DEFINE_FLOAT(C_l1, 2.354f);
 
 
 /**
  * estimated drag coefficients used for lift and drag computation
+ * 
  * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
  * where alpha is the angle of attack.
- * @unit ()
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
  */
 PARAM_DEFINE_FLOAT(C_d0, 0.0288f);
-PARAM_DEFINE_FLOAT(C_d1, 0.3783f);
-PARAM_DEFINE_FLOAT(C_d2, 1.984f);
 
 /**
- * air density used for lift and drag computation
- * @unit kg/m3
+ * estimated drag coefficients used for lift and drag computation
+ * 
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(RHO, 1.223f);
+PARAM_DEFINE_FLOAT(C_d1, 0.3783f);
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ * 
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(C_d2, 1.984f);
+
 
 /**
  * coefficients of the butterworth filter used for smoothing the IMU
- * @unit ()
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
  */
 PARAM_DEFINE_FLOAT(FILTER_A1, 0.0f);
+
+/**
+ * coefficients of the butterworth filter used for smoothing the IMU
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
 PARAM_DEFINE_FLOAT(FILTER_A2, 0.0f);
+
+/**
+ * coefficients of the butterworth filter used for smoothing the IMU
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
 PARAM_DEFINE_FLOAT(FILTER_B1, 0.0f);
+
+/**
+ * coefficients of the butterworth filter used for smoothing the IMU
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
 PARAM_DEFINE_FLOAT(FILTER_B2, 0.0f);
+
+/**
+ * coefficients of the butterworth filter used for smoothing the IMU
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
 PARAM_DEFINE_FLOAT(FILTER_B3, 0.0f);