diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 6454c9e40c..e477851929 100644 --- a/src/lib/CMakeLists.txt +++ b/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) diff --git a/src/lib/fw_indi_control/CMakeLists.txt b/src/lib/fw_indi_control/CMakeLists.txt new file mode 100644 index 0000000000..813f1f7d0f --- /dev/null +++ b/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) diff --git a/src/lib/fw_indi_control/fw_indi_control.cpp b/src/lib/fw_indi_control/fw_indi_control.cpp new file mode 100644 index 0000000000..ee9340c999 --- /dev/null +++ b/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 + +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; +} diff --git a/src/lib/fw_indi_control/fw_indi_control.hpp b/src/lib/fw_indi_control/fw_indi_control.hpp new file mode 100644 index 0000000000..1d17336b99 --- /dev/null +++ b/src/lib/fw_indi_control/fw_indi_control.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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 + +#include +#include + +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}; +}; diff --git a/src/lib/fw_indi_control/fw_indi_control_test.cpp b/src/lib/fw_indi_control/fw_indi_control_test.cpp new file mode 100644 index 0000000000..b7b2a51d59 --- /dev/null +++ b/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 +#include + +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()); +} diff --git a/src/modules/fw_indi_pos_control/CMakeLists.txt b/src/modules/fw_indi_pos_control/CMakeLists.txt index f897988a96..1e572e60cd 100644 --- a/src/modules/fw_indi_pos_control/CMakeLists.txt +++ b/src/modules/fw_indi_pos_control/CMakeLists.txt @@ -39,6 +39,6 @@ px4_add_module( FixedwingIndiPosControl.hpp DEPENDS px4_work_queue - RateControl + FixedWingIndiControl SlewRate ) diff --git a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp index 73203a7fd1..85baf2851f 100644 --- a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp +++ b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp @@ -153,8 +153,18 @@ FixedwingIndiPosControl::airspeed_poll() _airspeed_valid = airspeed_valid; } +void +FixedwingIndiPosControl::vehicle_status_poll() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _vehicle_status = vehicle_status; + } +} + Quatf -FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f) +FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f, float rho_corrected) { const Vector3f vel_air = vel - wind_estimate_; @@ -180,15 +190,6 @@ FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f) 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; @@ -201,19 +202,18 @@ FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f) return q; } -Vector3f +Quatf FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref) { Dcmf R_ib(vehicle_attitude_); Dcmf R_bi(R_ib.transpose()); // apply LP filter to acceleration & velocity - Vector3f acc_filtered; // Poll acceleration PX4_INFO("[PX4PosiNDI] - _acc: %f, %f, %f", double(_acc(0)), double(_acc(1)), double(_acc(2))); - acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0)); - acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1)); - acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2)); + acc_filtered_(0) = _lp_filter_accel[0].apply(_acc(0)); + acc_filtered_(1) = _lp_filter_accel[1].apply(_acc(1)); + acc_filtered_(2) = _lp_filter_accel[2].apply(_acc(2)); // ========================================= // apply PD control law on the body position @@ -226,8 +226,7 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3 // ================================== // compute expected aerodynamic force // ================================== - Vector3f _wind_estimate{0.0, 0.0, 0.0}; ///TODO: Plug in wind estimate - Vector3f airvel_body = R_bi * (vehicle_velocity_ - _wind_estimate); + Vector3f airvel_body = R_bi * (vehicle_velocity_ - wind_estimate_); float angle_of_attack = atan2f(airvel_body(2), airvel_body(0)) + _aoa_offset; @@ -258,7 +257,7 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3 // ================================ // get force command in world frame // ================================ - Vector3f f_command = _mass * (acc_command - acc_filtered) + f_current_filtered; + Vector3f f_command = _mass * (acc_command - acc_filtered_) + f_current_filtered; // ============================================================================================================ // apply some filtering to the force command. This introduces some time delay, @@ -300,10 +299,19 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3 // ========================================================================== // get required attitude (assuming we can fly the target velocity), and error // ========================================================================== - Dcmf R_ref(get_flat_attitude(vel_ref, f_command)); - Quatf ref_attitude(R_ref); + Quatf ref_attitude = get_flat_attitude(vel_ref, f_command, rho_corrected); + return ref_attitude; +} + +Vector3f FixedwingIndiPosControl::controlAttitude(Quatf ref_attitude) +{ PX4_INFO("[PX4PosiNDI] - R_ref: %f, %f, %f, %f", double(ref_attitude(0)), double(ref_attitude(1)), double(ref_attitude(2)), double(ref_attitude(3))); + + Dcmf R_ref(ref_attitude); + Dcmf R_ib(vehicle_attitude_); + Dcmf R_bi(R_ib.transpose()); + // get attitude error Dcmf R_ref_true(R_ref.transpose()*R_ib); // get required rotation vector (in body frame) @@ -340,9 +348,9 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3 // ============================================================== // overwrite rudder rot_acc_command with turn coordination values // ============================================================== - Vector3f vel_air = vehicle_velocity_ - _wind_estimate; + Vector3f vel_air = vehicle_velocity_ - wind_estimate_; Vector3f vel_normalized = vel_air.normalized(); - Vector3f acc_normalized = acc_filtered.normalized(); + Vector3f acc_normalized = acc_filtered_.normalized(); PX4_INFO("[PX4PosiNDI] - acc_normalized: %f, %f, %f", double(acc_normalized(0)), double(acc_normalized(1)), double(acc_normalized(2))); @@ -350,7 +358,7 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3 Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized); Vector3f omega_turn_ref; // constuct acc perpendicular to flight path - Vector3f acc_perp = acc_filtered - (acc_filtered * vel_normalized) * vel_normalized; + Vector3f acc_perp = acc_filtered_ - (acc_filtered_ * vel_normalized) * vel_normalized; if (_airspeed_valid && _cal_airspeed > _stall_speed) { omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized(); @@ -397,6 +405,7 @@ void FixedwingIndiPosControl::Run() parameters_update(); } + vehicle_status_poll(); // Poll local position vehicle_attitude_poll(); @@ -466,18 +475,29 @@ void FixedwingIndiPosControl::Run() vel_ref_ = 15.0f * progress_vector; // Compute bodyrate commands - Vector3f rate_command = computeIndi(pos_ref_, vel_ref_, acc_ref_); + Quatf ref_attitude = computeIndi(pos_ref_, vel_ref_, acc_ref_); + // TODO: Place holder + // ref_attitude = Quatf(1.0, 0.0, 0.0, 0.0); + Vector3f rate_command = controlAttitude(ref_attitude); PX4_INFO("[PX4PosiNDI] angular_rate_command: %f, %f, %f", double(rate_command(0)), double(rate_command(1)), double(rate_command(2))); - vehicle_rates_setpoint_s rates_sp; - rates_sp.thrust_body[0] = 0.5; ///TODO: Add some throttle - rates_sp.thrust_body[1] = 0.0; - rates_sp.thrust_body[2] = 0.0; - rates_sp.roll = rate_command(0); - rates_sp.pitch = rate_command(1); - rates_sp.yaw = rate_command(2); - _rate_sp_pub.publish(rates_sp); + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + vehicle_attitude_setpoint_s attitude_sp; + attitude_sp.timestamp = hrt_absolute_time(); + ref_attitude.copyTo(attitude_sp.q_d); + _attitude_sp_pub.publish(attitude_sp); + + vehicle_rates_setpoint_s rates_sp; + rates_sp.thrust_body[0] = 0.7; ///TODO: Add some throttle + rates_sp.thrust_body[1] = 0.0; + rates_sp.thrust_body[2] = 0.0; + rates_sp.roll = rate_command(0); + rates_sp.pitch = rate_command(1); + rates_sp.yaw = rate_command(2); + rates_sp.timestamp = hrt_absolute_time(); + _rate_sp_pub.publish(rates_sp); + } // ================================= // publish offboard control commands diff --git a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp index 26b2bfa441..ce1bae9f52 100644 --- a/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp +++ b/src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -101,11 +102,23 @@ public: private: void Run() override; + /** + * Update our local parameter cache. + */ + int parameters_update(); - matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f); + void airspeed_poll(); + void vehicle_acceleration_poll(); + void vehicle_local_position_poll(); + void vehicle_attitude_poll(); + void vehicle_status_poll(); - matrix::Vector3f computeIndi(matrix::Vector3f pos_ref, matrix::Vector3f vel_ref, matrix::Vector3f acc_ref); + matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f, float rho_corrected); + + matrix::Quatf computeIndi(matrix::Vector3f pos_ref, matrix::Vector3f vel_ref, matrix::Vector3f acc_ref); + + matrix::Vector3f controlAttitude(matrix::Quatf ref_attitude); uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; @@ -126,6 +139,7 @@ private: uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; manual_control_setpoint_s _manual_control_setpoint{}; @@ -158,11 +172,11 @@ private: matrix::Vector3f vehicle_position_; matrix::Vector3f vehicle_velocity_; matrix::Vector3f _acc; - matrix::Vector3f wind_estimate_; + matrix::Vector3f wind_estimate_{0.0, 0.0, 0.0}; matrix::Quatf vehicle_attitude_; float _cal_airspeed{0.0f}; - float _rho{1.0}; + float _rho{1.225f}; float _true_airspeed{1.0}; @@ -172,6 +186,7 @@ private: matrix::Vector3f _f_command {}; matrix::Vector3f _m_command {}; + matrix::Vector3f acc_filtered_{}; matrix::Matrix3f _K_x; matrix::Matrix3f _K_v; @@ -187,7 +202,7 @@ private: float _C_D2{1.984}; float _area{0.4}; float _stall_speed{1.0}; - float _mass{1.0}; + float _mass{1.5}; bool _landed{true}; bool _switch_saturation{true}; @@ -204,16 +219,4 @@ private: (ParamFloat) _param_rot_k_roll, (ParamFloat) _param_rot_k_pitch ) - - RateControl _rate_control; ///< class for rate control calculations - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - void airspeed_poll(); - void vehicle_acceleration_poll(); - void vehicle_local_position_poll(); - void vehicle_attitude_poll(); }; diff --git a/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c b/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c index cbefb178ad..91e8a10425 100644 --- a/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c +++ b/src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c @@ -139,7 +139,7 @@ PARAM_DEFINE_FLOAT(FW_INDI_KV_Z, 0.1f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 30.0f); +PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 7.0f); /** * control gain of attitude PD-controller (body roll-direction) @@ -151,4 +151,4 @@ PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 30.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FW_INDI_K_PITCH, 10.0f); +PARAM_DEFINE_FLOAT(FW_INDI_K_PITCH, 2.0f);