From 5d0763b2f282b3d75059bb462306cbffe441dc93 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Thu, 7 Apr 2022 14:59:42 +0200 Subject: [PATCH] added cmakelists and functions --- boards/px4/sitl/default.cmake | 1 + .../fw_dyn_soar_control/CMakeLists.txt | 43 +++++ .../FixedwingPositionINDIControl.cpp | 170 ++++++++++++++++-- .../FixedwingPositionINDIControl.hpp | 74 +++++--- .../fw_dyn_soar_control_params.c | 145 +++++++++++++-- 5 files changed, 388 insertions(+), 45 deletions(-) create mode 100644 src/modules/fw_dyn_soar_control/CMakeLists.txt diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index d2221a0cdf..415c39a62d 100644 --- a/boards/px4/sitl/default.cmake +++ b/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 diff --git a/src/modules/fw_dyn_soar_control/CMakeLists.txt b/src/modules/fw_dyn_soar_control/CMakeLists.txt new file mode 100644 index 0000000000..24a25b3c5d --- /dev/null +++ b/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 + ) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 1abcb7fce3..3a3141980c 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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 FixedwingPositionINDIControl::_get_basis_funs(float t) { - std::vector vec = {1}; + Vector 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); -} \ No newline at end of file + return vec; +} + +Vector +FixedwingPositionINDIControl::_get_d_dt_basis_funs(float t) +{ + Vector 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 +FixedwingPositionINDIControl::_get_d2_dt2_basis_funs(float t) +{ + Vector 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 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 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 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); +} diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 5bf30baf0f..ca2d6d89cc 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -13,6 +13,8 @@ #include +#include +#include #include #include #include @@ -41,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -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, public ModuleParams, public px4::WorkItem @@ -97,30 +108,17 @@ private: // Publishers - uORB::Publication _attitude_sp_pub; - uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication - + uORB::Publication _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) _param_fw_airspd_max, - (ParamFloat) _param_fw_airspd_min, - (ParamFloat) _param_fw_airspd_trim, + // controller methods + void _set_wind_estimate(Vector3f wind); + Vector _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1] + Vector _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1] + Vector _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 _basis_coeffs_x; // coefficients of the current path + Vector _basis_coeffs_y; // coefficients of the current path + Vector _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 _f_list; + std::array _a_list; + std::array _f_lpf_list; + std::array _a_lpf_list; }; diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index a382b4a4fd..96d525659b 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/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);