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

implemented shear param update in feedthrough mode

Browse Source
This commit is contained in:
Marvin Harms
2022-08-31 16:52:24 +02:00
parent 910b8560f0
commit fb0f81dea1
5 changed files with 77 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+45 -32
View File
@@ -78,11 +78,6 @@ FixedwingPositionINDIControl::init()
return false;
}
PX4_INFO("Starting FW_DYN_SOAR_CONTROLLER");
//_read_trajectory_coeffs_csv("trajectory5.csv");
//_read_trajectory_coeffs_csv("trajectory4.csv");
//_read_trajectory_coeffs_csv("trajectory3.csv");
//_read_trajectory_coeffs_csv("trajectory2.csv");
//_read_trajectory_coeffs_csv("trajectory1.csv");
char filename[] = "trajectory0.csv";
_read_trajectory_coeffs_csv(filename);
@@ -173,19 +168,20 @@ FixedwingPositionINDIControl::parameters_update()
_loiter = _param_loiter.get();
_shear_heading = _param_shear_heading.get()/180.f * M_PI_F + M_PI_2_F;
_shear_h_ref = _param_shear_height.get();
_select_trajectory(0.0f);
_thrust = _param_thrust.get();
_thrust_pos = _param_thrust.get();
_thrust = _thrust_pos;
_switch_saturation = _param_switch_saturation.get();
_switch_filter = _param_switch_filter.get();
_switch_origin_hardcoded = _param_switch_origin_hardcoded.get();
// only update shear heading and height with params, if desired
if (_switch_origin_hardcoded) {
_shear_heading = _param_shear_heading.get()/180.f * M_PI_F + M_PI_2_F;
_shear_h_ref = _param_shear_height.get();
}
// sanity check parameters
// TODO: include sanity check
return PX4_OK;
}
@@ -289,13 +285,15 @@ 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;
if (_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;
_thrust = _thrust_pos;
}
}
}
@@ -409,6 +407,21 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
return;
}
void
FixedwingPositionINDIControl::soaring_estimator_shear_poll()
{
if (_soaring_estimator_shear_sub.update(&_soaring_estimator_shear)){
if (!_switch_origin_hardcoded) {
_shear_v_max = _soaring_estimator_shear.v_max;
_shear_alpha = _soaring_estimator_shear.alpha;
_shear_h_ref = _soaring_estimator_shear.h_ref;
_shear_heading = _soaring_estimator_shear.psi;
}
// TODO: include some safety guards for large drifts, e.g. limit the maximum heading shift
}
}
float
FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target)
{
@@ -478,27 +491,22 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
<alpha> in [0.20, 1.00]
<energy> in [E_min, E_max]
*/
/*
float v = _findClosest(_v_max_arr, _gridsize, _shear_v_max); // wind velocity
float a = _findClosest(_alpha_arr, _gridsize, _shear_alpha); // shear strength
float e = _findClosest(_energy_arr, _gridsize, _shear_energy); // initial energy
PX4_INFO("V, A, E: \t%.2f\t%.2f\t%.2f", double(v), double(a), double(e));
/*
float e = 20.f;//_findClosest(_energy_arr, _gridsize, _airspeed); // initial energy
char file[30] = "nominal";
char file[30] = "trajec_robust";
char v_str[6];
char a_str[6];
char e_str[6];
strcat(file,"_");
strcat(file,gcvt(v, 2, v_str));
strcat(file,gcvt(_shear_v_max, 2, v_str));
strcat(file,"_");
strcat(file,gcvt(a, 3, a_str));
strcat(file,gcvt(_shear_alpha, 3, a_str));
strcat(file,"_");
strcat(file,gcvt(e, 2, e_str));
strcat(file,".csv");
PX4_INFO("filename: \t%.30s", file);
*/
//PX4_INFO("filename: \t%.30s", file);
// select loiter trajectory for loiter test
char filename[16];
@@ -551,8 +559,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
// =======================================================================
bool error = false;
//char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
//char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
//PX4_ERR(home_dir);
strcat(home_dir,filename);
FILE* fp = fopen(home_dir, "r");
@@ -733,6 +741,11 @@ FixedwingPositionINDIControl::Run()
vehicle_angular_acceleration_poll();
soaring_controller_status_poll();
// update the shear estimate, only if we are flying in manual feedthrough for safety reasons
if (_switch_manual) {
soaring_estimator_shear_poll();
}
// update transform from trajectory frame to ENU frame (soaring frame)
_compute_trajectory_transform();
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+11 -6
View File
@@ -69,6 +69,7 @@
#include <uORB/topics/soaring_controller_position.h>
#include <uORB/topics/soaring_controller_status.h>
#include <uORB/topics/soaring_controller_wind.h>
#include <uORB/topics/soaring_estimator_shear.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/wind.h>
@@ -125,6 +126,7 @@ private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags
uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)}; // shear params for trajectory selection
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)};
@@ -164,6 +166,7 @@ private:
soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint
soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos
soaring_controller_wind_s _soaring_controller_wind{}; ///< soaring controller wind
soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring estimator shear
debug_value_s _debug_value{}; // slip angle
// parameter struct
@@ -219,7 +222,9 @@ private:
// force saturation
(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation,
// command filtering
(ParamInt<px4::params::DS_SWITCH_FILTER>) _param_switch_filter
(ParamInt<px4::params::DS_SWITCH_FILTER>) _param_switch_filter,
// hardcoded trajectory center
(ParamInt<px4::params::DS_SWITCH_ORI_HC>) _param_switch_origin_hardcoded
)
@@ -254,6 +259,7 @@ private:
void vehicle_control_mode_poll();
void vehicle_status_poll();
void soaring_controller_status_poll();
void soaring_estimator_shear_poll();
//
void status_publish();
@@ -361,12 +367,15 @@ private:
int _loiter;
// thrust
float _thrust;
float _thrust_pos;
// controller mode
bool _switch_manual = 1;
// force limit
bool _switch_saturation;
//
bool _switch_filter;
//
bool _switch_origin_hardcoded;
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.
@@ -382,11 +391,7 @@ private:
// vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength
const static size_t _gridsize = 11;
/*
float _energy_arr[_gridsize] = {14.f,16.f,18.f,20.f,22.f,24.f,26.f,28.f,30.f,32.f,34.f};
float _v_max_arr[_gridsize] = {10.f,11.f,12.f,13.f,14.f,15.f,16.f,17.f,18.f,19.f,20.f};
float _alpha_arr[_gridsize] = {0.1f,0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f,1.1f};
*/
// helper variables
Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+16 -1
View File
@@ -578,4 +578,19 @@ PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1);
* @increment 1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_INT32(DS_SWITCH_FILTER, 0);
PARAM_DEFINE_INT32(DS_SWITCH_FILTER, 0);
// ======================================================
// ============= hardcoded trajectory center ============
// ======================================================
/**
* integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate
* @unit
* @min 0
* @max 1
* @decimal 1
* @increment 1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1);
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+4 -4
View File
@@ -341,12 +341,12 @@ FixedwingShearEstimator::Run()
}
// get the correct shear params for trajectory selection
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 shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v, 2) +
powf(_X_posterior_horizontal(1)*_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));
float heading = atan2f(_X_posterior_horizontal(0),
_X_posterior_horizontal(1));
_soaring_estimator_shear.v_max = v;
_soaring_estimator_shear.alpha = a;
_soaring_estimator_shear.h_ref = _X_posterior_horizontal(4)*_unit_h;
src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
+1 -1
View File
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f);
* @increment 0.000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.02f);
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.01f);
/**
* Standard deviation of velicity measurement (wind)