From 784e7728a6d3e1ae9cbae6c13943777a47bfc763 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Sun, 7 Aug 2022 09:30:41 +0200 Subject: [PATCH] added shear estimator module, not working yet --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/sitl/default.cmake | 1 + boards/px4/sitl/test.cmake | 1 + msg/CMakeLists.txt | 1 + msg/soaring_controller_wind.msg | 1 + msg/soaring_estimator_shear.msg | 13 ++ .../fw_dyn_soar_estimator/CMakeLists.txt | 44 +++++ .../FixedwingShearEstimator.cpp | 183 ++++++++++++++++++ .../FixedwingShearEstimator.hpp | 127 ++++++++++++ .../fw_dyn_soar_estimator_params.c | 67 +++++++ 11 files changed, 440 insertions(+) create mode 100644 msg/soaring_estimator_shear.msg create mode 100644 src/modules/fw_dyn_soar_estimator/CMakeLists.txt create mode 100644 src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp create mode 100644 src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp create mode 100644 src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 444f536939..77ccd719ec 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,7 @@ ekf2 start & fw_att_control start fw_pos_control_l1 start fw_dyn_soar_control start +fw_dyn_soar_estimator start airspeed_selector start # # Start Land Detector. diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 42b89a5af0..89de917044 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -68,6 +68,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_dyn_soar_control + fw_dyn_soar_estimator fw_pos_control_l1 gyro_calibration gyro_fft diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 8cab1f995c..24f5418aab 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -37,6 +37,7 @@ px4_add_board( fw_att_control fw_pos_control_l1 fw_dyn_soar_control + fw_dyn_soar_estimator gyro_calibration gyro_fft land_detector diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 757f1fd982..579051f3bd 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -37,6 +37,7 @@ px4_add_board( fw_att_control fw_pos_control_l1 fw_dyn_soar_control + fw_dyn_soar_estimator gyro_calibration gyro_fft land_detector diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2696a9d644..6f7f3277d4 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -145,6 +145,7 @@ set(msg_files soaring_controller_position.msg soaring_controller_status.msg soaring_controller_wind.msg + soaring_estimator_shear.msg system_power.msg takeoff_status.msg task_stack_info.msg diff --git a/msg/soaring_controller_wind.msg b/msg/soaring_controller_wind.msg index 63c88755c6..a3efee6224 100644 --- a/msg/soaring_controller_wind.msg +++ b/msg/soaring_controller_wind.msg @@ -4,3 +4,4 @@ uint64 timestamp # time since system start (microseconds) float32[3] wind_estimate # WIND ESTIMATE IN ENU FRAME float32[3] wind_estimate_filtered # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER +float32[3] position # position of the current estimate in the soaring frame diff --git a/msg/soaring_estimator_shear.msg b/msg/soaring_estimator_shear.msg new file mode 100644 index 0000000000..103d35c3f4 --- /dev/null +++ b/msg/soaring_estimator_shear.msg @@ -0,0 +1,13 @@ +# SOARING ESTIMATOR WIND ESTIMATE, USED FOR SELECTING THE CORRECT TRAJECTORY + +uint64 timestamp # time since system start (microseconds) + +float32 vx # maximum wind in x-direction +float32 vy # maximum wind in y-direction +float32 bx # wind offset in x-direction +float32 by # wind offset in y-direction +float32 h # vertical position of shear layer in soaring frame +float32 a # shear strength + +bool params_healthy # plausibility check +uint64 reset_counter # filter reset counter diff --git a/src/modules/fw_dyn_soar_estimator/CMakeLists.txt b/src/modules/fw_dyn_soar_estimator/CMakeLists.txt new file mode 100644 index 0000000000..56a4f38902 --- /dev/null +++ b/src/modules/fw_dyn_soar_estimator/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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_estimator + MAIN fw_dyn_soar_estimator + STACK_MAIN 1024 + SRCS + FixedwingShearEstimator.cpp + FixedwingShearEstimator.hpp + DEPENDS + ) diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp new file mode 100644 index 0000000000..217c3c8b5e --- /dev/null +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "FixedwingShearEstimator.hpp" + +using math::constrain; +using math::max; +using math::min; +using math::radians; +using matrix::Matrix; +using matrix::Vector3f; +using matrix::Vector; +using matrix::wrap_pi; + + + +FixedwingShearEstimator::FixedwingShearEstimator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::test1), + _soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)) + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + // limit to 10 Hz + _soaring_controller_wind_sub.set_interval_ms(100.f); + + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingShearEstimator::~FixedwingShearEstimator() +{ + perf_free(_loop_perf); +} + +bool +FixedwingShearEstimator::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle position callback registration failed!"); + return false; + } + PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR"); + return true; +} + +int +FixedwingShearEstimator::parameters_update() +{ + updateParams(); + + // update params... + _Q_horizontal(0,0) = powf(_param_sigma_q_vel.get(),2); + _Q_horizontal(1,1) = powf(_param_sigma_q_vel.get(),2); + _Q_horizontal(2,2) = powf(_param_sigma_q_vel.get(),2); + _Q_horizontal(3,3) = powf(_param_sigma_q_vel.get(),2); + _Q_horizontal(4,4) = powf(_param_sigma_q_h.get(),2); + _Q_horizontal(5,5) = powf(_param_sigma_q_a.get(),2); + + _R_horizontal(0,0) = powf(_param_sigma_r_vel.get(),2); + _R_horizontal(1,1) = powf(_param_sigma_r_vel.get(),2); + + for (int i=0;i<_dim_vertical;i++){ + _Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2); + } + + _R_vertical = powf(_param_sigma_r_vel.get(),2); + + return PX4_OK; +} + +void +FixedwingShearEstimator::reset_filter() +{ + // reset all states of the filter to some initial guess. + + // reset horizontal wind state + for (int i=0;i<6;i++){ + _X_prior_horizontal(i,i) = 0.0f; + _X_posterior_horizontal(i,i) = 0.0f; + _P_prior_horizontal(i,i) = 1.0f; + _P_posterior_horizontal(i,i) = 1.0f; + } + + // reset vertical wind state + for (int i=0;i<_dim_vertical;i++){ + _X_prior_vertical(i,i) = 0.0f; + _X_posterior_vertical(i,i) = 0.0f; + _P_prior_vertical(i,i) = 1.0f; + _P_posterior_vertical(i,i) = 1.0f; + } +} + +void +FixedwingShearEstimator::perform_prior_update() +{ + +} + +void +FixedwingShearEstimator::perform_posterior_update() +{ + +} + +void +FixedwingPositionINDIControl::Run() +{ + if (should_exit()) { + _soaring_controller_wind_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if wind info changed + if (_soaring_controller_wind_sub.update(&soaring_controller_wind)) + { + // 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(); + } + + // get current measurement + _current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered); + _current_height = Vector3f(soaring_controller_wind.position)(2); + + // prior update + perform_prior_update(); + + // posterior update + perform_posterior_update(_current_height, _current_wind); + + // check if filter diverges + // maybe reset filters... + + // publish shear params + } + + +} + + diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp new file mode 100644 index 0000000000..b3c2e89ca7 --- /dev/null +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp @@ -0,0 +1,127 @@ +/** + * Implementation of a sigmoidal shear EKF for estimating the shear parameters of the wind. + * The estimate is then used by the dynmic soaring controller in "fw_dyn_soar_control". + * + * @author Marvin Harms + */ + +// use inclusion guards +#ifndef FIXEDWINGSHEARESTIMATOR_HPP_ +#define FIXEDWINGSHEARESTIMATOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +using matrix::Dcmf; +using matrix::Quatf; +using matrix::Vector; +using matrix::Matrix; +using matrix::Matrix3f; +using matrix::Vector3f; + + +class FixedwingShearEstimator final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FixedwingShearEstimator(); + ~FixedwingShearEstimator() 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; + + orb_advert_t _mavlink_log_pub{nullptr}; + + // make the main task run, whenever a new body rate becomes available + uORB::SubscriptionCallbackWorkItem _soaring_controller_wind_sub{this, ORB_ID(soaring_controller_wind)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + // Subscriptions + //uORB::Subscription _soaring_controller_wind_sub{ORB_ID(soaring_controller_wind)}; + + // Publishers + uORB::Publication _soaring_estimator_shear_pub{ORB_ID(soaring_estimator_shear)}; + + // Message structs + soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring controller pos + soaring_controller_wind_s _soaring_controller_wind{}; ///< soaring controller wind + + // parameter struct + DEFINE_PARAMETERS( + // aircraft params + (ParamFloat) _param_sigma_q_vel, + (ParamFloat) _param_sigma_q_h, + (ParamFloat) _param_sigma_q_a, + (ParamFloat) _param_sigma_r_vel + ) + + + perf_counter_t _loop_perf; ///< loop performance counter + + // Update our local parameter cache. + int parameters_update(); + void reset_filter(); + void perform_prior_update(); + void perform_posterior_update(float height, Vector3f wind); + bool check_plausibility(); + void publish_estimate(); + + // control variables + const static size_t _dim_vertical = 2; // order of vertical approximation function for vertical wind + + Vector _X_prior_horizontal= {}; + Matrix _P_prior_horizontal = {}; + Vector _X_posterior_horizontal= {}; + Matrix _P_prosterior_horizontal = {}; + Matrix _Q_horizontal = {}; + Matrix _R_horizontal = {}; + Matrix _H_horizontal = {}; + + Vector _X_prior_vertical= {}; + Matrix _P_prior_vertical = {}; + Vector _X_posterior_vertical= {}; + Matrix _P_posterior_vertical = {}; + Vector _X_vertical = {}; // params of vertical wind + Matrix _Q_vertical = {}; + float _R_vertical = {}; + Matrix _H_vertical = {}; + + // measurement variables + Vector3f _current_wind = {}; + float _current_height = {}; + +}; + + +#endif // FIXEDWINGSHEARESTIMATOR_HPP_ \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c new file mode 100644 index 0000000000..39d0fe4353 --- /dev/null +++ b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c @@ -0,0 +1,67 @@ +/** + * @file fw_dyn_soar_estimator_params.c + * + * Parameters defined by the INDI position controller + * + * @author Marvin Harms + */ + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * Standard deviation of velicity state in shear model + * + * This is the std dev of the wind velocity in each direction + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 1.f); + +/** + * Standard deviation of vertical shear position + * + * This is the std dev of the shear vertical position + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f); + +/** + * Standard deviation of velicity state in shear model + * + * This is the std dev of the shear strenght param + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 1.f); + +/** + * Standard deviation of velicity measurement (wind) + * + * This is the std dev of the wind pseudomeasurement passed to the EKF + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f); \ No newline at end of file