fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 08:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

backup push

Browse Source
This commit is contained in:
Marvin Harms
2022-04-25 12:59:33 +02:00
parent ac9ac55b61
commit e2d2aa971d
3 changed files with 90 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+54 -21
View File
@@ -359,22 +359,22 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv()
*/
// 100m radius circle trajec
_basis_coeffs_x(0) = 0.000064f;
_basis_coeffs_x(1) = 3020.233571f;
_basis_coeffs_x(2) = -10609.960177f;
_basis_coeffs_x(3) = 17956.458964f;
_basis_coeffs_x(4) = -15735.479961f;
_basis_coeffs_x(5) = 2399.573434f;
_basis_coeffs_x(6) = 11421.854705f;
_basis_coeffs_x(7) = -12388.936542f;
_basis_coeffs_x(8) = -120.944433f;
_basis_coeffs_x(9) = 12530.869640f;
_basis_coeffs_x(10) = -11346.431128f;
_basis_coeffs_x(11) = -2643.369342f;
_basis_coeffs_x(12) = 15999.009519f;
_basis_coeffs_x(13) = -18127.094775f;
_basis_coeffs_x(14) = 10676.696033f;
_basis_coeffs_x(15) = -3032.667571f;
_basis_coeffs_x(0) = -0.000064f;
_basis_coeffs_x(1) = -3020.233571f;
_basis_coeffs_x(2) = 10609.960177f;
_basis_coeffs_x(3) = -17956.458964f;
_basis_coeffs_x(4) = 15735.479961f;
_basis_coeffs_x(5) = -2399.573434f;
_basis_coeffs_x(6) = -11421.854705f;
_basis_coeffs_x(7) = 12388.936542f;
_basis_coeffs_x(8) = 120.944433f;
_basis_coeffs_x(9) = -12530.869640f;
_basis_coeffs_x(10) = 11346.431128f;
_basis_coeffs_x(11) = 2643.369342f;
_basis_coeffs_x(12) = -15999.009519f;
_basis_coeffs_x(13) = 18127.094775f;
_basis_coeffs_x(14) = -10676.696033f;
_basis_coeffs_x(15) = 3032.667571f;
_basis_coeffs_y(0) = 100.005984f;
_basis_coeffs_y(1) = 4686.100637f;
@@ -544,7 +544,7 @@ FixedwingPositionINDIControl::Run()
_actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0);
_actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1);
_actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2);
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.5f;
_actuators_0_pub.publish(_actuators);
//print_message(_actuators);
}
@@ -809,6 +809,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
// compute angular acceleration command (in body frame)
Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref;
rot_acc_command = _K_w*(omega_ref-_omega) + alpha_ref;
// apply LP filtered values for incremental part
return rot_acc_command;
}
@@ -816,9 +819,39 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
Vector3f
FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
{
// compute the required body moment to produce the desired body angular acceleration
Vector3f moment = _inertia*ctrl + _omega.cross(_inertia*_omega);
return moment;
Vector3f moment = _inertia*_alpha + _omega.cross(_inertia*_omega);
moment = 1.f*Vector3f{0.1f*_actuators.control[actuator_controls_s::INDEX_ROLL], 1.f*_actuators.control[actuator_controls_s::INDEX_PITCH], 0.1f*_actuators.control[actuator_controls_s::INDEX_YAW]};
Vector3f moment_filtered = _apply_LP_filter(moment, _m_list, _m_lpf_list);
Vector3f alpha_filtered = _apply_LP_filter(_alpha, _l_list, _l_lpf_list);
Vector3f command = _inertia*(ctrl-alpha_filtered) + moment_filtered;
//command = _inertia*ctrl + _omega.cross(_inertia*_omega);
return command;
}
Vector3f
FixedwingPositionINDIControl::_apply_LP_filter(Vector3f new_input, Vector<Vector3f, 3> &old_input, Vector<Vector3f, 2> &old_output)
{
old_input(0) = old_input(1);
old_input(1) = old_input(2);
old_input(2) = new_input;
//
Vector3f output = Vector3f{0.f,0.f,0.f};
//
output += _a1*old_output(1);
output += _a2*old_output(0);
//
output += _b1*old_input(2);
output += _b2*old_input(1);
output += _b3*old_input(0);
//
old_output(0) = old_output(1);
old_output(1) = output;
//
return output;
}
Vector3f
@@ -826,8 +859,8 @@ FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl)
{
// compute airspeed scaling
const float airspeed_constrained = constrain(_airspeed, 5.f, 50.f);
float airspeed_scaling = 1.f/(powf(airspeed_constrained,2)+1.f);
airspeed_scaling = 1.f;
float airspeed_scaling = 20.f/(powf(airspeed_constrained,2)+1.f);
airspeed_scaling = 1.0f;
// compute the normalized actuator deflection, including airspeed scaling
Vector3f deflection = airspeed_scaling*_K_actuators*ctrl;
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+18 -6
View File
@@ -16,10 +16,12 @@
#include <vector>
#include <array>
#include <drivers/drv_hrt.h>
#include "fw_att_control/ecl_pitch_controller.h"
#include "fw_att_control/ecl_roll_controller.h"
#include "fw_att_control/ecl_wheel_controller.h"
#include "fw_att_control/ecl_yaw_controller.h"
#include <lib/ecl/geo/geo.h>
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
#include <lib/npfg/npfg.hpp>
#include <lib/tecs/TECS.hpp>
#include <lib/landing_slope/Landingslope.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
@@ -237,6 +239,7 @@ private:
Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
Vector3f _compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
Vector3f _compute_NDI_stage_2(Vector3f ctrl);
Vector3f _apply_LP_filter(Vector3f new_input, Vector<Vector3f, 3> &old_input, Vector<Vector3f, 2> &old_output);
Vector3f _compute_actuator_deflections(Vector3f ctrl);
// control variables
@@ -260,10 +263,14 @@ private:
hrt_abstime _last_run{0};
// 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;
Vector<Vector3f, 3> _f_list; // force
Vector<Vector3f, 3> _m_list; // moment
Vector<Vector3f, 3> _a_list; // linear accel
Vector<Vector3f, 3> _l_list; // angular accel
Vector<Vector3f, 2> _f_lpf_list;
Vector<Vector3f, 2> _m_lpf_list;
Vector<Vector3f, 2> _a_lpf_list;
Vector<Vector3f, 2> _l_lpf_list;
// parameter variables
Matrix3f _inertia {};
@@ -281,6 +288,11 @@ private:
float _b2;
float _b3;
// body rate controllers
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists
hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts.
float _airspeed{0.0f};
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+18 -19
View File
@@ -169,6 +169,17 @@ PARAM_DEFINE_FLOAT(C_D1, 0.3783f);
*/
PARAM_DEFINE_FLOAT(C_D2, 1.984f);
/**
* 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_A1, 1.82292669f);
/**
* coefficients of the butterworth filter used for smoothing the IMU
@@ -180,7 +191,7 @@ PARAM_DEFINE_FLOAT(C_D2, 1.984f);
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(FILTER_A1, 0.0f);
PARAM_DEFINE_FLOAT(FILTER_A2, -0.83737699f);
/**
* coefficients of the butterworth filter used for smoothing the IMU
@@ -192,7 +203,7 @@ PARAM_DEFINE_FLOAT(FILTER_A1, 0.0f);
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(FILTER_A2, 0.0f);
PARAM_DEFINE_FLOAT(FILTER_B1, 0.00361257f);
/**
* coefficients of the butterworth filter used for smoothing the IMU
@@ -204,7 +215,7 @@ PARAM_DEFINE_FLOAT(FILTER_A2, 0.0f);
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(FILTER_B1, 0.0f);
PARAM_DEFINE_FLOAT(FILTER_B2, 0.00722515f);
/**
* coefficients of the butterworth filter used for smoothing the IMU
@@ -216,19 +227,7 @@ PARAM_DEFINE_FLOAT(FILTER_B1, 0.0f);
* @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);
PARAM_DEFINE_FLOAT(FILTER_B3, 0.00361257f);
// ========================================================
// =================== CONTROL GAINS ======================
@@ -423,7 +422,7 @@ PARAM_DEFINE_FLOAT(K_W_YAW, 5.0f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f);
PARAM_DEFINE_FLOAT(K_ACT_ROLL, 1.0f);
/**
* pitch gain of K_ACT (actuator deflection gain)
@@ -435,7 +434,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f);
PARAM_DEFINE_FLOAT(K_ACT_PITCH, 1.0f);
/**
* yaw gain of K_ACT (actuator deflection gain)
@@ -447,4 +446,4 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f);
PARAM_DEFINE_FLOAT(K_ACT_YAW, 1.0f);