From 0b16d672a1197194558c2da06376579bc4243303 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Wed, 6 Apr 2022 16:23:31 +0200 Subject: [PATCH] created controller files --- .../FixedwingPositionINDIControl.cpp | 47 ++++++ .../FixedwingPositionINDIControl.hpp | 156 ++++++++++++++++++ .../fw_dyn_soar_control_params.c | 74 +++++++++ 3 files changed, 277 insertions(+) create mode 100644 src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp create mode 100644 src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp create mode 100644 src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp new file mode 100644 index 0000000000..480945fe47 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 "FixedwingPositionINDIControl.hpp" + +using math::constrain; +using math::max; +using math::min; +using math::radians; + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector2d; +using matrix::Vector3f; +using matrix::wrap_pi; \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp new file mode 100644 index 0000000000..5bf30baf0f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -0,0 +1,156 @@ +/** + * Implementation of a generic incremental position controller + * using incremental nonlinear dynamic inversion and differential flatness + * for a fixed wing aircraft during dynamic soaring cycles. + * The controller directly outputs actuator deflections for ailerons, elevator and rudder. + * + * @author Marvin Harms + */ + +// use inclusion guards +#ifndef FIXEDWINGPOSITIONINDICONTROL_HPP_ +#define FIXEDWINGPOSITIONINDICONTROL_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace time_literals; + + +class FixedwingPositionINDIControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FixedwingPositionINDIControl(); + ~FixedwingPositionINDIControl() 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}; + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + // Subscriptions + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed + uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack + uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; // global position + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position + uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)}; // vehicle velocity + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // vehicle acceleration + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; // vehicle body rates + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel + + + // Publishers + uORB::Publication _attitude_sp_pub; + uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication + + // 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 + + + // Update our local parameter cache. + int parameters_update(); + + // Update subscriptions + void airspeed_poll(); + void control_update(); + void manual_control_setpoint_poll(); + void vehicle_attitude_poll(); + void vehicle_command_poll(); + void vehicle_control_mode_poll(); + void vehicle_status_poll(); + void wind_poll(); + + void status_publish(); + + DEFINE_PARAMETERS( + + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + + ) + +}; + + + +#endif // FIXEDWINGPOSITIONINDICONTROL_HPP_ \ No newline at end of file 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 new file mode 100644 index 0000000000..a382b4a4fd --- /dev/null +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -0,0 +1,74 @@ +/** + * @file fw_dyn_soar_control_params.c + * + * Parameters defined by the INDI position controller + * + * @author Marvin Harms + */ + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * total takeoff mass + * This is the mass of the aircraft, used for the INDI + * @unit kg + */ +PARAM_DEFINE_FLOAT(FW_MASS, 1.4f); + +/** + * total wing area used for lift and drag computation + * @unit m2 + */ +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 + */ +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 () + */ +PARAM_DEFINE_FLOAT(C_l0, 0.356f); +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 () + */ +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 + */ +PARAM_DEFINE_FLOAT(RHO, 1.223f); + +/** + * coefficients of the butterworth filter used for smoothing the IMU + * @unit () + */ +PARAM_DEFINE_FLOAT(FILTER_A1, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_A2, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_B1, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_B2, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_B3, 0.0f); + +