fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-09 16:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

removed param DS_SWITCH_MANUAL, added shear message

Browse Source
This commit is contained in:
Marvin Harms
2022-08-30 17:08:36 +02:00
parent dacd0b64d4
commit 910b8560f0
6 changed files with 36 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: a3eef9d3a1...d3cb480a15
msg/soaring_estimator_shear.msg
+5
View File
@@ -20,5 +20,10 @@ float32 coeff_0 # offset vertical wind
float32 coeff_1 # linear coeff vertical wind
float32 coeff_2 # quadratic coeff vertical wind
float32 v_max # discrete value of v_max (shear velocity)
float32 alpha # discrete value of alpha (shear strength)
float32 h_ref # discrete value of h_ref (shear location)
float32 psi # discrete value of shear heading
bool soaring_feasible # plausibility check
uint64 reset_counter # filter reset counter
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+15 -3
View File
@@ -179,8 +179,6 @@ FixedwingPositionINDIControl::parameters_update()
_thrust = _param_thrust.get();
_switch_manual = _param_switch_manual.get();
_switch_saturation = _param_switch_saturation.get();
_switch_filter = _param_switch_filter.get();
@@ -288,6 +286,19 @@ FixedwingPositionINDIControl::manual_control_setpoint_poll()
}
}
void
FixedwingPositionINDIControl::rc_channels_poll()
{
_rc_channels_sub.update(&_rc_channels);
// use flaps channel to select manual feedthrough
if (_rc_channels.channels[5]>=0.f){
_switch_manual = 1;
}
else{
_switch_manual = 0;
}
}
void
FixedwingPositionINDIControl::vehicle_attitude_poll()
{
@@ -713,6 +724,7 @@ FixedwingPositionINDIControl::Run()
airspeed_poll();
airflow_aoa_poll();
airflow_slip_poll();
rc_channels_poll();
manual_control_setpoint_poll();
vehicle_local_position_poll();
vehicle_attitude_poll();
@@ -1395,7 +1407,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
// ====================================
if (_switch_manual){
// get an attitude setpoint from the current manual inputs
float roll_ref = 1.f * _manual_control_setpoint.y * M_PI_4_F;
float roll_ref = 1.f * _manual_control_setpoint.y * 1.0f;
float pitch_ref = -1.f* _manual_control_setpoint.x * M_PI_4_F;
Eulerf E_current(Quatf(_attitude.q));
float yaw_ref = E_current.psi();
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+5 -3
View File
@@ -44,6 +44,7 @@
#include <uORB/topics/airflow_aoa.h>
#include <uORB/topics/airflow_slip.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -126,6 +127,7 @@ private:
uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
// Publishers
uORB::Publication<actuator_controls_s> _actuators_0_pub;
@@ -144,6 +146,7 @@ private:
vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
actuator_controls_s _actuators {}; // actuator commands
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
rc_channels_s _rc_channels {}; ///< rc channels
vehicle_local_position_s _local_pos {}; ///< vehicle local position
vehicle_acceleration_s _acceleration {}; ///< vehicle acceleration
vehicle_attitude_s _attitude {}; ///< vehicle attitude
@@ -213,8 +216,6 @@ private:
(ParamFloat<px4::params::DS_W_HEIGHT>) _param_shear_height,
// thrust params
(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
// RC feedthrough params
(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual,
// force saturation
(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation,
// command filtering
@@ -248,6 +249,7 @@ private:
void control_update();
void manual_control_setpoint_poll();
void rc_channels_poll();
void vehicle_command_poll();
void vehicle_control_mode_poll();
void vehicle_status_poll();
@@ -360,7 +362,7 @@ private:
// thrust
float _thrust;
// controller mode
bool _switch_manual;
bool _switch_manual = 1;
// force limit
bool _switch_saturation;
//
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
-16
View File
@@ -549,22 +549,6 @@ PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f);
*/
PARAM_DEFINE_FLOAT(DS_THRUST, 0);
// ======================================================
// ================ Stick feedthorugh ===================
// ======================================================
/**
* integer in {0,1} defining if manual attitude setpoints are commanded by the pilot, 0=DS-controller, 1=manual feedthrough
*
* @unit
* @min 0
* @max 1
* @decimal 1
* @increment 1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);
// ======================================================
// ============= controller force saturation ============
// ======================================================
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+10 -4
View File
@@ -341,10 +341,16 @@ FixedwingShearEstimator::Run()
}
// get the correct shear params for trajectory selection
float v = _findClosest(_v_max_arr, 5, _shear_v_max); // wind velocity
float a = _findClosest(_alpha_arr, 9, _shear_alpha); // shear strength
float shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v - _X_posterior_horizontal(2)*_unit_v, 2) +
powf(_X_posterior_horizontal(1)*_unit_v - _X_posterior_horizontal(3)*_unit_v, 2));
float v = _findClosest(_v_max_arr, 5, shear); // wind velocity
float a = _findClosest(_alpha_arr, 9, _X_posterior_horizontal(5)*_unit_a); // shear strength
float heading = -atan2f(_X_posterior_horizontal(1)-_X_posterior_horizontal(3),
_X_posterior_horizontal(0)-_X_posterior_horizontal(2));
_soaring_estimator_shear.v_max = v;
_soaring_estimator_shear.alpha = a;
_soaring_estimator_shear.h_ref = _X_posterior_horizontal(4)*_unit_h;
_soaring_estimator_shear.psi = heading;
// publish shear params
// ========================================
@@ -408,7 +414,7 @@ bool FixedwingShearEstimator::check_feasibility()
}
float
FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target)
FixedwingShearEstimator::_getClosest(float val1, float val2, float target)
{
if (target - val1 >= val2 - target)
return val2;
@@ -417,7 +423,7 @@ FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target)
}
float
FixedwingPositionINDIControl::_findClosest(float arr[], int n, float target)
FixedwingShearEstimator::_findClosest(float arr[], int n, float target)
{
// Corner cases
if (target <= arr[0])